From 3ed979e88ca498de2bbbaa9dd3e982c7cb1fce10 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 8 May 2014 11:54:20 +0100 Subject: [PATCH] BUGFIX - pwmReadRawRC was broken in 2d248676f5415fe4a1fe50f3345bddcb41bd4a59. MSP RX ignored rc channel mapping. Symptoms: power up FC using parallel PWM or PPM with no receiver attached. Connect GUI tool to view receiver data. Observe that each channel has a reading of '2' instead of somewhere near 'midrc'. Symptoms: rxMsp ignored rc channel map, cli 'map' command had no effect. This commit re-instates the code that checked to see if an rc channel value was in the correct range but moved it into computeRC where it really belongs. Additionally to the code to remap rc channels was moved from the rx implementations into the computeRc method. This results in smaller code size, no duplication of logic avoids having to pass rxConfig to each rx read method and finally fixes rxMsp so that channel mapping can be used. Failsafe still works as intended, verified by using parallel PWM, arming and then pulling out the throttle PWM inpout cable. This also improves code cleanliness since now each rawRawRc method in the RX implementations does not have to remap the channels. The methods now do only what their name says. --- src/rx_common.c | 20 +++++++++++++++++--- src/rx_common.h | 2 +- src/rx_msp.c | 2 +- src/rx_pwm.c | 4 ++-- src/rx_sbus.c | 6 +++--- src/rx_spektrum.c | 16 ++++++++-------- src/rx_sumd.c | 6 +++--- 7 files changed, 35 insertions(+), 21 deletions(-) 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; }