S.PORT Push now working on internal module

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

@@ -244,7 +244,6 @@ local function runFieldsPage(event)
   return 0
 end
 
-
 local function runSettingsPage(event)
   fields = settingsFields
   return runFieldsPage(event)

radio/src/datastructs.h

@@ -350,8 +350,7 @@ PACK(struct TelemetrySensor {
   union {
     PACK(struct {
       uint8_t physID:5;            // instance ID to allow handling multiple instances of same value type, for FrSky can be the physical ID of the sensor
-      uint8_t module:1;
+      uint8_t rxIndex:3;
-      uint8_t rx_uid:2;
     }) frskyInstance;
     uint8_t instance;
     NOBACKUP(uint8_t formula);

radio/src/lua/api_general.cpp

@@ -419,22 +419,61 @@ When called without parameters, it will only return the status of the output buf
 
 @status current Introduced in 2.2.0
 */
+
+TelemetryEndpoint getTelemetryEndpoint(uint8_t receiverIndex)
+{
+  TelemetryEndpoint result;
+
+  for (uint8_t module=0; module<NUM_MODULES; module++) {
+    for (uint8_t receiver=0; receiver<PXX2_MAX_RECEIVERS_PER_MODULE; receiver++) {
+      uint8_t candidateIndex = g_model.moduleData[module].pxx2.getReceiverSlot(receiver) - 1;
+      if (candidateIndex == receiverIndex) {
+        result.module = module;
+        result.rxUid = candidateIndex;
+        return result;
+      }
+    }
+  }
+
+  result.value = TELEMETRY_ENDPOINT_NONE;
+  return result;
+}
+
 static int luaSportTelemetryPush(lua_State * L)
 {
   if (lua_gettop(L) == 0) {
     lua_pushboolean(L, outputTelemetryBuffer.isAvailable());
+    return 1;
   }
-  else if (outputTelemetryBuffer.isAvailable()) {
+
-    outputTelemetryBuffer.sport.physicalId = getDataId(luaL_checkunsigned(L, 1)); // ignored
+  if (outputTelemetryBuffer.isAvailable()) {
+    for (uint8_t i=0; i<MAX_TELEMETRY_SENSORS; i++) {
+      TelemetrySensor & sensor = g_model.telemetrySensors[i];
+      if (sensor.id == outputTelemetryBuffer.sport.dataId) {
+        if (sensor.frskyInstance.rxIndex == TELEMETRY_ENDPOINT_SPORT) {
+          SportTelemetryPacket packet;
+          packet.physicalId = getDataId(luaL_checkunsigned(L, 1));
+          packet.primId = luaL_checkunsigned(L, 2);
+          packet.dataId = luaL_checkunsigned(L, 3);
+          packet.value = luaL_checkunsigned(L, 4);
+          outputTelemetryBuffer.pushSportPacketWithBytestuffing(packet);
+          outputTelemetryBuffer.setDestination(TELEMETRY_ENDPOINT_SPORT);
+        }
+        else {
+          outputTelemetryBuffer.sport.physicalId = getDataId(luaL_checkunsigned(L, 1));
           outputTelemetryBuffer.sport.primId = luaL_checkunsigned(L, 2);
           outputTelemetryBuffer.sport.dataId = luaL_checkunsigned(L, 3);
           outputTelemetryBuffer.sport.value = luaL_checkunsigned(L, 4);
-    outputTelemetryBuffer.setDestination(TELEMETRY_ENDPOINT_ANY);
+          TelemetryEndpoint destination = getTelemetryEndpoint(sensor.frskyInstance.rxIndex);
+          outputTelemetryBuffer.setDestination(destination.value);
+        }
         lua_pushboolean(L, true);
+        return 1;
       }
-  else {
+    }
+  }
+
   lua_pushboolean(L, false);
-  }
   return 1;
 }
 

radio/src/telemetry/frsky_sport.cpp

@@ -150,42 +150,16 @@ void sportProcessTelemetryPacket(const uint8_t * packet)
   sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet);
 }
 
-void sportOutputByteStuffAndCrc()
+void sportProcessTelemetryPacketWithoutCrc(uint8_t originValue, const uint8_t * packet)
-{
-  uint16_t crc = 0;
-
-  uint8_t count = sizeof(SportTelemetryPacket);
-  for (uint8_t i = 1/*no bytestuffing for the physicalID*/; i < count; i++) {
-    uint8_t byte = outputTelemetryBuffer.data[i];
-    if (byte == 0x7E || byte == 0x7D) {
-      memmove(&outputTelemetryBuffer.data[i + 2], &outputTelemetryBuffer.data[i + 1], sizeof(SportTelemetryPacket) - i - 1);
-      outputTelemetryBuffer.data[i] = 0x7D;
-      outputTelemetryBuffer.data[i + 1] = 0x20 ^ byte;
-      i += 1, count += 1;
-    }
-    crc += byte; // 0-1FF
-    crc += crc >> 8; // 0-100
-    crc &= 0x00ff;
-  }
-  outputTelemetryBuffer.data[count] = 0xFF - crc;
-  outputTelemetryBuffer.size = count + 1;
-}
-
-void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet)
 {
+  TelemetryEndpoint & origin = (TelemetryEndpoint &)originValue;
   uint8_t physicalId = packet[0] & 0x1F;
   uint8_t primId = packet[1];
   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) {
+    uint8_t instance = physicalId + (origin.rxUid << 5);
-      // outputTelemetryBuffer.sport.physicalId = packet[0];
-      if (origin == SPORT_MODULE)
-        sportOutputByteStuffAndCrc();
-      outputTelemetryBuffer.setDestination(origin);
-    }
-    uint8_t instance = physicalId + 1;
     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);

