From ccc9a5a9d4d9cea77feef2508e4822f9af309baa Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 14 Sep 2016 20:57:59 +0100 Subject: [PATCH 1/2] Minor tidy of receiver code --- src/main/config/config.c | 7 +------ src/main/config/config.h | 2 ++ src/main/config/config_master.h | 2 +- src/main/fc/rc_controls.h | 3 +++ src/main/rx/ibus.c | 4 ++-- src/main/rx/jetiexbus.c | 4 ++-- src/main/rx/msp.c | 2 +- src/main/rx/msp.h | 1 + src/main/rx/pwm.c | 4 ++-- src/main/rx/pwm.h | 1 + src/main/rx/rx.c | 12 +----------- src/main/rx/rx.h | 2 +- src/main/rx/sbus.c | 4 ++-- src/main/rx/sbus.h | 1 + src/main/rx/spektrum.c | 4 ++-- src/main/rx/spektrum.h | 5 ++--- src/main/rx/sumd.c | 4 ++-- src/main/rx/sumd.h | 1 + src/main/rx/sumh.c | 8 ++------ src/main/rx/sumh.h | 1 + src/main/rx/xbus.c | 4 ++-- src/main/telemetry/ltm.h | 3 ++- 22 files changed, 35 insertions(+), 44 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 5afd827e37..37ede806b3 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -35,8 +35,6 @@ #include "drivers/accgyro.h" #include "drivers/compass.h" #include "drivers/system.h" -#include "drivers/io.h" -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/serial.h" @@ -48,8 +46,8 @@ #include "sensors/compass.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" -#include "sensors/boardalignment.h" #include "sensors/battery.h" +#include "sensors/boardalignment.h" #include "io/beeper.h" #include "io/serial.h" @@ -77,7 +75,6 @@ #include "config/config.h" #include "config/config_eeprom.h" - #include "config/config_profile.h" #include "config/config_master.h" #include "config/feature.h" @@ -93,8 +90,6 @@ #define BRUSHLESS_MOTORS_PWM_RATE 400 #endif -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); -void targetConfiguration(master_t *config); master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; diff --git a/src/main/config/config.h b/src/main/config/config.h index 292fb13af8..d0e85f3612 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -83,3 +83,5 @@ void changeControlRateProfile(uint8_t profileIndex); bool canSoftwareSerialBeUsed(void); uint16_t getCurrentMinthrottle(void); +struct master_s; +void targetConfiguration(struct master_s *config); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index db3a3364cf..d902906c4d 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -18,7 +18,7 @@ #pragma once // System-wide -typedef struct master_t { +typedef struct master_s { uint8_t version; uint16_t size; uint8_t magic_be; // magic number, should be 0xBE diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 27d423e67e..dd25f7fa57 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -262,3 +262,6 @@ bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); +struct pidProfile_s; +struct escAndServoConfig_s; +void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct escAndServoConfig_s *escAndServoConfigToUse, struct pidProfile_s *pidProfileToUse); diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 562999ccf1..51a2a92f63 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -52,7 +52,7 @@ static bool ibusFrameDone = false; static uint32_t ibusChannelData[IBUS_MAX_CHANNEL]; static void ibusDataReceive(uint16_t c); -static uint16_t ibusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { @@ -141,7 +141,7 @@ uint8_t ibusFrameStatus(void) return frameStatus; } -static uint16_t ibusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { UNUSED(rxRuntimeConfig); return ibusChannelData[chan]; diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 2c8d2a72ab..3e19702578 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -208,7 +208,7 @@ static uint8_t jetiExBusRequestFrame[EXBUS_MAX_REQUEST_FRAME_SIZE]; static uint16_t jetiExBusChannelData[JETIEXBUS_CHANNEL_COUNT]; static void jetiExBusDataReceive(uint16_t c); -static uint16_t jetiExBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static void jetiExBusFrameReset(); @@ -416,7 +416,7 @@ uint8_t jetiExBusFrameStatus() } -static uint16_t jetiExBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t jetiExBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { if (chan >= rxRuntimeConfig->channelCount) return 0; diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index b850b9cab6..631fa412d6 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -36,7 +36,7 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; -static uint16_t rxMspReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) +static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { UNUSED(rxRuntimeConfigPtr); return mspFrame[chan]; diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 8b2ee6f7ce..4b004edc31 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -17,5 +17,6 @@ #pragma once +void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); bool rxMspFrameComplete(void); void rxMspFrameReceive(uint16_t *frame, int channelCount); diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index e01653e2f5..3ad5277a7b 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -37,13 +37,13 @@ #include "rx/rx.h" #include "rx/pwm.h" -static uint16_t pwmReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) +static uint16_t pwmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) { UNUSED(rxRuntimeConfigPtr); return pwmRead(channel); } -static uint16_t ppmReadRawRC(rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) +static uint16_t ppmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) { UNUSED(rxRuntimeConfigPtr); return ppmRead(channel); diff --git a/src/main/rx/pwm.h b/src/main/rx/pwm.h index e563a48c45..dc95576e39 100644 --- a/src/main/rx/pwm.h +++ b/src/main/rx/pwm.h @@ -16,3 +16,4 @@ */ #pragma once +void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 1cd27234d8..2b0fa6e1b2 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -58,16 +58,6 @@ //#define DEBUG_RX_SIGNAL_LOSS -void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); - -bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); -bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); -bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); -bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); -bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); - -void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); - const char rcChannelLetters[] = "AERT12345678abcdefgh"; uint16_t rssi = 0; // range: [0;1023] @@ -101,7 +91,7 @@ rxRuntimeConfig_t rxRuntimeConfig; static rxConfig_t *rxConfig; static uint8_t rcSampleIndex = 0; -static uint16_t nullReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { +static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { UNUSED(rxRuntimeConfig); UNUSED(channel); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index eef0aeb973..5e672e18bf 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -146,7 +146,7 @@ typedef struct rxRuntimeConfig_s { extern rxRuntimeConfig_t rxRuntimeConfig; -typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data +typedef uint16_t (*rcReadRawDataPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data struct modeActivationCondition_s; void rxInit(rxConfig_t *rxConfig, struct modeActivationCondition_s *modeActivationConditions); diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 742f783406..205d4fd5c1 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -80,7 +80,7 @@ static uint16_t sbusStateFlags = 0; static bool sbusFrameDone = false; static void sbusDataReceive(uint16_t c); -static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; @@ -254,7 +254,7 @@ uint8_t sbusFrameStatus(void) return SERIAL_RX_FRAME_COMPLETE; } -static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { UNUSED(rxRuntimeConfig); // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h index b48212de59..a7fd626f92 100644 --- a/src/main/rx/sbus.h +++ b/src/main/rx/sbus.h @@ -18,3 +18,4 @@ #pragma once uint8_t sbusFrameStatus(void); +bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 16fc166e71..610fc94301 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -60,7 +60,7 @@ static bool spekHiRes = false; static volatile uint8_t spekFrame[SPEK_FRAME_SIZE]; static void spektrumDataReceive(uint16_t c); -static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); static rxRuntimeConfig_t *rxRuntimeConfigPtr; @@ -163,7 +163,7 @@ uint8_t spektrumFrameStatus(void) return SERIAL_RX_FRAME_COMPLETE; } -static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { uint16_t data; diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index 5481bdebc0..ba2f7eb241 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -21,6 +21,5 @@ #define SPEKTRUM_SAT_BIND_MAX 10 uint8_t spektrumFrameStatus(void); -struct rxConfig_s; -void spektrumBind(struct rxConfig_s *rxConfig); - +void spektrumBind(rxConfig_t *rxConfig); +bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 70457d1110..8d7ef4f704 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -51,7 +51,7 @@ static uint16_t sumdChannels[SUMD_MAX_CHANNEL]; static uint16_t crc; static void sumdDataReceive(uint16_t c); -static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { @@ -186,7 +186,7 @@ uint8_t sumdFrameStatus(void) return frameStatus; } -static uint16_t sumdReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { UNUSED(rxRuntimeConfig); return sumdChannels[chan] / 8; diff --git a/src/main/rx/sumd.h b/src/main/rx/sumd.h index 946a242e94..e1bd4f3004 100644 --- a/src/main/rx/sumd.h +++ b/src/main/rx/sumd.h @@ -18,3 +18,4 @@ #pragma once uint8_t sumdFrameStatus(void); +bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 3b608a5d39..8364d53b99 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -54,15 +54,11 @@ static bool sumhFrameDone = false; static uint8_t sumhFrame[SUMH_FRAME_SIZE]; static uint32_t sumhChannels[SUMH_MAX_CHANNEL_COUNT]; -static void sumhDataReceive(uint16_t c); -static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); - static serialPort_t *sumhPort; static void sumhDataReceive(uint16_t c); -static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); - +static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) @@ -140,7 +136,7 @@ uint8_t sumhFrameStatus(void) return SERIAL_RX_FRAME_COMPLETE; } -static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/sumh.h b/src/main/rx/sumh.h index 42d6720eb3..3fbcf757d5 100644 --- a/src/main/rx/sumh.h +++ b/src/main/rx/sumh.h @@ -18,3 +18,4 @@ #pragma once uint8_t sumhFrameStatus(void); +bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index bbf4848bb7..c337d7056a 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -86,7 +86,7 @@ static volatile uint8_t xBusFrame[XBUS_RJ01_FRAME_SIZE]; static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT]; static void xBusDataReceive(uint16_t c); -static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); +static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) { @@ -319,7 +319,7 @@ uint8_t xBusFrameStatus(void) return SERIAL_RX_FRAME_COMPLETE; } -static uint16_t xBusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) { uint16_t data; diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index 6e4b80cbfa..e5b4790c5a 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -19,7 +19,8 @@ #pragma once -void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig); +struct telemetryConfig_s; +void initLtmTelemetry(struct telemetryConfig_s *initialTelemetryConfig); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); From 40ebc5d0a7f3ce8b7c4a6a49435ffe0998a4e20e Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 17 Sep 2016 13:15:31 +0100 Subject: [PATCH 2/2] Moved RX functions pointers into rxRuntimeConfig. --- src/main/fc/mw.c | 2 +- src/main/rx/ibus.c | 22 +++-- src/main/rx/ibus.h | 2 +- src/main/rx/jetiexbus.c | 32 ++++--- src/main/rx/jetiexbus.h | 2 +- src/main/rx/msp.c | 16 ++-- src/main/rx/msp.h | 4 +- src/main/rx/pwm.c | 29 +++--- src/main/rx/pwm.h | 3 +- src/main/rx/rx.c | 164 ++++++++++++++++----------------- src/main/rx/rx.h | 28 +++--- src/main/rx/sbus.c | 26 ++++-- src/main/rx/sbus.h | 2 +- src/main/rx/spektrum.c | 58 +++++++----- src/main/rx/spektrum.h | 2 +- src/main/rx/sumd.c | 22 +++-- src/main/rx/sumd.h | 2 +- src/main/rx/sumh.c | 21 +++-- src/main/rx/sumh.h | 2 +- src/main/rx/xbus.c | 64 +++++++------ src/main/rx/xbus.h | 4 +- src/main/telemetry/telemetry.c | 2 +- src/main/telemetry/telemetry.h | 2 +- 23 files changed, 271 insertions(+), 240 deletions(-) diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 40ae307f6a..d756d11826 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -266,7 +266,7 @@ void processRcCommand(void) case(RC_SMOOTHING_OFF): case(RC_SMOOTHING_DEFAULT): default: - initRxRefreshRate(&rxRefreshRate); + rxRefreshRate = rxGetRefreshRate(); } rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 51a2a92f63..9deea55d31 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -25,7 +25,9 @@ #include #include -#include +#include "platform.h" + +#ifdef SERIAL_RX #include "build/build_config.h" @@ -54,16 +56,17 @@ static uint32_t ibusChannelData[IBUS_MAX_CHANNEL]; static void ibusDataReceive(uint16_t c); static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); -bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxConfig); - if (callback) - *callback = ibusReadRawRC; - rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL; + rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + rxRuntimeConfig->rcReadRawFunc = ibusReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = ibusFrameStatus; + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { return false; } @@ -116,7 +119,7 @@ static void ibusDataReceive(uint16_t c) uint8_t ibusFrameStatus(void) { uint8_t i, offset; - uint8_t frameStatus = SERIAL_RX_FRAME_PENDING; + uint8_t frameStatus = RX_FRAME_PENDING; uint16_t chksum, rxsum; if (!ibusFrameDone) { @@ -128,14 +131,14 @@ uint8_t ibusFrameStatus(void) chksum = 0xFFFF; for (i = 0; i < 30; i++) chksum -= ibus[i]; - + rxsum = ibus[30] + (ibus[31] << 8); if (chksum == rxsum) { for (i = 0, offset = 2; i < IBUS_MAX_CHANNEL; i++, offset += 2) { ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8); } - frameStatus = SERIAL_RX_FRAME_COMPLETE; + frameStatus = RX_FRAME_COMPLETE; } return frameStatus; @@ -146,3 +149,4 @@ static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t UNUSED(rxRuntimeConfig); return ibusChannelData[chan]; } +#endif diff --git a/src/main/rx/ibus.h b/src/main/rx/ibus.h index 1106659fd5..76bc244d2d 100755 --- a/src/main/rx/ibus.h +++ b/src/main/rx/ibus.h @@ -18,4 +18,4 @@ #pragma once uint8_t ibusFrameStatus(void); -bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 3e19702578..70292c6c80 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -39,6 +39,8 @@ #include "platform.h" +#ifdef SERIAL_RX + #include "build/build_config.h" #include "build/debug.h" @@ -53,19 +55,20 @@ #include "rx/rx.h" #include "rx/jetiexbus.h" - #ifdef TELEMETRY - #include #include "sensors/sensors.h" #include "sensors/battery.h" #include "sensors/barometer.h" #include "telemetry/telemetry.h" #include "telemetry/jetiexbus.h" - #endif //TELEMETRY +#include "build/debug.h" + +#include "rx/rx.h" + // // Serial driver for Jeti EX Bus receiver // @@ -225,19 +228,19 @@ uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen); uint16_t calcCRC16(uint8_t *pt, uint8_t msgLen); -bool jetiExBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxConfig); - serialPortConfig_t *portConfig; - - if (callback) { - *callback = jetiExBusReadRawRC; - } rxRuntimeConfig->channelCount = JETIEXBUS_CHANNEL_COUNT; + rxRuntimeConfig->rxRefreshRate = 5500; + + rxRuntimeConfig->rcReadRawFunc = jetiExBusReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = jetiExBusFrameStatus; + jetiExBusFrameReset(); - portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { return false; @@ -403,15 +406,15 @@ static void jetiExBusDataReceive(uint16_t c) uint8_t jetiExBusFrameStatus() { if (jetiExBusFrameState != EXBUS_STATE_RECEIVED) - return SERIAL_RX_FRAME_PENDING; + return RX_FRAME_PENDING; if(calcCRC16(jetiExBusChannelFrame, jetiExBusChannelFrame[EXBUS_HEADER_MSG_LEN]) == 0) { jetiExBusDecodeChannelFrame(jetiExBusChannelFrame); jetiExBusFrameState = EXBUS_STATE_ZERO; - return SERIAL_RX_FRAME_COMPLETE; + return RX_FRAME_COMPLETE; } else { jetiExBusFrameState = EXBUS_STATE_ZERO; - return SERIAL_RX_FRAME_PENDING; + return RX_FRAME_PENDING; } } @@ -610,4 +613,5 @@ void sendJetiExBusTelemetry(uint8_t packetID) requestLoop++; } -#endif //TELEMETRY +#endif // TELEMETRY +#endif // SERIAL_RX diff --git a/src/main/rx/jetiexbus.h b/src/main/rx/jetiexbus.h index 52b0bb45c4..98c18dcb2d 100644 --- a/src/main/rx/jetiexbus.h +++ b/src/main/rx/jetiexbus.h @@ -17,6 +17,6 @@ #pragma once -bool jetiExBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +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 631fa412d6..92e9bcd6d7 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -25,10 +25,8 @@ #include "build/build_config.h" #include "drivers/system.h" - #include "drivers/serial.h" #include "drivers/serial_uart.h" -#include "io/serial.h" #include "rx/rx.h" #include "rx/msp.h" @@ -56,21 +54,23 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) rxMspFrameDone = true; } -bool rxMspFrameComplete(void) +uint8_t rxMspFrameStatus(void) { if (!rxMspFrameDone) { - return false; + return RX_FRAME_PENDING; } rxMspFrameDone = false; - return true; + return RX_FRAME_COMPLETE; } -void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxConfig); + rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT; - if (callback) - *callback = rxMspReadRawRC; + + rxRuntimeConfig->rcReadRawFunc = rxMspReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = rxMspFrameStatus; } #endif diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 4b004edc31..94043ba7e2 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -17,6 +17,6 @@ #pragma once -void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); -bool rxMspFrameComplete(void); +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 3ad5277a7b..b8f0c89945 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -27,7 +27,6 @@ #include "build/build_config.h" -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" @@ -37,30 +36,32 @@ #include "rx/rx.h" #include "rx/pwm.h" -static uint16_t pwmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) +static uint16_t pwmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { - UNUSED(rxRuntimeConfigPtr); + UNUSED(rxRuntimeConfig); return pwmRead(channel); } -static uint16_t ppmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t channel) +static uint16_t ppmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { - UNUSED(rxRuntimeConfigPtr); + UNUSED(rxRuntimeConfig); return ppmRead(channel); } -void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfigPtr, rcReadRawDataPtr *callback) +void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { - UNUSED(rxRuntimeConfigPtr); + UNUSED(rxConfig); + + rxRuntimeConfig->rxRefreshRate = 20000; + // 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)) { - rxRuntimeConfigPtr->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; - *callback = pwmReadRawRC; - } - if (feature(FEATURE_RX_PPM)) { - rxRuntimeConfigPtr->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; - *callback = ppmReadRawRC; + rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; + rxRuntimeConfig->rcReadRawFunc = pwmReadRawRC; + } else if (feature(FEATURE_RX_PPM)) { + rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; + rxRuntimeConfig->rcReadRawFunc = ppmReadRawRC; } } -#endif // SKIP_RX_PWM_PPM +#endif diff --git a/src/main/rx/pwm.h b/src/main/rx/pwm.h index dc95576e39..100c2b7014 100644 --- a/src/main/rx/pwm.h +++ b/src/main/rx/pwm.h @@ -16,4 +16,5 @@ */ #pragma once -void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); + +void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 2b0fa6e1b2..b2b581bcad 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -71,6 +71,7 @@ static bool rxIsInFailsafeModeNotDataDriven = true; static uint32_t rxUpdateAt = 0; static uint32_t needRxSignalBefore = 0; +static uint32_t needRxSignalMaxDelayUs; static uint32_t suspendRxSignalUntil = 0; static uint8_t skipRxSamples = 0; @@ -88,22 +89,25 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; #define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent) rxRuntimeConfig_t rxRuntimeConfig; -static rxConfig_t *rxConfig; +static const rxConfig_t *rxConfig; static uint8_t rcSampleIndex = 0; -static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { +static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) +{ UNUSED(rxRuntimeConfig); UNUSED(channel); return PPM_RCVR_TIMEOUT; } -static rcReadRawDataPtr rcReadRawFunc = nullReadRawRC; -static uint16_t rxRefreshRate; +static uint8_t nullFrameStatus(void) +{ + return RX_FRAME_PENDING; +} -void serialRxInit(rxConfig_t *rxConfig); +void serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); -void useRxConfig(rxConfig_t *rxConfigToUse) +void useRxConfig(const rxConfig_t *rxConfigToUse) { rxConfig = rxConfigToUse; } @@ -145,13 +149,16 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChann } } -void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions) +void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions) { uint8_t i; uint16_t value; useRxConfig(rxConfig); + rxRuntimeConfig.rcReadRawFunc = nullReadRawRC; + rxRuntimeConfig.rcFrameStatusFunc = nullFrameStatus; rcSampleIndex = 0; + needRxSignalMaxDelayUs = DELAY_10_HZ; for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rcData[i] = rxConfig->midrc; @@ -162,7 +169,7 @@ void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationCondi // Initialize ARM switch to OFF position when arming via switch is defined for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[i]; + const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[i]; if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) { // ARM switch is defined, determine an OFF value if (modeActivationCondition->range.startStep > 0) { @@ -177,73 +184,61 @@ void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationCondi #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { - serialRxInit(rxConfig); + serialRxInit(rxConfig, &rxRuntimeConfig); } #endif #ifndef SKIP_RX_MSP if (feature(FEATURE_RX_MSP)) { - rxMspInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); + rxMspInit(rxConfig, &rxRuntimeConfig); + needRxSignalMaxDelayUs = DELAY_5_HZ; } #endif #ifndef SKIP_RX_PWM_PPM if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { - rxRefreshRate = 20000; - rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc); + rxPwmInit(rxConfig, &rxRuntimeConfig); } #endif - - rxRuntimeConfig.auxChannelCount = rxRuntimeConfig.channelCount - STICK_CHANNEL_COUNT; } #ifdef SERIAL_RX -void serialRxInit(rxConfig_t *rxConfig) +void serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { bool enabled = false; switch (rxConfig->serialrx_provider) { - case SERIALRX_SPEKTRUM1024: - rxRefreshRate = 22000; - enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); - break; - case SERIALRX_SPEKTRUM2048: - rxRefreshRate = 11000; - enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); - break; - case SERIALRX_SBUS: - rxRefreshRate = 11000; - enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); - break; - case SERIALRX_SUMD: - rxRefreshRate = 11000; - enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); - break; - case SERIALRX_SUMH: - rxRefreshRate = 11000; - enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); - break; - case SERIALRX_XBUS_MODE_B: - case SERIALRX_XBUS_MODE_B_RJ01: - rxRefreshRate = 11000; - enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); - break; - case SERIALRX_IBUS: - rxRefreshRate = 20000; // TODO - Verify speed - enabled = ibusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); - break; - case SERIALRX_JETIEXBUS: - rxRefreshRate = 5500; - enabled = jetiExBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); - break; + case SERIALRX_SPEKTRUM1024: + case SERIALRX_SPEKTRUM2048: + enabled = spektrumInit(rxConfig, rxRuntimeConfig); + break; + case SERIALRX_SBUS: + enabled = sbusInit(rxConfig, rxRuntimeConfig); + break; + case SERIALRX_SUMD: + enabled = sumdInit(rxConfig, rxRuntimeConfig); + break; + case SERIALRX_SUMH: + enabled = sumhInit(rxConfig, rxRuntimeConfig); + break; + case SERIALRX_XBUS_MODE_B: + case SERIALRX_XBUS_MODE_B_RJ01: + enabled = xBusInit(rxConfig, rxRuntimeConfig); + break; + case SERIALRX_IBUS: + enabled = ibusInit(rxConfig, rxRuntimeConfig); + break; + case SERIALRX_JETIEXBUS: + enabled = jetiExBusInit(rxConfig, rxRuntimeConfig); + break; } if (!enabled) { featureClear(FEATURE_RX_SERIAL); - rcReadRawFunc = nullReadRawRC; + rxRuntimeConfig->rcReadRawFunc = nullReadRawRC; } } -uint8_t serialRxFrameStatus(rxConfig_t *rxConfig) +static uint8_t serialRxFrameStatus(const rxConfig_t *rxConfig) { /** * FIXME: Each of the xxxxFrameStatus() methods MUST be able to survive being called without the @@ -255,28 +250,28 @@ uint8_t serialRxFrameStatus(rxConfig_t *rxConfig) * 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(); + 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 SERIAL_RX_FRAME_PENDING; + return RX_FRAME_PENDING; } #endif -uint8_t calculateChannelRemapping(uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap) +static uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap) { if (channelToRemap < channelMapEntryCount) { return channelMap[channelToRemap]; @@ -334,24 +329,24 @@ bool rxUpdate(uint32_t currentTime) #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { - uint8_t frameStatus = serialRxFrameStatus(rxConfig); - - if (frameStatus & SERIAL_RX_FRAME_COMPLETE) { + const uint8_t frameStatus = serialRxFrameStatus(rxConfig); + if (frameStatus & RX_FRAME_COMPLETE) { rxDataReceived = true; - rxIsInFailsafeMode = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) != 0; + rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0; rxSignalReceived = !rxIsInFailsafeMode; - needRxSignalBefore = currentTime + DELAY_10_HZ; + needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; } } #endif #ifndef SKIP_RX_MSP if (feature(FEATURE_RX_MSP)) { - rxDataReceived = rxMspFrameComplete(); - if (rxDataReceived) { - rxSignalReceived = true; + const uint8_t frameStatus = rxMspFrameStatus(); + if (frameStatus & RX_FRAME_COMPLETE) { + rxDataReceived = true; rxIsInFailsafeMode = false; - needRxSignalBefore = currentTime + DELAY_5_HZ; + rxSignalReceived = !rxIsInFailsafeMode; + needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; } } #endif @@ -361,7 +356,7 @@ bool rxUpdate(uint32_t currentTime) if (isPPMDataBeingReceived()) { rxSignalReceivedNotDataDriven = true; rxIsInFailsafeModeNotDataDriven = false; - needRxSignalBefore = currentTime + DELAY_10_HZ; + needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; resetPPMDataReceivedState(); } } @@ -370,7 +365,7 @@ bool rxUpdate(uint32_t currentTime) if (isPWMDataBeingReceived()) { rxSignalReceivedNotDataDriven = true; rxIsInFailsafeModeNotDataDriven = false; - needRxSignalBefore = currentTime + DELAY_10_HZ; + needRxSignalBefore = currentTime + needRxSignalMaxDelayUs; } } #endif @@ -407,7 +402,7 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) static uint16_t getRxfailValue(uint8_t channel) { - rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; + const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel]; switch(channelFailsafeConfiguration->mode) { case RX_FAILSAFE_MODE_AUTO: @@ -472,7 +467,7 @@ static void readRxChannelsApplyRanges(void) uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); // sample the channel - uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel); + uint16_t sample = rxRuntimeConfig.rcReadRawFunc(&rxRuntimeConfig, rawChannel); // apply the rx calibration if (channel < NON_AUX_CHANNEL_COUNT) { @@ -503,7 +498,7 @@ static void detectAndApplySignalLossBehaviour(void) #ifdef DEBUG_RX_SIGNAL_LOSS debug[0] = rxSignalReceived; debug[1] = rxIsInFailsafeMode; - debug[2] = rcReadRawFunc(&rxRuntimeConfig, 0); + debug[2] = rxRuntimeConfig.rcReadRawFunc(&rxRuntimeConfig, 0); #endif rxResetFlightChannelStatus(); @@ -641,6 +636,7 @@ void updateRSSI(uint32_t currentTime) } } -void initRxRefreshRate(uint16_t *rxRefreshRatePtr) { - *rxRefreshRatePtr = rxRefreshRate; +uint16_t rxGetRefreshRate(void) +{ + return rxRuntimeConfig.rxRefreshRate; } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 5e672e18bf..3a1d6c9d62 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -38,10 +38,10 @@ #define DEFAULT_SERVO_MAX_ANGLE 90 typedef enum { - SERIAL_RX_FRAME_PENDING = 0, - SERIAL_RX_FRAME_COMPLETE = (1 << 0), - SERIAL_RX_FRAME_FAILSAFE = (1 << 1) -} serialrxFrameState_t; + RX_FRAME_PENDING = 0, + RX_FRAME_COMPLETE = (1 << 0), + RX_FRAME_FAILSAFE = (1 << 1) +} rxFrameState_e; typedef enum { SERIALRX_SPEKTRUM1024 = 0, @@ -139,31 +139,33 @@ 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 struct rxRuntimeConfig_s { uint8_t channelCount; // number of rc channels as reported by current input driver - uint8_t auxChannelCount; + uint16_t rxRefreshRate; + rcReadRawDataPtr rcReadRawFunc; + rcFrameStatusPtr rcFrameStatusFunc; } rxRuntimeConfig_t; -extern rxRuntimeConfig_t rxRuntimeConfig; - -typedef uint16_t (*rcReadRawDataPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data +extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount struct modeActivationCondition_s; -void rxInit(rxConfig_t *rxConfig, struct modeActivationCondition_s *modeActivationConditions); -void useRxConfig(rxConfig_t *rxConfigToUse); +void rxInit(const rxConfig_t *rxConfig, const struct modeActivationCondition_s *modeActivationConditions); +void useRxConfig(const rxConfig_t *rxConfigToUse); bool rxUpdate(uint32_t currentTime); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime); void parseRcChannels(const char *input, rxConfig_t *rxConfig); -uint8_t serialRxFrameStatus(rxConfig_t *rxConfig); void updateRSSI(uint32_t currentTime); void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration); -void initRxRefreshRate(uint16_t *rxRefreshRatePtr); void suspendRxSignal(void); void resumeRxSignal(void); -void initRxRefreshRate(uint16_t *rxRefreshRatePtr); +uint16_t rxGetRefreshRate(void); diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 205d4fd5c1..e4c30df786 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -21,6 +21,8 @@ #include "platform.h" +#ifdef SERIAL_RX + #include "build/build_config.h" #include "drivers/system.h" @@ -84,16 +86,19 @@ static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t static uint32_t sbusChannelData[SBUS_MAX_CHANNEL]; -bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { - int b; - for (b = 0; b < SBUS_MAX_CHANNEL; b++) + for (int b = 0; b < SBUS_MAX_CHANNEL; b++) { sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408; - if (callback) - *callback = sbusReadRawRC; - rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL; + } - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL; + rxRuntimeConfig->rxRefreshRate = 11000; + + rxRuntimeConfig->rcReadRawFunc = sbusReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = sbusFrameStatus; + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { return false; } @@ -194,7 +199,7 @@ static void sbusDataReceive(uint16_t c) uint8_t sbusFrameStatus(void) { if (!sbusFrameDone) { - return SERIAL_RX_FRAME_PENDING; + return RX_FRAME_PENDING; } sbusFrameDone = false; @@ -245,13 +250,13 @@ uint8_t sbusFrameStatus(void) debug[0] = sbusStateFlags; #endif // RX *should* still be sending valid channel data, so use it. - return SERIAL_RX_FRAME_COMPLETE | SERIAL_RX_FRAME_FAILSAFE; + return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; } #ifdef DEBUG_SBUS_PACKETS debug[0] = sbusStateFlags; #endif - return SERIAL_RX_FRAME_COMPLETE; + return RX_FRAME_COMPLETE; } static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) @@ -261,3 +266,4 @@ static uint16_t sbusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t // 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; } +#endif diff --git a/src/main/rx/sbus.h b/src/main/rx/sbus.h index a7fd626f92..6799f5fcf9 100644 --- a/src/main/rx/sbus.h +++ b/src/main/rx/sbus.h @@ -18,4 +18,4 @@ #pragma once uint8_t sbusFrameStatus(void); -bool sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 610fc94301..202ee1e498 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -21,6 +21,8 @@ #include "platform.h" +#ifdef SERIAL_RX + #include "build/debug.h" #include "drivers/io.h" @@ -49,6 +51,7 @@ #define SPEKTRUM_1024_CHANNEL_COUNT 7 #define SPEK_FRAME_SIZE 16 +#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000 #define SPEKTRUM_BAUDRATE 115200 @@ -71,31 +74,33 @@ static IO_t BindPin = DEFIO_IO(NONE); static IO_t BindPlug = DEFIO_IO(NONE); #endif -bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { rxRuntimeConfigPtr = rxRuntimeConfig; switch (rxConfig->serialrx_provider) { - case SERIALRX_SPEKTRUM2048: - // 11 bit frames - spek_chan_shift = 3; - spek_chan_mask = 0x07; - spekHiRes = true; - rxRuntimeConfig->channelCount = SPEKTRUM_2048_CHANNEL_COUNT; - break; - case SERIALRX_SPEKTRUM1024: - // 10 bit frames - spek_chan_shift = 2; - spek_chan_mask = 0x03; - spekHiRes = false; - rxRuntimeConfig->channelCount = SPEKTRUM_1024_CHANNEL_COUNT; - break; + case SERIALRX_SPEKTRUM2048: + // 11 bit frames + spek_chan_shift = 3; + spek_chan_mask = 0x07; + spekHiRes = true; + rxRuntimeConfig->channelCount = SPEKTRUM_2048_CHANNEL_COUNT; + rxRuntimeConfig->rxRefreshRate = 11000; + break; + case SERIALRX_SPEKTRUM1024: + // 10 bit frames + spek_chan_shift = 2; + spek_chan_mask = 0x03; + spekHiRes = false; + rxRuntimeConfig->channelCount = SPEKTRUM_1024_CHANNEL_COUNT; + rxRuntimeConfig->rxRefreshRate = 22000; + break; } - if (callback) - *callback = spektrumReadRawRC; + rxRuntimeConfig->rcReadRawFunc = spektrumReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = spektrumFrameStatus; - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { return false; } @@ -120,14 +125,15 @@ bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcRe // Receive ISR callback static void spektrumDataReceive(uint16_t c) { - uint32_t spekTime; - static uint32_t spekTimeLast, spekTimeInterval; - static uint8_t spekFramePosition; + uint32_t spekTime, spekTimeInterval; + static uint32_t spekTimeLast = 0; + static uint8_t spekFramePosition = 0; spekTime = micros(); spekTimeInterval = spekTime - spekTimeLast; spekTimeLast = spekTime; - if (spekTimeInterval > 5000) { + + if (spekTimeInterval > SPEKTRUM_NEEDED_FRAME_INTERVAL) { spekFramePosition = 0; } @@ -148,7 +154,7 @@ uint8_t spektrumFrameStatus(void) uint8_t b; if (!rcFrameComplete) { - return SERIAL_RX_FRAME_PENDING; + return RX_FRAME_PENDING; } rcFrameComplete = false; @@ -160,7 +166,7 @@ uint8_t spektrumFrameStatus(void) } } - return SERIAL_RX_FRAME_COMPLETE; + return RX_FRAME_COMPLETE; } static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) @@ -254,4 +260,6 @@ void spektrumBind(rxConfig_t *rxConfig) #endif } -#endif +#endif // SPEKTRUM_BIND +#endif // SERIAL_RX + diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index ba2f7eb241..22ee2af41c 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -22,4 +22,4 @@ uint8_t spektrumFrameStatus(void); void spektrumBind(rxConfig_t *rxConfig); -bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 8d7ef4f704..d0ef36cf89 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -21,12 +21,14 @@ #include "platform.h" +#ifdef SERIAL_RX + #include "build/build_config.h" #include "drivers/system.h" - #include "drivers/serial.h" #include "drivers/serial_uart.h" + #include "io/serial.h" #ifdef TELEMETRY @@ -53,16 +55,17 @@ static uint16_t crc; static void sumdDataReceive(uint16_t c); static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); -bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxConfig); - if (callback) - *callback = sumdReadRawRC; - rxRuntimeConfig->channelCount = SUMD_MAX_CHANNEL; + rxRuntimeConfig->rxRefreshRate = 11000; - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + rxRuntimeConfig->rcReadRawFunc = sumdReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = sumdFrameStatus; + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { return false; } @@ -150,7 +153,7 @@ uint8_t sumdFrameStatus(void) { uint8_t channelIndex; - uint8_t frameStatus = SERIAL_RX_FRAME_PENDING; + uint8_t frameStatus = RX_FRAME_PENDING; if (!sumdFrameDone) { return frameStatus; @@ -165,10 +168,10 @@ uint8_t sumdFrameStatus(void) switch (sumd[1]) { case SUMD_FRAME_STATE_FAILSAFE: - frameStatus = SERIAL_RX_FRAME_COMPLETE | SERIAL_RX_FRAME_FAILSAFE; + frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; break; case SUMD_FRAME_STATE_OK: - frameStatus = SERIAL_RX_FRAME_COMPLETE; + frameStatus = RX_FRAME_COMPLETE; break; default: return frameStatus; @@ -191,3 +194,4 @@ static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t UNUSED(rxRuntimeConfig); return sumdChannels[chan] / 8; } +#endif diff --git a/src/main/rx/sumd.h b/src/main/rx/sumd.h index e1bd4f3004..376fe74f0b 100644 --- a/src/main/rx/sumd.h +++ b/src/main/rx/sumd.h @@ -18,4 +18,4 @@ #pragma once uint8_t sumdFrameStatus(void); -bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 8364d53b99..c3c518a304 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -27,6 +27,8 @@ #include "platform.h" +#ifdef SERIAL_RX + #include "build/build_config.h" #include "drivers/system.h" @@ -56,21 +58,21 @@ static uint32_t sumhChannels[SUMH_MAX_CHANNEL_COUNT]; static serialPort_t *sumhPort; - static void sumhDataReceive(uint16_t c); static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); -bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxConfig); - if (callback) - *callback = sumhReadRawRC; - rxRuntimeConfig->channelCount = SUMH_MAX_CHANNEL_COUNT; + rxRuntimeConfig->rxRefreshRate = 11000; - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + rxRuntimeConfig->rcReadRawFunc = sumhReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = sumhFrameStatus; + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { return false; } @@ -120,20 +122,20 @@ uint8_t sumhFrameStatus(void) uint8_t channelIndex; if (!sumhFrameDone) { - return SERIAL_RX_FRAME_PENDING; + return RX_FRAME_PENDING; } sumhFrameDone = false; if (!((sumhFrame[0] == 0xA8) && (sumhFrame[SUMH_FRAME_SIZE - 2] == 0))) { - return SERIAL_RX_FRAME_PENDING; + return RX_FRAME_PENDING; } for (channelIndex = 0; channelIndex < SUMH_MAX_CHANNEL_COUNT; channelIndex++) { sumhChannels[channelIndex] = (((uint32_t)(sumhFrame[(channelIndex << 1) + 3]) << 8) + sumhFrame[(channelIndex << 1) + 4]) / 6.4f - 375; } - return SERIAL_RX_FRAME_COMPLETE; + return RX_FRAME_COMPLETE; } static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) @@ -146,3 +148,4 @@ static uint16_t sumhReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t return sumhChannels[chan]; } +#endif diff --git a/src/main/rx/sumh.h b/src/main/rx/sumh.h index 3fbcf757d5..3d3b398cbd 100644 --- a/src/main/rx/sumh.h +++ b/src/main/rx/sumh.h @@ -18,4 +18,4 @@ #pragma once uint8_t sumhFrameStatus(void); -bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index c337d7056a..15d2d84623 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -21,6 +21,8 @@ #include "platform.h" +#ifdef SERIAL_RX + #include "drivers/system.h" #include "drivers/serial.h" @@ -88,41 +90,42 @@ static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT]; static void xBusDataReceive(uint16_t c); static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); -bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback) +bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { uint32_t baudRate; switch (rxConfig->serialrx_provider) { - case SERIALRX_XBUS_MODE_B: - rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT; - xBusFrameReceived = false; - xBusDataIncoming = false; - xBusFramePosition = 0; - baudRate = XBUS_BAUDRATE; - xBusFrameLength = XBUS_FRAME_SIZE; - xBusChannelCount = XBUS_CHANNEL_COUNT; - xBusProvider = SERIALRX_XBUS_MODE_B; - break; - case SERIALRX_XBUS_MODE_B_RJ01: - rxRuntimeConfig->channelCount = XBUS_RJ01_CHANNEL_COUNT; - xBusFrameReceived = false; - xBusDataIncoming = false; - xBusFramePosition = 0; - baudRate = XBUS_RJ01_BAUDRATE; - xBusFrameLength = XBUS_RJ01_FRAME_SIZE; - xBusChannelCount = XBUS_RJ01_CHANNEL_COUNT; - xBusProvider = SERIALRX_XBUS_MODE_B_RJ01; - break; - default: - return false; - break; + case SERIALRX_XBUS_MODE_B: + rxRuntimeConfig->channelCount = XBUS_CHANNEL_COUNT; + xBusFrameReceived = false; + xBusDataIncoming = false; + xBusFramePosition = 0; + baudRate = XBUS_BAUDRATE; + xBusFrameLength = XBUS_FRAME_SIZE; + xBusChannelCount = XBUS_CHANNEL_COUNT; + xBusProvider = SERIALRX_XBUS_MODE_B; + break; + case SERIALRX_XBUS_MODE_B_RJ01: + rxRuntimeConfig->channelCount = XBUS_RJ01_CHANNEL_COUNT; + xBusFrameReceived = false; + xBusDataIncoming = false; + xBusFramePosition = 0; + baudRate = XBUS_RJ01_BAUDRATE; + xBusFrameLength = XBUS_RJ01_FRAME_SIZE; + xBusChannelCount = XBUS_RJ01_CHANNEL_COUNT; + xBusProvider = SERIALRX_XBUS_MODE_B_RJ01; + break; + default: + return false; + break; } - if (callback) { - *callback = xBusReadRawRC; - } + rxRuntimeConfig->rxRefreshRate = 11000; - serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + rxRuntimeConfig->rcReadRawFunc = xBusReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = xBusFrameStatus; + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); if (!portConfig) { return false; } @@ -311,12 +314,12 @@ static void xBusDataReceive(uint16_t c) uint8_t xBusFrameStatus(void) { if (!xBusFrameReceived) { - return SERIAL_RX_FRAME_PENDING; + return RX_FRAME_PENDING; } xBusFrameReceived = false; - return SERIAL_RX_FRAME_COMPLETE; + return RX_FRAME_COMPLETE; } static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) @@ -332,3 +335,4 @@ static uint16_t xBusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t return data; } +#endif diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h index cfaa920ffd..ee6e6ee686 100644 --- a/src/main/rx/xbus.h +++ b/src/main/rx/xbus.h @@ -17,7 +17,5 @@ #pragma once -struct rxConfig_s; -struct rxRuntimeConfig_s; -bool xBusInit(struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig, rcReadRawDataPtr *callback); +bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); uint8_t xBusFrameStatus(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 84f40b0204..a93e21c2ac 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -74,7 +74,7 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) return enabled; } -bool telemetryCheckRxPortShared(serialPortConfig_t *portConfig) +bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig) { return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK; } diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 11c9e7a45c..c730ae0dec 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -48,7 +48,7 @@ typedef struct telemetryConfig_s { } telemetryConfig_t; void telemetryInit(void); -bool telemetryCheckRxPortShared(serialPortConfig_t *portConfig); +bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); extern serialPort_t *telemetrySharedPort; void telemetryCheckState(void);