src/main/rx/rx.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21
  22 #include <string.h>
  23
  24 #include "build_config.h"
  25
  26 #include "platform.h"
  27
  28 #include "common/maths.h"
  29
  30 #include "config/config.h"
  31
  32 #include "drivers/serial.h"
  33 #include "drivers/adc.h"
  34 #include "io/serial.h"
  35
  36 #include "flight/failsafe.h"
  37
  38 #include "drivers/gpio.h"
  39 #include "drivers/timer.h"
  40 #include "drivers/pwm_rx.h"
  41 #include "rx/pwm.h"
  42 #include "rx/sbus.h"
  43 #include "rx/spektrum.h"
  44 #include "rx/sumd.h"
  45 #include "rx/sumh.h"
  46 #include "rx/msp.h"
  47 #include "rx/xbus.h"
  48
  49 #include "rx/rx.h"
  50
  51 extern int16_t debug[4];
  52
  53 void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
  54
  55 bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
  56 bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
  57 bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
  58 bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
  59
  60 bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
  61
  62 const char rcChannelLetters[] = "AERT12345678abcdefgh";
  63
  64 uint16_t rssi = 0;                  // range: [0;1023]
  65
  66 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
  67
  68 #define PPM_AND_PWM_SAMPLE_COUNT 4
  69
  70 #define PULSE_MIN   750       // minimum PWM pulse width which is considered valid
  71 #define PULSE_MAX   2250      // maximum PWM pulse width which is considered valid
  72
  73 #define DELAY_50_HZ (1000000 / 50)
  74
  75 static rcReadRawDataPtr rcReadRawFunc = NULL;  // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
  76
  77 rxRuntimeConfig_t rxRuntimeConfig;
  78 static rxConfig_t *rxConfig;
  79
  80 void serialRxInit(rxConfig_t *rxConfig);
  81
  82 void useRxConfig(rxConfig_t *rxConfigToUse)
  83 {
  84     rxConfig = rxConfigToUse;
  85 }
  86
  87 #define STICK_CHANNEL_COUNT 4
  88
  89 void rxInit(rxConfig_t *rxConfig)
  90 {
  91     uint8_t i;
  92
  93     useRxConfig(rxConfig);
  94
  95     for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
  96         rcData[i] = rxConfig->midrc;
  97     }
  98
  99 #ifdef SERIAL_RX
 100     if (feature(FEATURE_RX_SERIAL)) {
 101         serialRxInit(rxConfig);
 102     }
 103 #endif
 104
 105     if (feature(FEATURE_RX_MSP)) {
 106         rxMspInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
 107     }
 108
 109     if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
 110         rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc);
 111     }
 112
 113     rxRuntimeConfig.auxChannelCount = rxRuntimeConfig.channelCount - STICK_CHANNEL_COUNT;
 114 }
 115
 116 #ifdef SERIAL_RX
 117 void serialRxInit(rxConfig_t *rxConfig)
 118 {
 119     bool enabled = false;
 120     switch (rxConfig->serialrx_provider) {
 121         case SERIALRX_SPEKTRUM1024:
 122         case SERIALRX_SPEKTRUM2048:
 123             enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
 124             break;
 125         case SERIALRX_SBUS:
 126             enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
 127             break;
 128         case SERIALRX_SUMD:
 129             enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
 130             break;
 131         case SERIALRX_SUMH:
 132             enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
 133             break;
 134         case SERIALRX_XBUS_MODE_B:
 135         case SERIALRX_XBUS_MODE_B_RJ01:
 136             enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc);
 137             break;
 138     }
 139
 140     if (!enabled) {
 141         featureClear(FEATURE_RX_SERIAL);
 142         rcReadRawFunc = NULL;
 143     }
 144 }
 145
 146 uint8_t serialRxFrameStatus(rxConfig_t *rxConfig)
 147 {
 148     /**
 149      * FIXME: Each of the xxxxFrameStatus() methods MUST be able to survive being called without the
 150      * corresponding xxxInit() method having been called first.
 151      *
 152      * This situation arises when the cli or the msp changes the value of rxConfig->serialrx_provider
 153      *
 154      * A solution is for the ___Init() to configure the serialRxFrameStatus function pointer which
 155      * should be used instead of the switch statement below.
 156      */
 157     switch (rxConfig->serialrx_provider) {
 158         case SERIALRX_SPEKTRUM1024:
 159         case SERIALRX_SPEKTRUM2048:
 160             return spektrumFrameStatus();
 161         case SERIALRX_SBUS:
 162             return sbusFrameStatus();
 163         case SERIALRX_SUMD:
 164             return sumdFrameStatus();
 165         case SERIALRX_SUMH:
 166             return sumhFrameStatus();
 167         case SERIALRX_XBUS_MODE_B:
 168         case SERIALRX_XBUS_MODE_B_RJ01:
 169             return xBusFrameStatus();
 170     }
 171     return SERIAL_RX_FRAME_PENDING;
 172 }
 173 #endif
 174
 175 uint8_t calculateChannelRemapping(uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap)
 176 {
 177     if (channelToRemap < channelMapEntryCount) {
 178         return channelMap[channelToRemap];
 179     }
 180     return channelToRemap;
 181 }
 182
 183 static bool rcDataReceived = false;
 184
 185 static uint32_t rxUpdateAt = 0;
 186
 187
 188 void updateRx(void)
 189 {
 190     rcDataReceived = false;
 191
 192 #ifdef SERIAL_RX
 193     if (feature(FEATURE_RX_SERIAL)) {
 194         uint8_t frameStatus = serialRxFrameStatus(rxConfig);
 195
 196         if (frameStatus & SERIAL_RX_FRAME_COMPLETE) {
 197             rcDataReceived = true;
 198             if ((frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0 && feature(FEATURE_FAILSAFE)) {
 199                 failsafeReset();
 200             }
 201         }
 202     }
 203 #endif
 204
 205     if (feature(FEATURE_RX_MSP)) {
 206         rcDataReceived = rxMspFrameComplete();
 207         if (rcDataReceived && feature(FEATURE_FAILSAFE)) {
 208             failsafeReset();
 209         }
 210     }
 211 }
 212
 213 bool shouldProcessRx(uint32_t currentTime)
 214 {
 215     return rcDataReceived || ((int32_t)(currentTime - rxUpdateAt) >= 0); // data driven or 50Hz
 216 }
 217
 218 static bool isRxDataDriven(void) {
 219     return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM));
 220 }
 221
 222 static uint8_t rcSampleIndex = 0;
 223
 224 static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample)
 225 {
 226     static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT];
 227     static int16_t rcDataMean[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT];
 228     static bool rxSamplesCollected = false;
 229
 230     uint8_t currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT;
 231
 232     // update the recent samples and compute the average of them
 233     rcSamples[chan][currentSampleIndex] = sample;
 234
 235     // avoid returning an incorrect average which would otherwise occur before enough samples
 236     if (!rxSamplesCollected) {
 237         if (rcSampleIndex < PPM_AND_PWM_SAMPLE_COUNT) {
 238             return sample;
 239         }
 240         rxSamplesCollected = true;
 241     }
 242
 243     rcDataMean[chan] = 0;
 244
 245     uint8_t sampleIndex;
 246     for (sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++)
 247         rcDataMean[chan] += rcSamples[chan][sampleIndex];
 248
 249     return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
 250 }
 251
 252 static void processRxChannels(void)
 253 {
 254     uint8_t chan;
 255
 256     if (feature(FEATURE_RX_MSP)) {
 257         return; // rcData will have already been updated by MSP_SET_RAW_RC
 258     }
 259
 260     bool shouldCheckPulse = true;
 261
 262     if (feature(FEATURE_FAILSAFE)) {
 263         if (feature(FEATURE_RX_PPM)) {
 264             shouldCheckPulse = isPPMDataBeingReceived();
 265             resetPPMDataReceivedState();
 266         } else {
 267             shouldCheckPulse = !isRxDataDriven();
 268         }
 269     }
 270
 271     for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) {
 272
 273         if (!rcReadRawFunc) {
 274             rcData[chan] = rxConfig->midrc;
 275             continue;
 276         }
 277
 278         uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, chan);
 279
 280         // sample the channel
 281         uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
 282
 283         if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) {
 284             failsafeCheckPulse(chan, sample);
 285         }
 286
 287         // validate the range
 288         if (sample < PULSE_MIN || sample > PULSE_MAX)
 289             sample = rxConfig->midrc;
 290
 291         if (isRxDataDriven()) {
 292             rcData[chan] = sample;
 293         } else {
 294             rcData[chan] = calculateNonDataDrivenChannel(chan, sample);
 295         }
 296     }
 297 }
 298
 299 static void processDataDrivenRx(void)
 300 {
 301     processRxChannels();
 302 }
 303
 304 static void processNonDataDrivenRx(void)
 305 {
 306     rcSampleIndex++;
 307
 308     processRxChannels();
 309 }
 310
 311 void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
 312 {
 313     rxUpdateAt = currentTime + DELAY_50_HZ;
 314
 315     if (feature(FEATURE_FAILSAFE)) {
 316         failsafeOnRxCycle();
 317     }
 318
 319     if (isRxDataDriven()) {
 320         processDataDrivenRx();
 321     } else {
 322         processNonDataDrivenRx();
 323     }
 324 }
 325
 326 void parseRcChannels(const char *input, rxConfig_t *rxConfig)
 327 {
 328     const char *c, *s;
 329
 330     for (c = input; *c; c++) {
 331         s = strchr(rcChannelLetters, *c);
 332         if (s && (s < rcChannelLetters + MAX_MAPPABLE_RX_INPUTS))
 333             rxConfig->rcmap[s - rcChannelLetters] = c - input;
 334     }
 335 }
 336
 337 void updateRSSIPWM(void)
 338 {
 339     int16_t pwmRssi = 0;
 340     // Read value of AUX channel as rssi
 341     pwmRssi = rcData[rxConfig->rssi_channel - 1];
 342
 343
 344     // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
 345     rssi = (uint16_t)((constrain(pwmRssi - 1000, 0, 1000) / 1000.0f) * 1023.0f);
 346 }
 347
 348 #define RSSI_ADC_SAMPLE_COUNT 16
 349 //#define RSSI_SCALE (0xFFF / 100.0f)
 350
 351 void updateRSSIADC(uint32_t currentTime)
 352 {
 353 #ifndef USE_ADC
 354     UNUSED(currentTime);
 355 #else
 356     static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT];
 357     static uint8_t adcRssiSampleIndex = 0;
 358     static uint32_t rssiUpdateAt = 0;
 359
 360     if ((int32_t)(currentTime - rssiUpdateAt) < 0) {
 361         return;
 362     }
 363     rssiUpdateAt = currentTime + DELAY_50_HZ;
 364
 365     int16_t adcRssiMean = 0;
 366     uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
 367     uint8_t rssiPercentage = adcRssiSample / rxConfig->rssi_scale;
 368
 369     adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT;
 370
 371     adcRssiSamples[adcRssiSampleIndex] = rssiPercentage;
 372
 373     uint8_t sampleIndex;
 374
 375     for (sampleIndex = 0; sampleIndex < RSSI_ADC_SAMPLE_COUNT; sampleIndex++) {
 376         adcRssiMean += adcRssiSamples[sampleIndex];
 377     }
 378
 379     adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT;
 380
 381     rssi = (uint16_t)((constrain(adcRssiMean, 0, 100) / 100.0f) * 1023.0f);
 382 #endif
 383 }
 384
 385 void updateRSSI(uint32_t currentTime)
 386 {
 387
 388     if (rxConfig->rssi_channel > 0) {
 389         updateRSSIPWM();
 390     } else if (feature(FEATURE_RSSI_ADC)) {
 391         updateRSSIADC(currentTime);
 392     }
 393 }
 394
 395