From 93cab8805bd27cb87d94027efb8d871ad84ab758 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 11 Nov 2016 21:07:05 +0000 Subject: [PATCH] Call serial frame status functions via pointer rather than switch statement. Added type specific serial build flags as per iNav for finer grain ROM size control for F1 targets. Improved efficiency of rxUpdateCheck function. --- src/main/fc/fc_msp.c | 2 +- src/main/rx/ibus.c | 6 +- src/main/rx/ibus.h | 1 - src/main/rx/jetiexbus.c | 6 +- src/main/rx/jetiexbus.h | 1 - src/main/rx/msp.c | 11 +- src/main/rx/msp.h | 1 - src/main/rx/pwm.c | 4 +- src/main/rx/rx.c | 211 ++++++++++++--------------------- src/main/rx/rx.h | 21 ++-- src/main/rx/rx_spi.c | 4 +- src/main/rx/sbus.c | 8 +- src/main/rx/sbus.h | 1 - src/main/rx/spektrum.c | 6 +- src/main/rx/spektrum.h | 1 - src/main/rx/sumd.c | 6 +- src/main/rx/sumd.h | 1 - src/main/rx/sumh.c | 6 +- src/main/rx/sumh.h | 1 - src/main/rx/xbus.c | 14 +-- src/main/rx/xbus.h | 1 - src/main/target/CJMCU/target.h | 2 +- src/main/target/common.h | 18 ++- 23 files changed, 136 insertions(+), 197 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a23fb2546d..71b38af517 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1225,7 +1225,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_RAW_RC: -#ifndef SKIP_RX_MSP +#ifdef USE_RX_MSP { uint8_t channelCount = dataSize / sizeof(uint16_t); if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index e4e87e758d..4ec4c1d298 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -108,7 +108,7 @@ static void ibusDataReceive(uint16_t c) } } -uint8_t ibusFrameStatus(void) +static uint8_t ibusFrameStatus(void) { uint8_t i, offset; uint8_t frameStatus = RX_FRAME_PENDING; @@ -153,8 +153,8 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL; rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed - rxRuntimeConfig->rcReadRawFunc = ibusReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = ibusFrameStatus; + rxRuntimeConfig->rcReadRawFn = ibusReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = ibusFrameStatus; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/ibus.h b/src/main/rx/ibus.h index 76bc244d2d..73221c44fd 100755 --- a/src/main/rx/ibus.h +++ b/src/main/rx/ibus.h @@ -17,5 +17,4 @@ #pragma once -uint8_t ibusFrameStatus(void); bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 440f144007..04b6ee19fb 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -375,7 +375,7 @@ static void jetiExBusDataReceive(uint16_t c) // Check if it is time to read a frame from the data... -uint8_t jetiExBusFrameStatus() +static uint8_t jetiExBusFrameStatus() { if (jetiExBusFrameState != EXBUS_STATE_RECEIVED) return RX_FRAME_PENDING; @@ -593,8 +593,8 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfi rxRuntimeConfig->channelCount = JETIEXBUS_CHANNEL_COUNT; rxRuntimeConfig->rxRefreshRate = 5500; - rxRuntimeConfig->rcReadRawFunc = jetiExBusReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = jetiExBusFrameStatus; + rxRuntimeConfig->rcReadRawFn = jetiExBusReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = jetiExBusFrameStatus; jetiExBusFrameReset(); diff --git a/src/main/rx/jetiexbus.h b/src/main/rx/jetiexbus.h index 98c18dcb2d..9fe45e12e1 100644 --- a/src/main/rx/jetiexbus.h +++ b/src/main/rx/jetiexbus.h @@ -18,5 +18,4 @@ #pragma once bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); -uint8_t jetiExBusFrameStatus(void); diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index 2200ea57f6..5841182e2c 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -20,7 +20,7 @@ #include "platform.h" -#ifndef SKIP_RX_MSP +#ifdef USE_RX_MSP #include "common/utils.h" @@ -36,6 +36,9 @@ static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint return mspFrame[chan]; } +/* + * Called from MSP command handler - mspFcProcessCommand + */ void rxMspFrameReceive(uint16_t *frame, int channelCount) { for (int i = 0; i < channelCount; i++) { @@ -50,7 +53,7 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) rxMspFrameDone = true; } -uint8_t rxMspFrameStatus(void) +static uint8_t rxMspFrameStatus(void) { if (!rxMspFrameDone) { return RX_FRAME_PENDING; @@ -67,7 +70,7 @@ void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT; rxRuntimeConfig->rxRefreshRate = 20000; - rxRuntimeConfig->rcReadRawFunc = rxMspReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = rxMspFrameStatus; + rxRuntimeConfig->rcReadRawFn = rxMspReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = rxMspFrameStatus; } #endif diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 94043ba7e2..69c2839282 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -17,6 +17,5 @@ #pragma once -uint8_t rxMspFrameStatus(void); void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); void rxMspFrameReceive(uint16_t *frame, int channelCount); diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 5f2c69b802..b9f42c13b8 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -57,10 +57,10 @@ void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled if (feature(FEATURE_RX_PARALLEL_PWM)) { rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; - rxRuntimeConfig->rcReadRawFunc = pwmReadRawRC; + rxRuntimeConfig->rcReadRawFn = pwmReadRawRC; } else if (feature(FEATURE_RX_PPM)) { rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; - rxRuntimeConfig->rcReadRawFunc = ppmReadRawRC; + rxRuntimeConfig->rcReadRawFn = ppmReadRawRC; } } #endif diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 6ae0df55eb..b9b6101eb7 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -151,29 +151,46 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig { bool enabled = false; switch (rxConfig->serialrx_provider) { +#ifdef USE_SERIALRX_SPEKTRUM case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: enabled = spektrumInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_SBUS case SERIALRX_SBUS: enabled = sbusInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_SUMD case SERIALRX_SUMD: enabled = sumdInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_SUMH case SERIALRX_SUMH: enabled = sumhInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_XBUS case SERIALRX_XBUS_MODE_B: case SERIALRX_XBUS_MODE_B_RJ01: enabled = xBusInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_IBUS case SERIALRX_IBUS: enabled = ibusInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_JETIEXBUS case SERIALRX_JETIEXBUS: enabled = jetiExBusInit(rxConfig, rxRuntimeConfig); break; +#endif + default: + enabled = false; + break; } return enabled; } @@ -182,8 +199,8 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions) { useRxConfig(rxConfig); - rxRuntimeConfig.rcReadRawFunc = nullReadRawRC; - rxRuntimeConfig.rcFrameStatusFunc = nullFrameStatus; + rxRuntimeConfig.rcReadRawFn = nullReadRawRC; + rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rcSampleIndex = 0; needRxSignalMaxDelayUs = DELAY_10_HZ; @@ -215,12 +232,13 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct const bool enabled = serialRxInit(rxConfig, &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SERIAL); - rxRuntimeConfig.rcReadRawFunc = nullReadRawRC; + rxRuntimeConfig.rcReadRawFn = nullReadRawRC; + rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; } } #endif -#ifndef SKIP_RX_MSP +#ifdef USE_RX_MSP if (feature(FEATURE_RX_MSP)) { rxMspInit(rxConfig, &rxRuntimeConfig); needRxSignalMaxDelayUs = DELAY_5_HZ; @@ -232,7 +250,8 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct const bool enabled = rxSpiInit(rxConfig, &rxRuntimeConfig); if (!enabled) { featureClear(FEATURE_RX_SPI); - rxRuntimeConfig.rcReadRawFunc = nullReadRawRC; + rxRuntimeConfig.rcReadRawFn = nullReadRawRC; + rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; } } #endif @@ -244,40 +263,6 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct #endif } -#ifdef SERIAL_RX -static uint8_t serialRxFrameStatus(const rxConfig_t *rxConfig) -{ - /** - * FIXME: Each of the xxxxFrameStatus() methods MUST be able to survive being called without the - * corresponding xxxInit() method having been called first. - * - * This situation arises when the cli or the msp changes the value of rxConfig->serialrx_provider - * - * A solution is for the ___Init() to configure the serialRxFrameStatus function pointer which - * should be used instead of the switch statement below. - */ - switch (rxConfig->serialrx_provider) { - case SERIALRX_SPEKTRUM1024: - case SERIALRX_SPEKTRUM2048: - return spektrumFrameStatus(); - case SERIALRX_SBUS: - return sbusFrameStatus(); - case SERIALRX_SUMD: - return sumdFrameStatus(); - case SERIALRX_SUMH: - return sumhFrameStatus(); - case SERIALRX_XBUS_MODE_B: - case SERIALRX_XBUS_MODE_B_RJ01: - return xBusFrameStatus(); - case SERIALRX_IBUS: - return ibusFrameStatus(); - case SERIALRX_JETIEXBUS: - return jetiExBusFrameStatus(); - } - return RX_FRAME_PENDING; -} -#endif - static uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap) { if (channelToRemap < channelMapEntryCount) { @@ -295,20 +280,10 @@ bool rxAreFlightChannelsValid(void) { return rxFlightChannelsValid; } -static bool isRxDataDriven(void) { - return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)); -} -static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime) +static bool isRxDataDriven(void) { - if (!rxSignalReceived) { - return; - } - - if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) { - rxSignalReceived = false; - rxSignalReceivedNotDataDriven = false; - } + return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)); } void suspendRxSignal(void) @@ -329,48 +304,12 @@ bool rxUpdateCheck(uint32_t currentTime, uint32_t currentDeltaTime) { UNUSED(currentDeltaTime); - resetRxSignalReceivedFlagIfNeeded(currentTime); - - if (isRxDataDriven()) { - rxDataReceived = false; - } - - -#ifdef SERIAL_RX - if (feature(FEATURE_RX_SERIAL)) { - const uint8_t frameStatus = serialRxFrameStatus(rxConfig); - if (frameStatus & RX_FRAME_COMPLETE) { - rxDataReceived = true; - rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0; - rxSignalReceived = !rxIsInFailsafeMode; - needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; + if (rxSignalReceived) { + if (currentTime >= needRxSignalBefore) { + rxSignalReceived = false; + rxSignalReceivedNotDataDriven = false; } } -#endif - -#ifdef USE_RX_SPI - if (feature(FEATURE_RX_SPI)) { - const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFunc(); - if (frameStatus & RX_FRAME_COMPLETE) { - rxDataReceived = true; - rxIsInFailsafeMode = false; - rxSignalReceived = true; - needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; - } - } -#endif - -#ifndef SKIP_RX_MSP - if (feature(FEATURE_RX_MSP)) { - const uint8_t frameStatus = rxMspFrameStatus(); - if (frameStatus & RX_FRAME_COMPLETE) { - rxDataReceived = true; - rxIsInFailsafeMode = false; - rxSignalReceived = !rxIsInFailsafeMode; - needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; - } - } -#endif #ifndef SKIP_RX_PWM_PPM if (feature(FEATURE_RX_PPM)) { @@ -380,16 +319,24 @@ bool rxUpdateCheck(uint32_t currentTime, uint32_t currentDeltaTime) needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; resetPPMDataReceivedState(); } - } - - if (feature(FEATURE_RX_PARALLEL_PWM)) { + } else if (feature(FEATURE_RX_PARALLEL_PWM)) { if (isPWMDataBeingReceived()) { rxSignalReceivedNotDataDriven = true; rxIsInFailsafeModeNotDataDriven = false; needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; } - } + } else #endif + { + rxDataReceived = false; + const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(); + if (frameStatus & RX_FRAME_COMPLETE) { + rxDataReceived = true; + rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0; + rxSignalReceived = !rxIsInFailsafeMode; + needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; + } + } return rxDataReceived || (currentTime >= rxUpdateAt); // data driven or 50Hz } @@ -399,7 +346,7 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) static int16_t rcDataMean[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT]; static bool rxSamplesCollected = false; - uint8_t currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT; + const uint8_t currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT; // update the recent samples and compute the average of them rcSamples[chan][currentSampleIndex] = sample; @@ -413,11 +360,9 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) } rcDataMean[chan] = 0; - - uint8_t sampleIndex; - for (sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++) + for (int sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++) { rcDataMean[chan] += rcSamples[chan][sampleIndex]; - + } return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT; } @@ -426,28 +371,27 @@ static uint16_t getRxfailValue(uint8_t channel) const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; switch(channelFailsafeConfiguration->mode) { - case RX_FAILSAFE_MODE_AUTO: - switch (channel) { - case ROLL: - case PITCH: - case YAW: - return rxConfig->midrc; + case RX_FAILSAFE_MODE_AUTO: + switch (channel) { + case ROLL: + case PITCH: + case YAW: + return rxConfig->midrc; + case THROTTLE: + if (feature(FEATURE_3D)) + return rxConfig->midrc; + else + return rxConfig->rx_min_usec; + } + /* no break */ - case THROTTLE: - if (feature(FEATURE_3D)) - return rxConfig->midrc; - else - return rxConfig->rx_min_usec; - } - /* no break */ + default: + case RX_FAILSAFE_MODE_INVALID: + case RX_FAILSAFE_MODE_HOLD: + return rcData[channel]; - default: - case RX_FAILSAFE_MODE_INVALID: - case RX_FAILSAFE_MODE_HOLD: - return rcData[channel]; - - case RX_FAILSAFE_MODE_SET: - return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step); + case RX_FAILSAFE_MODE_SET: + return RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step); } } @@ -481,14 +425,13 @@ static uint8_t getRxChannelCount(void) { static void readRxChannelsApplyRanges(void) { - uint8_t channel; + const int channelCount = getRxChannelCount(); + for (int channel = 0; channel < channelCount; channel++) { - for (channel = 0; channel < getRxChannelCount(); channel++) { - - uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); + const uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel - uint16_t sample = rxRuntimeConfig.rcReadRawFunc(&rxRuntimeConfig, rawChannel); + uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel); // apply the rx calibration if (channel < NON_AUX_CHANNEL_COUNT) { @@ -499,13 +442,11 @@ static void readRxChannelsApplyRanges(void) } } -static void detectAndApplySignalLossBehaviour(void) +static void detectAndApplySignalLossBehaviour(uint32_t currentTime) { - int channel; - uint16_t sample; bool useValueFromRx = true; - bool rxIsDataDriven = isRxDataDriven(); - uint32_t currentMilliTime = millis(); + const bool rxIsDataDriven = isRxDataDriven(); + const uint32_t currentMilliTime = currentTime / 1000; if (!rxIsDataDriven) { rxSignalReceived = rxSignalReceivedNotDataDriven; @@ -519,14 +460,14 @@ static void detectAndApplySignalLossBehaviour(void) #ifdef DEBUG_RX_SIGNAL_LOSS debug[0] = rxSignalReceived; debug[1] = rxIsInFailsafeMode; - debug[2] = rxRuntimeConfig.rcReadRawFunc(&rxRuntimeConfig, 0); + debug[2] = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, 0); #endif rxResetFlightChannelStatus(); - for (channel = 0; channel < getRxChannelCount(); channel++) { + for (int channel = 0; channel < getRxChannelCount(); channel++) { - sample = (useValueFromRx) ? rcRaw[channel] : PPM_RCVR_TIMEOUT; + uint16_t sample = (useValueFromRx) ? rcRaw[channel] : PPM_RCVR_TIMEOUT; bool validPulse = isPulseValid(sample); @@ -556,7 +497,7 @@ static void detectAndApplySignalLossBehaviour(void) rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven = true; failsafeOnValidDataFailed(); - for (channel = 0; channel < getRxChannelCount(); channel++) { + for (int channel = 0; channel < getRxChannelCount(); channel++) { rcData[channel] = getRxfailValue(channel); } } @@ -579,7 +520,7 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) } readRxChannelsApplyRanges(); - detectAndApplySignalLossBehaviour(); + detectAndApplySignalLossBehaviour(currentTime); rcSampleIndex++; } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index ecea20120d..98a17e7c60 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -53,20 +53,17 @@ typedef enum { SERIALRX_XBUS_MODE_B_RJ01 = 6, SERIALRX_IBUS = 7, SERIALRX_JETIEXBUS = 8, - SERIALRX_PROVIDER_MAX = SERIALRX_JETIEXBUS + SERIALRX_PROVIDER_COUNT, + SERIALRX_PROVIDER_MAX = SERIALRX_PROVIDER_COUNT - 1 } SerialRXType; -#define SERIALRX_PROVIDER_COUNT (SERIALRX_PROVIDER_MAX + 1) - -#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12 -#define MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT 8 -#define MAX_SUPPORTED_RC_CHANNEL_COUNT (18) +#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12 +#define MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT 8 +#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18 #define NON_AUX_CHANNEL_COUNT 4 #define MAX_AUX_CHANNEL_COUNT (MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT) - - #if MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT > MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT #define MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT #else @@ -142,14 +139,14 @@ typedef struct rxConfig_s { #define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0])) struct rxRuntimeConfig_s; -typedef uint16_t (*rcReadRawDataPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data -typedef uint8_t (*rcFrameStatusPtr)(void); +typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data +typedef uint8_t (*rcFrameStatusFnPtr)(void); typedef struct rxRuntimeConfig_s { uint8_t channelCount; // number of RC channels as reported by current input driver uint16_t rxRefreshRate; - rcReadRawDataPtr rcReadRawFunc; - rcFrameStatusPtr rcFrameStatusFunc; + rcReadRawDataFnPtr rcReadRawFn; + rcFrameStatusFnPtr rcFrameStatusFn; } rxRuntimeConfig_t; extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount diff --git a/src/main/rx/rx_spi.c b/src/main/rx/rx_spi.c index f8fc711222..e4c5dbf80b 100644 --- a/src/main/rx/rx_spi.c +++ b/src/main/rx/rx_spi.c @@ -140,8 +140,8 @@ bool rxSpiInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxSpiNewPacketAvailable = false; rxRuntimeConfig->rxRefreshRate = 20000; - rxRuntimeConfig->rcReadRawFunc = rxSpiReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = rxSpiFrameStatus; + rxRuntimeConfig->rcReadRawFn = rxSpiReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = rxSpiFrameStatus; return ret; } diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index babea6fea8..95cb91c443 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -154,7 +154,7 @@ static void sbusDataReceive(uint16_t c) } } -uint8_t sbusFrameStatus(void) +static uint8_t sbusFrameStatus(void) { if (!sbusFrameDone) { return RX_FRAME_PENDING; @@ -222,7 +222,7 @@ static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t UNUSED(rxRuntimeConfig); // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D - return (0.625f * sbusChannelData[chan]) + 880; + return (5 * sbusChannelData[chan] / 8) + 880; } bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) @@ -234,8 +234,8 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL; rxRuntimeConfig->rxRefreshRate = 11000; - rxRuntimeConfig->rcReadRawFunc = sbusReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = sbusFrameStatus; + rxRuntimeConfig->rcReadRawFn = sbusReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h index 6799f5fcf9..1dbcc51942 100644 --- a/src/main/rx/sbus.h +++ b/src/main/rx/sbus.h @@ -17,5 +17,4 @@ #pragma once -uint8_t sbusFrameStatus(void); bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index aa29b169e5..c79ce8d8bd 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -106,7 +106,7 @@ static void spektrumDataReceive(uint16_t c) static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT]; -uint8_t spektrumFrameStatus(void) +static uint8_t spektrumFrameStatus(void) { if (!rcFrameComplete) { return RX_FRAME_PENDING; @@ -265,8 +265,8 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig break; } - rxRuntimeConfig->rcReadRawFunc = spektrumReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = spektrumFrameStatus; + rxRuntimeConfig->rcReadRawFn = spektrumReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = spektrumFrameStatus; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index 22ee2af41c..44aab64192 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -20,6 +20,5 @@ #define SPEKTRUM_SAT_BIND_DISABLED 0 #define SPEKTRUM_SAT_BIND_MAX 10 -uint8_t spektrumFrameStatus(void); void spektrumBind(rxConfig_t *rxConfig); bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 91906b0c7b..c2b8d8a524 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -112,7 +112,7 @@ static void sumdDataReceive(uint16_t c) #define SUMD_FRAME_STATE_OK 0x01 #define SUMD_FRAME_STATE_FAILSAFE 0x81 -uint8_t sumdFrameStatus(void) +static uint8_t sumdFrameStatus(void) { uint8_t channelIndex; @@ -165,8 +165,8 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->channelCount = SUMD_MAX_CHANNEL; rxRuntimeConfig->rxRefreshRate = 11000; - rxRuntimeConfig->rcReadRawFunc = sumdReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = sumdFrameStatus; + rxRuntimeConfig->rcReadRawFn = sumdReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = sumdFrameStatus; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/sumd.h b/src/main/rx/sumd.h index 376fe74f0b..97f1a2224d 100644 --- a/src/main/rx/sumd.h +++ b/src/main/rx/sumd.h @@ -17,5 +17,4 @@ #pragma once -uint8_t sumdFrameStatus(void); bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 98749638eb..56f4256632 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -80,7 +80,7 @@ static void sumhDataReceive(uint16_t c) } } -uint8_t sumhFrameStatus(void) +static uint8_t sumhFrameStatus(void) { uint8_t channelIndex; @@ -119,8 +119,8 @@ bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->channelCount = SUMH_MAX_CHANNEL_COUNT; rxRuntimeConfig->rxRefreshRate = 11000; - rxRuntimeConfig->rcReadRawFunc = sumhReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = sumhFrameStatus; + rxRuntimeConfig->rcReadRawFn = sumhReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = sumhFrameStatus; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/sumh.h b/src/main/rx/sumh.h index 3d3b398cbd..4914407a7d 100644 --- a/src/main/rx/sumh.h +++ b/src/main/rx/sumh.h @@ -17,5 +17,4 @@ #pragma once -uint8_t sumhFrameStatus(void); bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 09dbeb6a55..4ff900a8e4 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -238,10 +238,10 @@ static void xBusDataReceive(uint16_t c) // Done? if (xBusFramePosition == xBusFrameLength) { switch (xBusProvider) { - case SERIALRX_XBUS_MODE_B: - xBusUnpackModeBFrame(0); - case SERIALRX_XBUS_MODE_B_RJ01: - xBusUnpackRJ01Frame(); + case SERIALRX_XBUS_MODE_B: + xBusUnpackModeBFrame(0); + case SERIALRX_XBUS_MODE_B_RJ01: + xBusUnpackRJ01Frame(); } xBusDataIncoming = false; xBusFramePosition = 0; @@ -249,7 +249,7 @@ static void xBusDataReceive(uint16_t c) } // Indicate time to read a frame from the data... -uint8_t xBusFrameStatus(void) +static uint8_t xBusFrameStatus(void) { if (!xBusFrameReceived) { return RX_FRAME_PENDING; @@ -306,8 +306,8 @@ bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->rxRefreshRate = 11000; - rxRuntimeConfig->rcReadRawFunc = xBusReadRawRC; - rxRuntimeConfig->rcFrameStatusFunc = xBusFrameStatus; + rxRuntimeConfig->rcReadRawFn = xBusReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = xBusFrameStatus; const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h index ee6e6ee686..013ecb0fdc 100644 --- a/src/main/rx/xbus.h +++ b/src/main/rx/xbus.h @@ -18,4 +18,3 @@ #pragma once bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); -uint8_t xBusFrameStatus(void); diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index f47123b1b0..88983fc006 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -98,7 +98,7 @@ #else #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#undef SKIP_RX_MSP +#define USE_RX_MSP #define SPEKTRUM_BIND #define BIND_PIN PA3 // UART2, PA3 diff --git a/src/main/target/common.h b/src/main/target/common.h index e5de703ab8..fa3af098b3 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -21,7 +21,7 @@ #define I2C2_OVERCLOCK true -/* STM32F4 specific settings that apply to all F4 targets */ +// STM32F4 specific settings that apply to all F4 targets #ifdef STM32F4 #define MAX_AUX_CHANNELS 99 @@ -29,22 +29,27 @@ #define SCHEDULER_DELAY_LIMIT 10 #define I2C3_OVERCLOCK true -#else /* when not an F4 */ +#else // when not an F4 #define MAX_AUX_CHANNELS 6 #define TASK_GYROPID_DESIRED_PERIOD 1000 #define SCHEDULER_DELAY_LIMIT 100 -#endif +#endif // STM32F4 #ifdef STM32F1 // Using RX DMA disables the use of receive callbacks #define USE_UART1_RX_DMA #define USE_UART1_TX_DMA - -#endif +#endif // STM32F1 #define SERIAL_RX +#define USE_SERIALRX_SPEKTRUM // DSM2 and DSMX protocol +#define USE_SERIALRX_SBUS // Frsky and Futaba receivers +#define USE_SERIALRX_IBUS // FlySky and Turnigy receivers +#define USE_SERIALRX_SUMD // Graupner Hott protocol +#define USE_SERIALRX_SUMH // Graupner legacy protocol +#define USE_SERIALRX_XBUS // JR #define USE_CLI #if (FLASH_SIZE > 64) @@ -65,7 +70,8 @@ #define USE_MSP_DISPLAYPORT #define TELEMETRY_JETIEXBUS #define TELEMETRY_MAVLINK +#define USE_RX_MSP +#define USE_SERIALRX_JETIEXBUS #else #define SKIP_CLI_COMMAND_HELP -#define SKIP_RX_MSP #endif