multi: Fix TQly & TRSS sensors not being discovered (#1480)

move the TQly & TRSS hack where it belongs

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();

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);

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;

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)