S.PORT Push reworking

radio/sdcard/FrSky-utilities/FrSky SBEC.lua

@@ -55,9 +55,9 @@ end
 
 local settingsFields = {
   {"SBEC OUTPUT (V)", VALUE, 0x80, nil, 50, 84 },
-  --{"Physical ID", VALUE, 0x01, nil, 0, 26 },
+  {"Physical ID", VALUE, 0x01, nil, 0, 26 },
-  --{"Application IDgroup", VALUE, 0x0D, nil, 0, 15 },
+  {"Application IDgroup", VALUE, 0x0D, nil, 0, 15 },
-  --{"Data rate(*100ms)", VALUE, 0x22, nil, 1, 255 },
+  {"Data rate(*100ms)", VALUE, 0x22, nil, 1, 255 },
 }
 
 -- Change display attribute to current field
@@ -156,19 +156,19 @@ local function redrawFieldsPage()
 end
 
 local function telemetryRead(fieldx)
-  return sportTelemetryPush(0x08, 0x30, 0x0e50, fieldx)
+  return sportTelemetryPush(0, 0x30, 0x0e50, fieldx)
 end
 
 local function telemetryIdle(field)
-  return sportTelemetryPush(0x08,0x21,0x0e50,field)
+  return sportTelemetryPush(0, 0x21, 0x0e50, field)
 end
 
 local function telemetryUnIdle(field)
-  return sportTelemetryPush(0x08,0x20,0x0e50,field)
+  return sportTelemetryPush(0, 0x20, 0x0e50, field)
 end
 
 local function telemetryWrite(fieldx, valuex)
-  return sportTelemetryPush(0x08, 0x31, 0x0e50, fieldx + valuex*256)
+  return sportTelemetryPush(0, 0x31, 0x0e50, fieldx + valuex*256)
 end
 
 local telemetryPopTimeout = 0

radio/src/io/frsky_sport.cpp

@@ -20,12 +20,9 @@
 
 #include "opentx.h"
 
-bool isTelemetryOutputBufferAvailable()
+#warning "This need to be adapted"
-{
-  return (outputTelemetryBuffer.size == 0 && outputTelemetryBuffer.trigger == 0x7E);
-}
 
-void sportOutputPushByte(uint8_t byte)
+/*void sportOutputPushByte(uint8_t byte)
 {
   if (byte == 0x7E || byte == 0x7D) {
     outputTelemetryBuffer.push(0x7D);
@@ -52,3 +49,4 @@ void sportOutputPushPacket(SportTelemetryPacket * packet)
   outputTelemetryBuffer.setTrigger(packet->raw[0]); // physicalId
   outputTelemetryBuffer.setDestination(SPORT_MODULE);
 }
+*/

radio/src/io/frsky_sport.h

@@ -34,9 +34,6 @@ PACK(union SportTelemetryPacket
   };
   uint8_t raw[8];
 });
-
-bool isTelemetryOutputBufferAvailable();
-void sportOutputPushPacket(SportTelemetryPacket * packet);
 #endif
 
 #if defined(STM32)

radio/src/lua/api_general.cpp

