diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 07678fcacb..8f33a29e7c 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -290,7 +290,7 @@ bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) if ((portConfig->functionMask & FUNCTION_MSP) && (portConfig->functionMask & ALL_FUNCTIONS_SHARABLE_WITH_MSP)) { // MSP & telemetry #ifdef USE_TELEMETRY - } else if (telemetryCheckRxPortShared(portConfig)) { + } else if (telemetryCheckRxPortShared(portConfig, rxConfig()->serialrx_provider)) { // serial RX & telemetry #endif } else { diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index a447a628e0..f2e590198b 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -554,7 +554,7 @@ rx_spi_received_e frSkyXProcessFrame(uint8_t * const packet) remoteToProcessIndex = 0; telemetryRxBuffer[remoteToProcessId].needsProcessing = false; remoteProcessedId = remoteToProcessId; - remoteToProcessId = (remoteProcessedId + 1) % TELEMETRY_SEQUENCE_LENGTH; + remoteToProcessId = (remoteToProcessId + 1) % TELEMETRY_SEQUENCE_LENGTH; } } } diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 169ef497ca..80c6ef6ed5 100644 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -220,7 +220,6 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) } #ifdef USE_TELEMETRY - // bool portShared = telemetryCheckRxPortShared(portConfig); bool portShared = isSerialPortShared(portConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS); #else bool portShared = false; diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 6220788c28..eff0912c90 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -181,7 +181,7 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) } #ifdef USE_TELEMETRY - bool portShared = telemetryCheckRxPortShared(portConfig); + bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeConfig->serialrxProvider); #else bool portShared = false; #endif diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 903421f246..4405594b0a 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -245,7 +245,7 @@ void spektrumBind(rxConfig_t *rxConfig) switch (rxRuntimeConfig.serialrxProvider) { case SERIALRX_SRXL: #if defined(USE_TELEMETRY_SRXL) - if (featureIsEnabled(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) { + if (featureIsEnabled(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig, rxRuntimeConfig.serialrxProvider)) { bindPin = txPin; } break; @@ -352,7 +352,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig srxlEnabled = false; #if defined(USE_TELEMETRY_SRXL) - bool portShared = telemetryCheckRxPortShared(portConfig); + bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeConfig->serialrxProvider); #else bool portShared = false; #endif diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 717a4ba410..1795f2b025 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -170,7 +170,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) } #ifdef USE_TELEMETRY - bool portShared = telemetryCheckRxPortShared(portConfig); + bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeConfig->serialrxProvider); #else bool portShared = false; #endif diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 07af12aa20..b721e828f4 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -135,7 +135,7 @@ bool sumhInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) } #ifdef USE_TELEMETRY - bool portShared = telemetryCheckRxPortShared(portConfig); + bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeConfig->serialrxProvider); #else bool portShared = false; #endif diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index f6374e77c4..b02405c953 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -303,7 +303,7 @@ bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) } #ifdef USE_TELEMETRY - bool portShared = telemetryCheckRxPortShared(portConfig); + bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeConfig->serialrxProvider); #else bool portShared = false; #endif diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 5efc6af0f5..e9232731f6 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -494,7 +494,7 @@ static void configureFrSkyHubTelemetryPort(void) void checkFrSkyHubTelemetryState(void) { if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) { - if (telemetryCheckRxPortShared(portConfig)) { + if (telemetryCheckRxPortShared(portConfig, rxRuntimeConfig.serialrxProvider)) { if (frSkyHubPort == NULL && telemetrySharedPort != NULL) { frSkyHubPort = telemetrySharedPort; } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 24bf0ae34f..708d82c3cf 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -289,7 +289,7 @@ void configureLtmTelemetryPort(void) void checkLtmTelemetryState(void) { - if (portConfig && telemetryCheckRxPortShared(portConfig)) { + if (portConfig && telemetryCheckRxPortShared(portConfig, rxRuntimeConfig.serialrxProvider)) { if (!ltmEnabled && telemetrySharedPort != NULL) { ltmPort = telemetrySharedPort; ltmEnabled = true; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 66bfebf6d4..c5c8577b72 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -183,7 +183,7 @@ void configureMAVLinkTelemetryPort(void) void checkMAVLinkTelemetryState(void) { - if (portConfig && telemetryCheckRxPortShared(portConfig)) { + if (portConfig && telemetryCheckRxPortShared(portConfig, rxRuntimeConfig.serialrxProvider)) { if (!mavlinkTelemetryEnabled && telemetrySharedPort != NULL) { mavlinkPort = telemetrySharedPort; mavlinkTelemetryEnabled = true; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 5bfb0987ab..e5741e042e 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -263,7 +263,6 @@ smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPor checksum += c; checksum = (checksum & 0xFF) + (checksum >> 8); if (checksum == 0xFF) { - return (smartPortPayload_t *)&rxBuffer; } } diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d836c06178..bdb66bd1c1 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -133,24 +133,24 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing) return enabled; } -bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig) +bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const SerialRXType serialrxProvider) { if (portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK && - (rxRuntimeConfig.serialrxProvider == SERIALRX_SPEKTRUM1024 || - rxRuntimeConfig.serialrxProvider == SERIALRX_SPEKTRUM2048 || - rxRuntimeConfig.serialrxProvider == SERIALRX_SBUS || - rxRuntimeConfig.serialrxProvider == SERIALRX_SUMD || - rxRuntimeConfig.serialrxProvider == SERIALRX_SUMH || - rxRuntimeConfig.serialrxProvider == SERIALRX_XBUS_MODE_B || - rxRuntimeConfig.serialrxProvider == SERIALRX_XBUS_MODE_B_RJ01 || - rxRuntimeConfig.serialrxProvider == SERIALRX_IBUS)) { + (serialrxProvider == SERIALRX_SPEKTRUM1024 || + serialrxProvider == SERIALRX_SPEKTRUM2048 || + serialrxProvider == SERIALRX_SBUS || + serialrxProvider == SERIALRX_SUMD || + serialrxProvider == SERIALRX_SUMH || + serialrxProvider == SERIALRX_XBUS_MODE_B || + serialrxProvider == SERIALRX_XBUS_MODE_B_RJ01 || + serialrxProvider == SERIALRX_IBUS)) { return true; } #ifdef USE_TELEMETRY_IBUS - if ( portConfig->functionMask & FUNCTION_TELEMETRY_IBUS + if (portConfig->functionMask & FUNCTION_TELEMETRY_IBUS && portConfig->functionMask & FUNCTION_RX_SERIAL - && rxRuntimeConfig.serialrxProvider == SERIALRX_IBUS) { + && serialrxProvider == SERIALRX_IBUS) { // IBUS serial RX & telemetry return true; } diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 2d84aff70c..0d0b727136 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -27,8 +27,12 @@ #pragma once -#include "pg/pg.h" #include "io/serial.h" + +#include "pg/pg.h" + +#include "rx/rx.h" + #include "telemetry/ibus_shared.h" typedef enum { @@ -90,7 +94,7 @@ PG_DECLARE(telemetryConfig_t, telemetryConfig); extern serialPort_t *telemetrySharedPort; void telemetryInit(void); -bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig); +bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const SerialRXType serialrxProvider); void telemetryCheckState(void); void telemetryProcess(uint32_t currentTime); diff --git a/src/test/unit/io_serial_unittest.cc b/src/test/unit/io_serial_unittest.cc index 817272c2b4..3fc4ff7adf 100644 --- a/src/test/unit/io_serial_unittest.cc +++ b/src/test/unit/io_serial_unittest.cc @@ -29,7 +29,13 @@ extern "C" { #include "io/serial.h" + #include "pg/pg.h" + #include "pg/pg_ids.h" + #include "pg/rx.h" + void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); + + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); } #include "unittest_macros.h" diff --git a/src/test/unit/rx_sumd_unittest.cc b/src/test/unit/rx_sumd_unittest.cc index fa055789a5..9dd89f924a 100644 --- a/src/test/unit/rx_sumd_unittest.cc +++ b/src/test/unit/rx_sumd_unittest.cc @@ -46,10 +46,12 @@ extern "C" { } -bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig) +bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const SerialRXType serialrxProvider) { //TODO: implement - (void) portConfig; + UNUSED(portConfig); + UNUSED(serialrxProvider); + return false; } diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 5d4ed099cd..4682f9454f 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -329,7 +329,7 @@ bool isSerialTransmitBufferEmpty(const serialPort_t *) { return true; } serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;} bool telemetryDetermineEnabledState(portSharing_e) {return true;} -bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} +bool telemetryCheckRxPortShared(const serialPortConfig_t *, SerialRXType) {return true;} bool telemetryIsSensorEnabled(sensor_e) {return true;} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;}