diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index ea8eb92ee..14635f868 100644 --- a/radio/src/telemetry/frsky.h +++ b/radio/src/telemetry/frsky.h @@ -278,8 +278,10 @@ void processHubPacket(uint8_t id, int16_t value); void frskyDProcessPacket(const uint8_t *packet); // FrSky S.PORT Telemetry Protocol -void sportProcessTelemetryPacket(const uint8_t * packet); -void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet); +bool sportProcessTelemetryPacket(const uint8_t * packet); +void sportProcessTelemetryPacket(uint16_t id, uint8_t subId, uint8_t instance, + uint32_t data, TelemetryUnit unit = UNIT_RAW); +void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t *packet); void telemetryWakeup(); void telemetryReset(); diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index 407d056ef..a4d4f2e89 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -132,7 +132,8 @@ bool checkSportPacket(const uint8_t * packet) uint16_t servosState; uint16_t rboxState; -void sportProcessTelemetryPacket(uint16_t id, uint8_t subId, uint8_t instance, uint32_t data, TelemetryUnit unit=UNIT_RAW) +void sportProcessTelemetryPacket(uint16_t id, uint8_t subId, uint8_t instance, + uint32_t data, TelemetryUnit unit) { const FrSkySportSensor * sensor = getFrSkySportSensor(id, subId); uint8_t precision = 255; @@ -158,15 +159,16 @@ void sportProcessTelemetryPacket(uint16_t id, uint8_t subId, uint8_t instance, u } } -void sportProcessTelemetryPacket(const uint8_t * packet) +bool sportProcessTelemetryPacket(const uint8_t * packet) { if (!checkSportPacket(packet)) { TRACE("sportProcessTelemetryPacket(): checksum error "); DUMP(packet, FRSKY_SPORT_PACKET_SIZE); - return; + return false; } sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet); + return true; } void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet) @@ -222,12 +224,6 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe else { telemetryData.rssi.set(data); } -#if defined(MULTIMODULE) - if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) { - sportProcessTelemetryPacket(TX_RSSI_ID, 0, instance, packet[5] >> 1u, UNIT_DB); - sportProcessTelemetryPacket(TX_LQI_ID, 0, instance, packet[7], UNIT_RAW); - } -#endif } else if (dataId == VALID_FRAME_RATE_ID) { data = 100 - SPORT_DATA_U8(packet); diff --git a/radio/src/telemetry/multi.cpp b/radio/src/telemetry/multi.cpp index d710b3d4f..b0bfecc23 100644 --- a/radio/src/telemetry/multi.cpp +++ b/radio/src/telemetry/multi.cpp @@ -436,8 +436,19 @@ static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module) break; case FrSkySportTelemetry: - if (len >= 4) - sportProcessTelemetryPacket(data); + if (len >= 4) { + if (sportProcessTelemetryPacket(data) && len >= 8) { + uint8_t primId = data[1]; + uint16_t dataId = *((uint16_t *)(data+2)); + if (primId == DATA_FRAME && dataId == RSSI_ID) { + // fetch MPM special TX_RSSI & TX_LQI sensors + uint8_t physicalId = data[0] & 0x1F; + uint8_t instance = physicalId + (TELEMETRY_ENDPOINT_SPORT << 5); + sportProcessTelemetryPacket(TX_RSSI_ID, 0, instance, data[5] >> 1u, UNIT_DB); + sportProcessTelemetryPacket(TX_LQI_ID, 0, instance, data[7], UNIT_RAW); + } + } + } else TRACE("[MP] Received sport telemetry len %d < 4", len); break; diff --git a/radio/src/tests/frsky.cpp b/radio/src/tests/frsky.cpp index 566246fbe..978e47455 100644 --- a/radio/src/tests/frsky.cpp +++ b/radio/src/tests/frsky.cpp @@ -23,7 +23,6 @@ void frskyDProcessPacket(const uint8_t *packet); bool checkSportPacket(const uint8_t *packet); -void sportProcessTelemetryPacket(const uint8_t * packet); bool checkSportPacket(const uint8_t *packet); TEST(FrSky, TelemetryValueWithMinAveraging)