@@ -422,15 +422,14 @@ When called without parameters, it will only return the status of the output buf
 static int luaSportTelemetryPush(lua_State * L)
 {
   if (lua_gettop(L) == 0) {
-    lua_pushboolean(L, isTelemetryOutputBufferAvailable());
+    lua_pushboolean(L, outputTelemetryBuffer.isAvailable());
   }
-  else if (isTelemetryOutputBufferAvailable()) {
+  else if (outputTelemetryBuffer.isAvailable()) {
-    SportTelemetryPacket packet;
+    outputTelemetryBuffer.sport.physicalId = getDataId(luaL_checkunsigned(L, 1)); // ignored
-    packet.physicalId = getDataId(luaL_checkunsigned(L, 1));
+    outputTelemetryBuffer.sport.primId = luaL_checkunsigned(L, 2);
-    packet.primId = luaL_checkunsigned(L, 2);
+    outputTelemetryBuffer.sport.dataId = luaL_checkunsigned(L, 3);
-    packet.dataId = luaL_checkunsigned(L, 3);
+    outputTelemetryBuffer.sport.value = luaL_checkunsigned(L, 4);
-    packet.value = luaL_checkunsigned(L, 4);
+    outputTelemetryBuffer.setDestination(TELEMETRY_ENDPOINT_ANY);
-    sportOutputPushPacket(&packet);
     lua_pushboolean(L, true);
   }
   else {
@@ -439,62 +438,6 @@ static int luaSportTelemetryPush(lua_State * L)
   return 1;
 }
 
-#if defined(PXX2)
-/*luadoc
-@function pxx2TelemetryPush()
-
-This functions allows for sending SPORT telemetry data toward the receiver,
-and more generally, to anything connected SPORT bus on the receiver or transmitter.
-
-When called without parameters, it will only return the status of the output buffer without sending anything.
-
-@param module    module index
-
-@param receiver  receiver index
-
-@param sensorId  physical sensor ID
-
-@param frameId   frame ID
-
-@param dataId    data ID
-
-@param value     value
-
-@retval boolean  data queued in output buffer or not.
-
-@status current Introduced in 2.3.0
-*/
-static int luaPXX2TelemetryPush(lua_State * L)
-{
-  if (lua_gettop(L) == 0) {
-    lua_pushboolean(L, isTelemetryOutputBufferAvailable());
-  }
-  else if (isTelemetryOutputBufferAvailable()) {
-    SportTelemetryPacket packet;
-    uint8_t module = luaL_checkunsigned(L, 1);
-    uint8_t receiver = luaL_checkunsigned(L, 2);
-    uint8_t rx_uid = g_model.moduleData[module].pxx2.getReceiverSlot(receiver);
-    if (rx_uid > 0) {
-      // TODO add more controls (module started, module = PXX2, etc.)
-      packet.physicalId = getDataId(luaL_checkunsigned(L, 3));
-      packet.primId = luaL_checkunsigned(L, 4);
-      packet.dataId = luaL_checkunsigned(L, 5);
-      packet.value = luaL_checkunsigned(L, 6);
-      // TODO we could avoid this new copy
-      pushPXX2TelemetryPacket(module, rx_uid - 1, &packet);
-      lua_pushboolean(L, true);
-    }
-    else {
-      lua_pushboolean(L, false);
-    }
-  }
-  else {
-    lua_pushboolean(L, false);
-  }
-  return 1;
-}
-#endif
-
 #if defined(CROSSFIRE)
 /*luadoc
 @function crossfireTelemetryPop()
@@ -1366,10 +1309,6 @@ const luaL_Reg opentxLib[] = {
   { "sportTelemetryPop", luaSportTelemetryPop },
   { "sportTelemetryPush", luaSportTelemetryPush },
   { "setTelemetryValue", luaSetTelemetryValue },
-#if defined(PXX2)
-  { "sportTelemetryPop", luaSportTelemetryPop },
-  { "pxx2TelemetryPush", luaPXX2TelemetryPush },
-#endif
 #if defined(CROSSFIRE)
   { "crossfireTelemetryPop", luaCrossfireTelemetryPop },
   { "crossfireTelemetryPush", luaCrossfireTelemetryPush },

radio/src/pulses/pxx2.cpp

@@ -122,28 +122,28 @@ void Pxx2Pulses::setupChannelsFrame(uint8_t module)
 void Pxx2Pulses::setupTelemetryFrame(uint8_t module)
 {
   addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_TELEMETRY);
-  for (uint8_t i = 0; i < outputTelemetryBuffer.size; i++) {
+  Pxx2Transport::addByte(outputTelemetryBuffer.destination.rxUid);
+  for (uint8_t i = 0; i < sizeof(SportTelemetryPacket); i++) {
     Pxx2Transport::addByte(outputTelemetryBuffer.data[i]);
   }
-  outputTelemetryBuffer.reset();
 }
 
 void Pxx2Pulses::setupHardwareInfoFrame(uint8_t module)
 {
-  if (reusableBuffer.hardwareAndSettings.modules[module].current <= reusableBuffer.hardwareAndSettings.modules[module].maximum) {
   if (reusableBuffer.hardwareAndSettings.modules[module].timeout == 0) {
+    if (reusableBuffer.hardwareAndSettings.modules[module].current <= reusableBuffer.hardwareAndSettings.modules[module].maximum) {
       addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_HW_INFO);
       Pxx2Transport::addByte(reusableBuffer.hardwareAndSettings.modules[module].current);
       reusableBuffer.hardwareAndSettings.modules[module].timeout = 20;
       reusableBuffer.hardwareAndSettings.modules[module].current++;
     }
     else {
-      reusableBuffer.hardwareAndSettings.modules[module].timeout--;
+      moduleSettings[module].mode = MODULE_MODE_NORMAL;
       setupChannelsFrame(module);
     }
   }
   else {
-    moduleSettings[module].mode = MODULE_MODE_NORMAL;
+    reusableBuffer.hardwareAndSettings.modules[module].timeout--;
     setupChannelsFrame(module);
   }
 }
@@ -319,8 +319,9 @@ void Pxx2Pulses::setupFrame(uint8_t module)
       setupShareMode(module);
       break;
     default:
-      if (outputTelemetryBuffer.size > 0 && outputTelemetryBuffer.destination == module) {
+      if (outputTelemetryBuffer.destination.module == module) {
         setupTelemetryFrame(module);
+        outputTelemetryBuffer.reset();
       }
       else {
         setupChannelsFrame(module);

radio/src/targets/taranis/telemetry_driver.cpp

@@ -158,8 +158,9 @@ extern "C" void TELEMETRY_USART_IRQHandler(void)
 #if defined(LUA)
       if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT) {
         static uint8_t prevdata;
-        if (prevdata == 0x7E && outputTelemetryBuffer.size > 0 && data == outputTelemetryBuffer.trigger && outputTelemetryBuffer.destination == SPORT_MODULE) {
+        if (prevdata == 0x7E && outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_SPORT && data == outputTelemetryBuffer.sport.physicalId) {
-          sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size);
+          #warning "bytestuffing + buffer send"
+          // sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size);
         }
         prevdata = data;
       }

radio/src/telemetry/frsky.cpp

@@ -20,76 +20,6 @@
 
 #include "opentx.h"
 
-bool checkPXX2PacketCRC(const uint8_t * packet)
-{
-  // TODO ...
-
-  return true;
-  // TRACE("checkPXX2PacketCRC(): checksum error ");
-  // DUMP(packet, FRSKY_SPORT_PACKET_SIZE);
-}
-
-void createFrSkyPXX2Sensor(uint16_t type, uint8_t value){
-  uint8_t buffer[5] = {0x18, 0x10, 0, 0, 0};
-  buffer[2] = type & 0xF;
-  buffer[3] = type >> 8;
-  buffer[4] = value;
-  sportProcessTelemetryPacketWithoutCrc(buffer);
-}
-
-void processFrskyPXX2Data(uint8_t data)
-{
-  static uint8_t dataState = STATE_DATA_IDLE;
-
-  switch (dataState) {
-    case STATE_DATA_IDLE:
-      if (data == START_STOP) {
-        telemetryRxBufferCount = 0;
-        dataState = STATE_DATA_START;
-      }
-      break;
-
-    case STATE_DATA_START:
-      if (telemetryRxBufferCount == 0 && data > 0x0C) {
-        // wrong length
-        telemetryRxBufferCount = 0;
-        dataState = STATE_DATA_IDLE;
-      }
-      else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
-        telemetryRxBuffer[telemetryRxBufferCount++] = data;
-        if (telemetryRxBuffer[0] + 3 /* 1 byte for length, 2 bytes for CRC */ == telemetryRxBufferCount) {
-          if (checkPXX2PacketCRC(telemetryRxBuffer)) {
-            R9ModuleStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
-            uint8_t R9Region = (telemetryRxBuffer[3] & 0x30) >> 4;
-            if (R9Region != 0x04) {
-              g_model.moduleData[EXTERNAL_MODULE].r9m.region = R9Region;
-            }
-            if (telemetryRxBuffer[2] & 0x80) {
-              createFrSkyPXX2Sensor(RSSI_ID, telemetryRxBuffer[2] & 0x7f);
-            }
-            else {
-              createFrSkyPXX2Sensor(BATT_ID, telemetryRxBuffer[2] & 0x7f);
-            }
-            if((telemetryRxBuffer[3] & 0x04) == 0) {
-              createFrSkyPXX2Sensor(R9_PWR_ID, (telemetryRxBuffer[3] & 0x03));
-            }
-            if (telemetryRxBuffer[0] == 0x0C) {
-              sportProcessTelemetryPacketWithoutCrc(telemetryRxBuffer + 5 /* LEN, TYPE, RSSI/BAT, TP/SS/FW_T, FW_VER, Data ID */);
-            }
-          }
-          telemetryRxBufferCount = 0;
-          dataState = STATE_DATA_IDLE;
-        }
-      }
-      else {
-        // overflow guard
-        telemetryRxBufferCount = 0;
-        dataState = STATE_DATA_IDLE;
-      }
-      break;
-  }
-}
-
 void processFrskyTelemetryData(uint8_t data)
 {
 #if defined(PCBSKY9X) && defined(BLUETOOTH)

radio/src/telemetry/frsky.h

@@ -284,7 +284,7 @@ void frskyDProcessPacket(const uint8_t *packet);
 
 // FrSky S.PORT Telemetry Protocol
 void sportProcessTelemetryPacket(const uint8_t * packet);
-void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet);
+void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet);
 
 void telemetryWakeup();
 void telemetryReset();
