PXX2 Rx side should be OK now

radio/src/telemetry/frsky.cpp

@@ -20,8 +20,16 @@
 
 #include "opentx.h"
 
-#if defined(PXX2)
-void processFrskyTelemetryData(uint8_t data)
+bool checkPXX2PacketCRC(const uint8_t * packet)
+{
+  // TODO ...
+
+  return true;
+  // TRACE("checkPXX2PacketCRC(): checksum error ");
+  // DUMP(packet, FRSKY_SPORT_PACKET_SIZE);
+}
+
+void processFrskyPXX2Data(uint8_t data)
 {
   static uint8_t dataState = STATE_DATA_IDLE;
 
@@ -41,8 +49,10 @@ void processFrskyTelemetryData(uint8_t data)
       }
       else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
         telemetryRxBuffer[telemetryRxBufferCount++] = data;
-        if (telemetryRxBuffer[0] + 2 == telemetryRxBufferCount) {
-          sportProcessPacket(telemetryRxBuffer + 2);
+        if (telemetryRxBuffer[0] + 3 /* 1 byte for length, 2 bytes for CRC */ == telemetryRxBufferCount) {
+          if (checkPXX2PacketCRC(telemetryRxBuffer)) {
+            sportProcessTelemetryPacketWithoutCrc(telemetryRxBuffer + 6 /* LEN, TYPE, RSSI/BAT, TP/SS/FW_T, FW_VER, Data ID */);
+          }
           telemetryRxBufferCount = 0;
           dataState = STATE_DATA_IDLE;
         }
@@ -55,7 +65,7 @@ void processFrskyTelemetryData(uint8_t data)
       break;
   }
 }
-#else
+
 void processFrskyTelemetryData(uint8_t data)
 {
   static uint8_t dataState = STATE_DATA_IDLE;
@@ -137,4 +147,4 @@ void processFrskyTelemetryData(uint8_t data)
   }
 #endif
 }
-#endif
+

radio/src/telemetry/frsky.h

@@ -202,7 +202,6 @@ enum FrSkyDataState {
 #define DATA_ID_SP2UH             0x45 // 5
 #define DATA_ID_SP2UR             0xC6 // 6
 
-
 #if defined(NO_RAS)
   #define IS_RAS_VALUE_VALID()            (false)
 #elif defined(PCBX10)
@@ -215,13 +214,6 @@ enum FrSkyDataState {
 
 #define IS_HIDDEN_TELEMETRY_VALUE(id)     ((id == SP2UART_A_ID) || (id == SP2UART_B_ID) || (id == XJT_VERSION_ID) || (id == RAS_ID) || (id == FACT_TEST_ID))
 
-enum AlarmLevel {
-  alarm_off = 0,
-  alarm_yellow = 1,
-  alarm_orange = 2,
-  alarm_red = 3
-};
-
 #define ALARM_GREATER(channel, alarm)     ((g_model.frsky.channels[channel].alarms_greater >> alarm) & 1)
 #define ALARM_LEVEL(channel, alarm)       ((g_model.frsky.channels[channel].alarms_level >> (2*alarm)) & 3)
 
@@ -283,11 +275,11 @@ typedef enum {
 
 // FrSky D Telemetry Protocol
 void processHubPacket(uint8_t id, int16_t value);
-void frskyDSendNextAlarm();
 void frskyDProcessPacket(const uint8_t *packet);
 
 // FrSky S.PORT Telemetry Protocol
 void sportProcessTelemetryPacket(const uint8_t * packet);
+void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet);
 
 void telemetryWakeup();
 void telemetryReset();
@@ -307,14 +299,6 @@ enum TelemetryProtocol
   TELEM_PROTO_FLYSKY_IBUS,
 };
 
-enum TelemAnas {
-  TELEM_ANA_A1,
-  TELEM_ANA_A2,
-  TELEM_ANA_A3,
-  TELEM_ANA_A4,
-  TELEM_ANA_COUNT
-};
-
 struct TelemetryData {
   TelemetryValueWithMin swr;          // TODO Min not needed
   TelemetryValueWithMin rssi;         // TODO Min not needed
@@ -324,12 +308,7 @@ struct TelemetryData {
 
 extern TelemetryData telemetryData;
 
-  typedef uint16_t frskyCellVoltage_t;
-
-void frskySetCellsCount(uint8_t cellscount);
-void frskySetCellVoltage(uint8_t battnumber, frskyCellVoltage_t cellVolts);
-void frskyUpdateCells();
-
 void processFrskyTelemetryData(uint8_t data);
+void processFrskyPXX2Data(uint8_t data);
 
 #endif // _FRSKY_H_

radio/src/telemetry/frsky_sport.cpp

@@ -92,7 +92,7 @@ const FrSkySportSensor * getFrSkySportSensor(uint16_t id, uint8_t subId=0)
   return result;
 }
 
-bool checkSportPacket(const uint8_t *packet)
+bool checkSportPacket(const uint8_t * packet)
 {
   short crc = 0;
   for (int i=1; i<FRSKY_SPORT_PACKET_SIZE; ++i) {
@@ -138,17 +138,22 @@ void sportProcessTelemetryPacket(uint16_t id, uint8_t subId, uint8_t instance, u
 
 void sportProcessTelemetryPacket(const uint8_t * packet)
 {
-  uint8_t physicalId = packet[0] & 0x1F;
-  uint8_t primId = packet[1];
-  uint16_t id = *((uint16_t *)(packet+2));
-  uint32_t data = SPORT_DATA_S32(packet);
-
   if (!checkSportPacket(packet)) {
     TRACE("sportProcessTelemetryPacket(): checksum error ");
     DUMP(packet, FRSKY_SPORT_PACKET_SIZE);
     return;
   }
 
+  sportProcessTelemetryPacketWithoutCrc(packet);
+}
+
+void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
+{
+  uint8_t physicalId = packet[0] & 0x1F;
+  uint8_t primId = packet[1];
+  uint16_t id = *((uint16_t *)(packet+2));
+  uint32_t data = SPORT_DATA_S32(packet);
+
   if (primId == DATA_FRAME) {
     uint8_t instance = physicalId + 1;
     if (id == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {

radio/src/telemetry/telemetry.cpp

@@ -46,6 +46,7 @@ void processTelemetryData(uint8_t data)
     return;
   }
 #endif
+
 #if defined(MULTIMODULE)
   if (telemetryProtocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
     processSpektrumTelemetryData(data);
@@ -60,6 +61,11 @@ void processTelemetryData(uint8_t data)
     return;
   }
 #endif
+
+  if (telemetryProtocol == PROTOCOL_TELEMETRY_PXX2) {
+    processFrskyPXX2Data(data);
+  }
+
   processFrskyTelemetryData(data);
 }