diff --git a/src/rx_common.c b/src/rx_common.c index e186f09a20..6a801d1360 100644 --- a/src/rx_common.c +++ b/src/rx_common.c @@ -86,13 +86,19 @@ bool isSerialRxFrameComplete(rxConfig_t *rxConfig) return false; } +uint8_t calculateChannelRemapping(uint8_t *rcmap, uint8_t channelToRemap) { + return rcmap[channelToRemap]; +} + void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { uint8_t chan; if (feature(FEATURE_SERIALRX)) { - for (chan = 0; chan < MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT; chan++) - rcData[chan] = rcReadRawFunc(rxConfig, rxRuntimeConfig, chan); + for (chan = 0; chan < MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT; chan++) { + uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, chan); + rcData[chan] = rcReadRawFunc(rxRuntimeConfig, rawChannel); + } } else { static int16_t rcSamples[MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT], rcDataMean[MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT]; static uint8_t rcSampleIndex = 0; @@ -103,8 +109,16 @@ void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) for (chan = 0; chan < MAX_SUPPORTED_RC_PPM_AND_PWM_CHANNEL_COUNT; chan++) { + uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, chan); + // sample the channel - rcSamples[chan][currentSampleIndex] = rcReadRawFunc(rxConfig, rxRuntimeConfig, chan); + uint16_t sample = rcReadRawFunc(rxRuntimeConfig, rawChannel); + + // validate the range + if (sample < 750 || sample > 2250) + sample = rxConfig->midrc; + + rcSamples[chan][currentSampleIndex] = sample; // compute the average of recent samples rcDataMean[chan] = 0; diff --git a/src/rx_common.h b/src/rx_common.h index 20e49e8530..28c2a4f9c0 100644 --- a/src/rx_common.h +++ b/src/rx_common.h @@ -38,7 +38,7 @@ typedef struct rxRuntimeConfig_s { extern rxRuntimeConfig_t rxRuntimeConfig; -typedef uint16_t (* rcReadRawDataPtr)(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data +typedef uint16_t (* rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/rx_msp.c b/src/rx_msp.c index 61a6318a82..813cb6e4db 100644 --- a/src/rx_msp.c +++ b/src/rx_msp.c @@ -18,7 +18,7 @@ failsafe_t *failsafe; static bool rxMspFrameDone = false; -static uint16_t rxMspReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t rxMspReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { return rcData[chan]; } diff --git a/src/rx_pwm.c b/src/rx_pwm.c index 2ee00a44d9..75d11d930e 100644 --- a/src/rx_pwm.c +++ b/src/rx_pwm.c @@ -15,9 +15,9 @@ #include "rx_common.h" #include "rx_pwm.h" -static uint16_t pwmReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t pwmReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { - return pwmRead(rxConfig->rcmap[chan]); + return pwmRead(chan); } void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback) diff --git a/src/rx_sbus.c b/src/rx_sbus.c index 4bec9934a8..ec81fd59f2 100644 --- a/src/rx_sbus.c +++ b/src/rx_sbus.c @@ -23,7 +23,7 @@ static bool sbusFrameDone = false; static void sbusDataReceive(uint16_t c); -static uint16_t sbusReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; @@ -117,8 +117,8 @@ bool sbusFrameComplete(void) return false; } -static uint16_t sbusReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { - return sbusChannelData[rxConfig->rcmap[chan]] / 2 + SBUS_OFFSET; + return sbusChannelData[chan] / 2 + SBUS_OFFSET; } diff --git a/src/rx_spektrum.c b/src/rx_spektrum.c index 05790b3f0c..0b3d77e5c2 100644 --- a/src/rx_spektrum.c +++ b/src/rx_spektrum.c @@ -31,7 +31,7 @@ static bool spekDataIncoming = false; volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; static void spektrumDataReceive(uint16_t c); -static uint16_t spektrumReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); failsafe_t *failsafe; @@ -88,7 +88,7 @@ bool spektrumFrameComplete(void) return rcFrameComplete; } -static uint16_t spektrumReadRawRC(rxConfig_t*rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { uint16_t data; static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT]; @@ -104,13 +104,13 @@ static uint16_t spektrumReadRawRC(rxConfig_t*rxConfig, rxRuntimeConfig_t *rxRunt } if (chan >= rxRuntimeConfig->channelCount || !spekDataIncoming) { - data = rxConfig->midrc; - } else { - if (spekHiRes) - data = 988 + (spekChannelData[rxConfig->rcmap[chan]] >> 1); // 2048 mode - else - data = 988 + spekChannelData[rxConfig->rcmap[chan]]; // 1024 mode + return 0; } + if (spekHiRes) + data = 988 + (spekChannelData[chan] >> 1); // 2048 mode + else + data = 988 + spekChannelData[chan]; // 1024 mode + return data; } diff --git a/src/rx_sumd.c b/src/rx_sumd.c index 6b276c5577..3048fc8f72 100644 --- a/src/rx_sumd.c +++ b/src/rx_sumd.c @@ -22,7 +22,7 @@ static bool sumdFrameDone = false; static void sumdDataReceive(uint16_t c); -static uint16_t sumdReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static uint32_t sumdChannelData[SUMD_MAX_CHANNEL]; @@ -89,7 +89,7 @@ bool sumdFrameComplete(void) return false; } -static uint16_t sumdReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { - return sumdChannelData[rxConfig->rcmap[chan]] / 8; + return sumdChannelData[chan] / 8; }