@@ -315,6 +315,5 @@ extern TelemetryData telemetryData;
 
 bool pushFrskyTelemetryData(uint8_t data); // returns true when end of frame detected
 void processFrskyTelemetryData(uint8_t data);
-void processFrskyPXX2Data(uint8_t data);
 
 #endif // _FRSKY_H_

radio/src/telemetry/frsky_pxx2.cpp

@@ -166,7 +166,10 @@ void processBindFrame(uint8_t module, uint8_t * frame)
 
 void processTelemetryFrame(uint8_t module, uint8_t * frame)
 {
-  sportProcessTelemetryPacketWithoutCrc(&frame[4]);
+  TelemetryEndpoint origin;
+  origin.module = module;
+  origin.rxUid = frame[3] & 0x0F;
+  sportProcessTelemetryPacketWithoutCrc(origin.value, &frame[4]);
 }
 
 void processSpectrumAnalyserFrame(uint8_t module, uint8_t * frame)
@@ -260,16 +263,3 @@ void processPXX2Frame(uint8_t module, uint8_t *frame)
       break;
   }
 }
-
-void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet)
-{
-  // Flag0
-  outputTelemetryBuffer.push(rx_uid);
-
-  for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
-    uint8_t byte = packet->raw[i];
-    outputTelemetryBuffer.push(byte);
-  }
-
-  outputTelemetryBuffer.setDestination(module);
-}