radio/src/telemetry/telemetry.h

@@ -155,9 +155,8 @@ void logTelemetryWriteByte(uint8_t data);
 #define LOG_TELEMETRY_WRITE_BYTE(data)
 #endif
 
-#define TELEMETRY_ENDPOINT_ANY     0xFF
+#define TELEMETRY_ENDPOINT_NONE    0xFF
-#define TELEMETRY_ENDPOINT_NONE    0xFE
+#define TELEMETRY_ENDPOINT_SPORT   0x07
-#define TELEMETRY_ENDPOINT_SPORT   0xFD
 
 union TelemetryEndpoint {
   PACK(struct {
@@ -169,22 +168,56 @@ union TelemetryEndpoint {
 
 class OutputTelemetryBuffer {
   public:
-    OutputTelemetryBuffer() {
+    OutputTelemetryBuffer()
+    {
       reset();
     }
 
-    void setDestination(uint8_t value) {
+    void setDestination(uint8_t value)
+    {
       destination.value = value;
     }
 
-    void reset() {
+    void reset()
+    {
       destination.value = TELEMETRY_ENDPOINT_NONE;
     }
 
-    bool isAvailable() {
+    bool isAvailable()
+    {
       return destination.value == TELEMETRY_ENDPOINT_NONE;
     }
 
+    void pushByte(uint8_t byte)
+    {
+      data[size++] = byte;
+    }
+
+    void pushByteWithBytestuffing(uint8_t byte)
+    {
+      if (byte == 0x7E || byte == 0x7D) {
+        pushByte(0x7D);
+        pushByte(0x20 ^ byte);
+      }
+      else {
+        pushByte(byte);
+      }
+    }
+
+    void pushSportPacketWithBytestuffing(SportTelemetryPacket & packet)
+    {
+      uint16_t crc = 0;
+      sport.physicalId = packet.physicalId; // no bytestuffing, no CRC
+      for (uint8_t i=1; i<sizeof(SportTelemetryPacket); i++) {
+        uint8_t byte = packet.raw[i];
+        pushByteWithBytestuffing(byte);
+        crc += byte; // 0-1FF
+        crc += crc >> 8; // 0-100
+        crc &= 0x00ff;
+      }
+      pushByte(0xFF - crc);
+    }
+
   public:
     union {
       SportTelemetryPacket sport;