diff --git a/src/main/io/serial.c b/src/main/io/serial.c index a0b5122da3..b2effbb4b7 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -194,7 +194,11 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction return NULL; } +#ifdef TELEMETRY +#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT) +#else #define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM) +#endif #define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK) bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) @@ -203,7 +207,8 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) /* * rules: * - 1 MSP port minimum, max MSP ports is defined and must be adhered to. - * - Only MSP is allowed to be shared with EITHER any telemetry OR blackbox. + * - MSP is allowed to be shared with EITHER any telemetry OR blackbox. + * - serial RX and FrSky / LTM telemetry can be shared * - No other sharing combinations are valid. */ uint8_t mspPortCount = 0; @@ -223,12 +228,14 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) return false; } - if (!(portConfig->functionMask & FUNCTION_MSP)) { - return false; - } - - if (!(portConfig->functionMask & ALL_FUNCTIONS_SHARABLE_WITH_MSP)) { - // some other bit must have been set. + if ((portConfig->functionMask & FUNCTION_MSP) && (portConfig->functionMask & ALL_FUNCTIONS_SHARABLE_WITH_MSP)) { + // MSP & telemetry +#ifdef TELEMETRY + } else if (telemetryCheckRxPortShared(portConfig)) { + // serial RX & telemetry +#endif + } else { + // some other combination return false; } } diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 299ff4fce2..507df1eff3 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -35,6 +35,10 @@ #include "drivers/serial_uart.h" #include "io/serial.h" +#ifdef TELEMETRY +#include "telemetry/telemetry.h" +#endif + #include "rx/rx.h" #include "rx/ibus.h" @@ -64,7 +68,19 @@ bool ibusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return false; } - serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); +#ifdef TELEMETRY + bool portShared = telemetryCheckRxPortShared(portConfig); +#else + bool portShared = false; +#endif + + serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED); + +#ifdef TELEMETRY + if (portShared) { + telemetrySharedPort = ibusPort; + } +#endif return ibusPort != NULL; } diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 5ae2669751..8613a28fdd 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -32,6 +32,9 @@ #include "drivers/serial_uart.h" #include "io/serial.h" +#ifdef TELEMETRY +#include "telemetry/telemetry.h" +#endif #include "rx/rx.h" #include "rx/sbus.h" @@ -95,8 +98,20 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return false; } +#ifdef TELEMETRY + bool portShared = telemetryCheckRxPortShared(portConfig); +#else + bool portShared = false; +#endif + portOptions_t options = (rxConfig->sbus_inversion) ? (SBUS_PORT_OPTIONS | SERIAL_INVERTED) : SBUS_PORT_OPTIONS; - serialPort_t *sBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sbusDataReceive, SBUS_BAUDRATE, MODE_RX, options); + serialPort_t *sBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sbusDataReceive, SBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, options); + +#ifdef TELEMETRY + if (portShared) { + telemetrySharedPort = sBusPort; + } +#endif return sBusPort != NULL; } diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 39fb03488a..9c0f211028 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -34,6 +34,10 @@ #include "config/config.h" +#ifdef TELEMETRY +#include "telemetry/telemetry.h" +#endif + #include "rx/rx.h" #include "rx/spektrum.h" @@ -95,7 +99,19 @@ bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcRe return false; } - serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); +#ifdef TELEMETRY + bool portShared = telemetryCheckRxPortShared(portConfig); +#else + bool portShared = false; +#endif + + serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED); + +#ifdef TELEMETRY + if (portShared) { + telemetrySharedPort = spektrumPort; + } +#endif return spektrumPort != NULL; } diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 5b8fe1e902..8391b8fa0d 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -29,6 +29,10 @@ #include "drivers/serial_uart.h" #include "io/serial.h" +#ifdef TELEMETRY +#include "telemetry/telemetry.h" +#endif + #include "rx/rx.h" #include "rx/sumd.h" @@ -63,7 +67,19 @@ bool sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return false; } - serialPort_t *sumdPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumdDataReceive, SUMD_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); +#ifdef TELEMETRY + bool portShared = telemetryCheckRxPortShared(portConfig); +#else + bool portShared = false; +#endif + + serialPort_t *sumdPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumdDataReceive, SUMD_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED); + +#ifdef TELEMETRY + if (portShared) { + telemetrySharedPort = sumdPort; + } +#endif return sumdPort != NULL; } diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 6950b5d90b..87a49fbb72 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -35,6 +35,10 @@ #include "drivers/serial_uart.h" #include "io/serial.h" +#ifdef TELEMETRY +#include "telemetry/telemetry.h" +#endif + #include "rx/rx.h" #include "rx/sumh.h" @@ -75,7 +79,19 @@ bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return false; } - sumhPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumhDataReceive, SUMH_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED); +#ifdef TELEMETRY + bool portShared = telemetryCheckRxPortShared(portConfig); +#else + bool portShared = false; +#endif + + sumhPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumhDataReceive, SUMH_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED); + +#ifdef TELEMETRY + if (portShared) { + telemetrySharedPort = sumhPort; + } +#endif return sumhPort != NULL; } diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index a9b8607618..ba70eb579b 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -27,6 +27,10 @@ #include "drivers/serial_uart.h" #include "io/serial.h" +#ifdef TELEMETRY +#include "telemetry/telemetry.h" +#endif + #include "rx/rx.h" #include "rx/xbus.h" @@ -123,7 +127,19 @@ bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return false; } - serialPort_t *xBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, xBusDataReceive, baudRate, MODE_RX, SERIAL_NOT_INVERTED); +#ifdef TELEMETRY + bool portShared = telemetryCheckRxPortShared(portConfig); +#else + bool portShared = false; +#endif + + serialPort_t *xBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, xBusDataReceive, baudRate, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED); + +#ifdef TELEMETRY + if (portShared) { + telemetrySharedPort = xBusPort; + } +#endif return xBusPort != NULL; } diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 9665436a3b..9e59d2709c 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -477,16 +477,23 @@ bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis) void checkFrSkyTelemetryState(void) { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing); + if (portConfig && telemetryCheckRxPortShared(portConfig)) { + if (!frskyTelemetryEnabled && telemetrySharedPort != NULL) { + frskyPort = telemetrySharedPort; + frskyTelemetryEnabled = true; + } + } else { + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing); - if (newTelemetryEnabledValue == frskyTelemetryEnabled) { - return; + if (newTelemetryEnabledValue == frskyTelemetryEnabled) { + return; + } + + if (newTelemetryEnabledValue) + configureFrSkyTelemetryPort(); + else + freeFrSkyTelemetryPort(); } - - if (newTelemetryEnabledValue) - configureFrSkyTelemetryPort(); - else - freeFrSkyTelemetryPort(); } void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index ad184b5aa6..bca5407492 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -299,12 +299,19 @@ void configureLtmTelemetryPort(void) void checkLtmTelemetryState(void) { - bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing); - if (newTelemetryEnabledValue == ltmEnabled) - return; - if (newTelemetryEnabledValue) - configureLtmTelemetryPort(); - else - freeLtmTelemetryPort(); + if (portConfig && telemetryCheckRxPortShared(portConfig)) { + if (!ltmEnabled && telemetrySharedPort != NULL) { + ltmPort = telemetrySharedPort; + ltmEnabled = true; + } + } else { + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing); + if (newTelemetryEnabledValue == ltmEnabled) + return; + if (newTelemetryEnabledValue) + configureLtmTelemetryPort(); + else + freeLtmTelemetryPort(); + } } #endif diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 4963d9df6e..f4f38ba947 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -74,6 +74,13 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) return enabled; } +bool telemetryCheckRxPortShared(serialPortConfig_t *portConfig) +{ + return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK; +} + +serialPort_t *telemetrySharedPort = NULL; + void telemetryCheckState(void) { checkFrSkyTelemetryState(); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index c53a15776c..8e692c4028 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -48,6 +48,9 @@ typedef struct telemetryConfig_s { uint8_t hottAlarmSoundInterval; } telemetryConfig_t; +bool telemetryCheckRxPortShared(serialPortConfig_t *portConfig); +extern serialPort_t *telemetrySharedPort; + void telemetryCheckState(void); void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); @@ -55,4 +58,6 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing); void telemetryUseConfig(telemetryConfig_t *telemetryConfig); +#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM) + #endif /* TELEMETRY_COMMON_H_ */