radio/src/telemetry/frsky_sport.cpp

@@ -147,19 +147,23 @@ void sportProcessTelemetryPacket(const uint8_t * packet)
     return;
   }
 
-  sportProcessTelemetryPacketWithoutCrc(packet);
+  sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet);
 }
 
-void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
+void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet)
 {
   uint8_t physicalId = packet[0] & 0x1F;
   uint8_t primId = packet[1];
-  uint16_t id = *((uint16_t *)(packet+2));
+  uint16_t dataId = *((uint16_t *)(packet+2));
   uint32_t data = SPORT_DATA_S32(packet);
 
   if (primId == DATA_FRAME) {
+    if (outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_ANY && dataId == outputTelemetryBuffer.sport.dataId) {
+      outputTelemetryBuffer.sport.physicalId = packet[0];
+      outputTelemetryBuffer.destination.value = origin;
+    }
     uint8_t instance = physicalId + 1;
-    if (id == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
+    if (dataId == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
       telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
       data = SPORT_DATA_U8(packet);
       if (data == 0)
@@ -167,17 +171,17 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
       else
         telemetryData.rssi.set(data);
     }
-    else if (id == R9_PWR_ID) {
+    else if (dataId == R9_PWR_ID) {
       uint32_t r9pwr[] = {100, 200, 500, 1000};
       data = r9pwr[SPORT_DATA_U8(packet) & 0x03];
     }
-    else if (id == XJT_VERSION_ID) {
+    else if (dataId == XJT_VERSION_ID) {
       telemetryData.xjtVersion = HUB_DATA_U16(packet);
       if (!IS_RAS_VALUE_VALID()) {
         telemetryData.swr.set(0x00);
       }
     }
-    else if (id == RAS_ID) {
+    else if (dataId == RAS_ID) {
       if (IS_RAS_VALUE_VALID())
         telemetryData.swr.set(SPORT_DATA_U8(packet));
       else
@@ -185,33 +189,33 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
     }
 
     if (TELEMETRY_STREAMING()/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) {
-      if ((id >> 8) == 0) {
+      if ((dataId >> 8) == 0) {
         // The old FrSky IDs
-        processHubPacket(id, HUB_DATA_U16(packet));
+        processHubPacket(dataId, HUB_DATA_U16(packet));
       }
-      else if (!IS_HIDDEN_TELEMETRY_VALUE(id)) {
+      else if (!IS_HIDDEN_TELEMETRY_VALUE(dataId)) {
-        if (id == ADC1_ID || id == ADC2_ID || id == BATT_ID || id == RAS_ID) {
+        if (dataId == ADC1_ID || dataId == ADC2_ID || dataId == BATT_ID || dataId == RAS_ID) {
           data = SPORT_DATA_U8(packet);
         }
-        if (id >= GPS_LONG_LATI_FIRST_ID && id <= GPS_LONG_LATI_LAST_ID) {
+        if (dataId >= GPS_LONG_LATI_FIRST_ID && dataId <= GPS_LONG_LATI_LAST_ID) {
           int32_t value = (data & 0x3fffffff);
           if (data & (1 << 30))
             value = -value;
           value = (value * 5) / 3; // min/10000 => deg/1000000
           if (data & (1 << 31))
-            sportProcessTelemetryPacket(id, 0, instance, value, UNIT_GPS_LONGITUDE);
+            sportProcessTelemetryPacket(dataId, 0, instance, value, UNIT_GPS_LONGITUDE);
           else
-            sportProcessTelemetryPacket(id, 0, instance, value, UNIT_GPS_LATITUDE);
+            sportProcessTelemetryPacket(dataId, 0, instance, value, UNIT_GPS_LATITUDE);
         }
-        else if (id >= RBOX_BATT1_FIRST_ID && id <= RBOX_BATT2_LAST_ID) {
+        else if (dataId >= RBOX_BATT1_FIRST_ID && dataId <= RBOX_BATT2_LAST_ID) {
-          sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
+          sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
-          sportProcessTelemetryPacket(id, 1, instance, data >> 16);
+          sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
         }
-        else if (id >= RBOX_CNSP_FIRST_ID && id <= RBOX_CNSP_LAST_ID) {
+        else if (dataId >= RBOX_CNSP_FIRST_ID && dataId <= RBOX_CNSP_LAST_ID) {
-          sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
+          sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
-          sportProcessTelemetryPacket(id, 1, instance, data >> 16);
+          sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
         }
-        else if (id >= RBOX_STATE_FIRST_ID && id <= RBOX_STATE_LAST_ID) {
+        else if (dataId >= RBOX_STATE_FIRST_ID && dataId <= RBOX_STATE_LAST_ID) {
           bool static isRB10 = false;
           uint16_t newServosState;
 
@@ -233,31 +237,31 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
           }
           servosState = newServosState;
           rboxState = newRboxState;
-          sportProcessTelemetryPacket(id, 0, instance, servosState);
+          sportProcessTelemetryPacket(dataId, 0, instance, servosState);
-          sportProcessTelemetryPacket(id, 1, instance, rboxState);
+          sportProcessTelemetryPacket(dataId, 1, instance, rboxState);
         }
-        else if (id >= ESC_POWER_FIRST_ID && id <= ESC_POWER_LAST_ID) {
+        else if (dataId >= ESC_POWER_FIRST_ID && dataId <= ESC_POWER_LAST_ID) {
-          sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
+          sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
-          sportProcessTelemetryPacket(id, 1, instance, data >> 16);
+          sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
         }
-        else if (id >= ESC_RPM_CONS_FIRST_ID && id <= ESC_RPM_CONS_LAST_ID) {
+        else if (dataId >= ESC_RPM_CONS_FIRST_ID && dataId <= ESC_RPM_CONS_LAST_ID) {
-          sportProcessTelemetryPacket(id, 0, instance, 100 * (data & 0xffff));
+          sportProcessTelemetryPacket(dataId, 0, instance, 100 * (data & 0xffff));
-          sportProcessTelemetryPacket(id, 1, instance, data >> 16);
+          sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
         }
-        else if (id >= ESC_TEMPERATURE_FIRST_ID && id <= ESC_TEMPERATURE_LAST_ID) {
+        else if (dataId >= ESC_TEMPERATURE_FIRST_ID && dataId <= ESC_TEMPERATURE_LAST_ID) {
-          sportProcessTelemetryPacket(id, 0, instance, data & 0x00ff);
+          sportProcessTelemetryPacket(dataId, 0, instance, data & 0x00ff);
         }
-        else if (id >= SBEC_POWER_FIRST_ID && id <= SBEC_POWER_LAST_ID) {
+        else if (dataId >= SBEC_POWER_FIRST_ID && dataId <= SBEC_POWER_LAST_ID) {
-          sportProcessTelemetryPacket(id, 0, instance, (data & 0xffff) / 10);
+          sportProcessTelemetryPacket(dataId, 0, instance, (data & 0xffff) / 10);
-          sportProcessTelemetryPacket(id, 1, instance, (data >> 16) / 10);
+          sportProcessTelemetryPacket(dataId, 1, instance, (data >> 16) / 10);
         }
-        else if (id >= DIY_STREAM_FIRST_ID && id <= DIY_STREAM_LAST_ID) {
+        else if (dataId >= DIY_STREAM_FIRST_ID && dataId <= DIY_STREAM_LAST_ID) {
 #if defined(LUA)
           if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) {
             SportTelemetryPacket luaPacket;
             luaPacket.physicalId = physicalId;
             luaPacket.primId = primId;
-            luaPacket.dataId = id;
+            luaPacket.dataId = dataId;
             luaPacket.value = data;
             for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
               luaInputTelemetryFifo->push(luaPacket.raw[i]);
@@ -266,7 +270,7 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
 #endif
         }
         else {
-          sportProcessTelemetryPacket(id, 0, instance, data);
+          sportProcessTelemetryPacket(dataId, 0, instance, data);
         }
       }
     }
@@ -277,7 +281,7 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
       SportTelemetryPacket luaPacket;
       luaPacket.physicalId = physicalId;
       luaPacket.primId = primId;
-      luaPacket.dataId = id;
+      luaPacket.dataId = dataId;
       luaPacket.value = data;
       for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
         luaInputTelemetryFifo->push(luaPacket.raw[i]);

radio/src/telemetry/telemetry.cpp

@@ -63,11 +63,6 @@ void processTelemetryData(uint8_t data)
   }
 #endif
 
-  if (telemetryProtocol == PROTOCOL_TELEMETRY_PXX2) {
-    processFrskyPXX2Data(data);
-    return;
-  }
-
   processFrskyTelemetryData(data);
 }
 

radio/src/telemetry/telemetry.h

@@ -155,7 +155,17 @@ void logTelemetryWriteByte(uint8_t data);
 #define LOG_TELEMETRY_WRITE_BYTE(data)
 #endif
 
-#define TELEMETRY_OUTPUT_FIFO_SIZE 20
+#define TELEMETRY_ENDPOINT_ANY     0xFF
+#define TELEMETRY_ENDPOINT_NONE    0xFE
+#define TELEMETRY_ENDPOINT_SPORT   0xFD
+
+union TelemetryEndpoint {
+  PACK(struct {
+    uint8_t module:4;
+    uint8_t rxUid:4;
+  });
+  uint8_t value;
+};
 
 class OutputTelemetryBuffer {
   public:
@@ -163,29 +173,24 @@ class OutputTelemetryBuffer {
       reset();
     }
 
-    void push(uint8_t byte) {
+    void setDestination(uint8_t value) {
-      data[size++] = byte;
+      destination.value = value;
-    }
-
-    void setTrigger(uint8_t byte) {
-      trigger = byte;
-    }
-
-    void setDestination(uint8_t module) {
-      destination = module;
     }
 
     void reset() {
-      size = 0;
+      destination.value = TELEMETRY_ENDPOINT_NONE;
-      trigger = 0x7E;
+    }
-      destination = 0xFF;
+
+    bool isAvailable() {
+      return destination.value == TELEMETRY_ENDPOINT_NONE;
     }
 
   public:
-    uint8_t data[TELEMETRY_OUTPUT_FIFO_SIZE];
+    union {
-    uint8_t size;
+      SportTelemetryPacket sport;
-    uint8_t trigger;
+      uint8_t data[16];
-    uint8_t destination;
+    };
+    TelemetryEndpoint destination;
 };
 
 extern OutputTelemetryBuffer outputTelemetryBuffer __DMA;
@@ -202,6 +207,5 @@ extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
 #endif
 
 void processPXX2Frame(uint8_t module, uint8_t *frame);
-void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet);
 
 #endif // _TELEMETRY_H_