[Crossfire] Upstream link development started

radio/sdcard/taranis/S6R/S6R.lua

@@ -120,11 +120,11 @@ local function redrawFieldsPage()
 end
 
 local function telemetryRead(field)
-  return telemetryPush(0x1A, 0x30, 0x0C30, field)
+  return sportTelemetryPush(0x1A, 0x30, 0x0C30, field)
 end
 
 local function telemetryWrite(field, value)
-  return telemetryPush(0x1A, 0x31, 0x0C30, field + value*256)
+  return sportTelemetryPush(0x1A, 0x31, 0x0C30, field + value*256)
 end
 
 local telemetryPopTimeout = 0
@@ -144,7 +144,7 @@ local function refreshNext()
       end
     end
   elseif refreshState == 1 then
-    physicalId, primId, dataId, value = telemetryPop()
+    physicalId, primId, dataId, value = sportTelemetryPop()
     if physicalId == 0x1A and primId == 0x32 and dataId == 0x0C30 then
       fieldId = value % 256
       if calibrationState == 2 then

radio/src/lua/api_general.cpp

@@ -296,7 +296,7 @@ bool luaFindFieldByName(const char * name, LuaField & field, unsigned int flags)
   return false;  // not found
 }
 
-static int luaTelemetryPop(lua_State *L)
+static int luaSportTelemetryPop(lua_State * L)
 {
   if (!luaInputTelemetryFifo) {
     luaInputTelemetryFifo = new Fifo<LuaTelemetryPacket, 16>();
@@ -307,10 +307,10 @@ static int luaTelemetryPop(lua_State *L)
 
   LuaTelemetryPacket packet;
   if (luaInputTelemetryFifo->pop(packet)) {
-    lua_pushnumber(L, packet.physicalId);
+    lua_pushnumber(L, packet.sport.physicalId);
-    lua_pushnumber(L, packet.primId);
+    lua_pushnumber(L, packet.sport.primId);
-    lua_pushnumber(L, packet.dataId);
+    lua_pushnumber(L, packet.sport.dataId);
-    lua_pushunsigned(L, packet.value);
+    lua_pushunsigned(L, packet.sport.value);
     return 4;
   }
 
@@ -327,17 +327,61 @@ uint8_t getDataId(uint8_t physicalId)
   return result;
 }
 
-static int luaTelemetryPush(lua_State *L)
+static int luaSportTelemetryPush(lua_State * L)
 {
-  if (luaOutputTelemetryPacket.physicalId != 0x7E) {
+  if (luaOutputTelemetryPacket.sport.physicalId != 0x7E) {
     lua_pushboolean(L, false);
     return 1;
   }
 
-  luaOutputTelemetryPacket.physicalId = getDataId(luaL_checkunsigned(L, 1));
+  luaOutputTelemetryPacket.sport.physicalId = getDataId(luaL_checkunsigned(L, 1));
-  luaOutputTelemetryPacket.primId = luaL_checkunsigned(L, 2);
+  luaOutputTelemetryPacket.sport.primId = luaL_checkunsigned(L, 2);
-  luaOutputTelemetryPacket.dataId = luaL_checkunsigned(L, 3);
+  luaOutputTelemetryPacket.sport.dataId = luaL_checkunsigned(L, 3);
-  luaOutputTelemetryPacket.value = luaL_checkunsigned(L, 4);
+  luaOutputTelemetryPacket.sport.value = luaL_checkunsigned(L, 4);
+
+  lua_pushboolean(L, true);
+  return 1;
+}
+
+static int luaCrossfireTelemetryPop(lua_State * L)
+{
+  if (!luaInputTelemetryFifo) {
+    luaInputTelemetryFifo = new Fifo<LuaTelemetryPacket, 16>();
+    if (!luaInputTelemetryFifo) {
+      return 0;
+    }
+  }
+
+  LuaTelemetryPacket packet;
+  if (luaInputTelemetryFifo->pop(packet)) {
+    lua_pushnumber(L, packet.crossfire.command);
+    lua_newtable(L);
+    for (int i=0; i<packet.crossfire.length; i++) {
+      lua_pushinteger(L, i);
+      lua_pushinteger(L, packet.crossfire.data[i]);
+      lua_settable(L, -3);
+    }
+    lua_settable(L, -3);
+    return 2;
+  }
+
+  return 0;
+}
+
+static int luaCrossfireTelemetryPush(lua_State * L)
+{
+  if (luaOutputTelemetryPacket.crossfire.command != 0) {
+    lua_pushboolean(L, false);
+    return 1;
+  }
+
+  luaOutputTelemetryPacket.crossfire.command = luaL_checkunsigned(L, 1);
+  luaL_checktype(L, 2, LUA_TTABLE);
+  luaOutputTelemetryPacket.crossfire.length = min<int>(sizeof(luaOutputTelemetryPacket.crossfire.data), luaL_len(L, 2));
+  for (int i=0; i<luaOutputTelemetryPacket.crossfire.length; i++) {
+    lua_rawgeti(L, 2, i+1);
+    luaOutputTelemetryPacket.crossfire.data[i] = luaL_checkunsigned(L, -1);
+  }
 
   lua_pushboolean(L, true);
   return 1;
@@ -818,8 +862,12 @@ const luaL_Reg opentxLib[] = {
 #if !defined(COLORLCD)
   { "GREY", luaGrey },
 #endif
-  { "telemetryPop", luaTelemetryPop },
+  { "sportTelemetryPop", luaSportTelemetryPop },
-  { "telemetryPush", luaTelemetryPush },
+  { "sportTelemetryPush", luaSportTelemetryPush },
+#if defined(CROSSFIRE)
+  { "crossfireTelemetryPop", luaCrossfireTelemetryPop },
+  { "crossfireTelemetryPush", luaCrossfireTelemetryPush },
+#endif
   { NULL, NULL }  /* sentinel */
 };
 

radio/src/pulses/crossfire.cpp

@@ -24,13 +24,25 @@
 #define CROSSFIRE_CH_BITS           11
 
 // Range for pulses (channels output) is [-1024:+1024]
-void createCrossfireFrame(uint8_t * frame, int16_t * pulses)
+uint8_t createCrossfireFrame(uint8_t * frame, int16_t * pulses, LuaTelemetryPacket * telemetryPacket)
 {
   uint8_t * buf = frame;
-  *buf++ = BROADCAST_ADDRESS;
+  *buf++ = MODULE_ADDRESS;
-  *buf++ = 24; // 1(ID) + 22 + 1(CRC)
-  *buf++ = CHANNELS_ID;
 
+  if (telemetryPacket->crossfire.command) {
+    *buf++ = 2 + telemetryPacket->crossfire.length; // 1(ID) + telemetryPacket->crossfire.length + 1(CRC)
+    uint8_t * crc_start = buf;
+    *buf++ = telemetryPacket->crossfire.command;
+    for (int i=0; i<telemetryPacket->crossfire.length; i++) {
+      *buf++ = telemetryPacket->crossfire.data[i];
+    }
+    *buf++ = crc8(crc_start, telemetryPacket->crossfire.length + 1);
+    telemetryPacket->crossfire.clear();
+  }
+  else {
+    *buf++ = 24; // 1(ID) + 22 + 1(CRC)
+    uint8_t * crc_start = buf;
+    *buf++ = CHANNELS_ID;
     uint32_t bits = 0;
     uint8_t bitsavailable = 0;
     for (int i=0; i<CROSSFIRE_CHANNELS_COUNT; i++) {
@@ -43,5 +55,8 @@ void createCrossfireFrame(uint8_t * frame, int16_t * pulses)
         bitsavailable -= 8;
       }
     }
-  *buf = crc8(&frame[2], 23);
+    *buf++ = crc8(crc_start, 23);
+  }
+
+  return buf - frame;
 }

radio/src/pulses/pulses_arm.cpp

@@ -28,6 +28,10 @@ uint8_t moduleFlag[NUM_MODULES] = { 0 };
 ModulePulsesData modulePulsesData[NUM_MODULES] __DMA;
 TrainerPulsesData trainerPulsesData __DMA;
 
+#if defined(CROSSFIRE)
+uint8_t createCrossfireFrame(uint8_t * frame, int16_t * pulses, LuaTelemetryPacket * telemetry);
+#endif
+
 uint8_t getRequiredProtocol(uint8_t port)
 {
   uint8_t required_protocol;
@@ -177,8 +181,8 @@ void setupPulses(uint8_t port)
     case PROTO_CROSSFIRE:
       if (telemetryProtocol == PROTOCOL_PULSES_CROSSFIRE && !init_needed) {
         uint8_t * crossfire = modulePulsesData[port].crossfire.pulses;
-        createCrossfireFrame(crossfire, &channelOutputs[g_model.moduleData[port].channelsStart]);
+        uint8_t len = createCrossfireFrame(crossfire, &channelOutputs[g_model.moduleData[port].channelsStart], &luaOutputTelemetryPacket);
-        sportSendBuffer(crossfire, CROSSFIRE_FRAME_LEN);
+        sportSendBuffer(crossfire, len);
       }
       scheduleNextMixerCalculation(port, CROSSFIRE_FRAME_PERIOD);
       break;

radio/src/pulses/pulses_arm.h

@@ -100,10 +100,10 @@ PACK(struct Dsm2TimerPulsesData {
 
 #define CROSSFIRE_BAUDRATE             400000
 #define CROSSFIRE_FRAME_PERIOD         4 // 4ms
-#define CROSSFIRE_FRAME_LEN            (3+22+1)
+#define CROSSFIRE_FRAME_MAXLEN         64
 #define CROSSFIRE_CHANNELS_COUNT       16
 PACK(struct CrossfirePulsesData {
-  uint8_t pulses[CROSSFIRE_FRAME_LEN];
+  uint8_t pulses[CROSSFIRE_FRAME_MAXLEN];
 });
 
 union ModulePulsesData {
@@ -146,8 +146,6 @@ void sendByteDsm2(uint8_t b);
 void putDsm2Flush();
 void putDsm2SerialBit(uint8_t bit);
 
-void createCrossfireFrame(uint8_t * frame, int16_t * pulses);
-
 #if defined(HUBSAN)
 void Hubsan_Init();
 #endif

radio/src/targets/taranis/telemetry_driver.cpp

@@ -147,9 +147,9 @@ extern "C" void TELEMETRY_USART_IRQHandler(void)
 #if defined(LUA)
       if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) {
         static uint8_t prevdata;
-        if (prevdata == 0x7E && data == luaOutputTelemetryPacket.physicalId) {
+        if (prevdata == 0x7E && data == luaOutputTelemetryPacket.sport.physicalId) {
           sportSendLuaPacket(luaOutputTelemetryPacket);
-          luaOutputTelemetryPacket.physicalId = 0x7E;
+          luaOutputTelemetryPacket.sport.clear();
         }
         prevdata = data;
       }

radio/src/telemetry/crossfire.cpp

@@ -192,6 +192,21 @@ void processCrossfireTelemetryFrame()
       }
       break;
     }
+
+#if defined(LUA)
+    default:
+      if (luaInputTelemetryFifo) {
+        LuaTelemetryPacket luaPacket;
+        luaPacket.crossfire.command = id;
+        luaPacket.crossfire.length = telemetryRxBuffer[1]-2;
+        for (int i=0; i<min<int>(sizeof(luaPacket.crossfire.data), luaPacket.crossfire.length); i++) {
+          luaPacket.crossfire.data[i] = telemetryRxBuffer[3+i];
+        }
+        luaInputTelemetryFifo->push(luaPacket);
+      }
+      break;
+#endif
+
   }
 }
 

radio/src/telemetry/crossfire.h

@@ -23,6 +23,7 @@
 
 // Device address
 #define BROADCAST_ADDRESS              0x00
+#define MODULE_ADDRESS                 0xEE
 
 // Frame id
 #define GPS_ID                         0x02
@@ -31,6 +32,9 @@
 #define CHANNELS_ID                    0x16
 #define ATTITUDE_ID                    0x1E
 #define FLIGHT_MODE_ID                 0x21
+#define PING_DEVICES_ID                0x28
+#define DEVICE_INFO_ID                 0x29
+#define REQUEST_SETTINGS_ID            0x2A
 
 void processCrossfireTelemetryData(uint8_t data);
 void crossfireSetDefault(int index, uint8_t id, uint8_t subId);

radio/src/telemetry/frsky.h

@@ -487,27 +487,38 @@ void processFrskyTelemetryData(uint8_t data);
 #if defined(LUA)
 struct LuaTelemetryPacket
 {
-  LuaTelemetryPacket() { };
+  LuaTelemetryPacket()
-  LuaTelemetryPacket(uint8_t physicalId, uint8_t primId, uint16_t dataId, uint32_t value):
-    physicalId(physicalId),
-    primId(primId),
-    dataId(dataId),
-    value(value)
   {
   }
 
-  PACK(struct {
+  union {
+    struct {
       uint8_t physicalId;
       union {
-      PACK(struct {
+        struct {
           uint8_t primId;
           uint16_t dataId;
           uint32_t value;
           uint8_t crc;
-      });
+        };
         uint8_t raw[8];
       };
-  });
+      void clear()
+      {
+        physicalId = 0x7E;
+      }
+    } sport;
+
+    struct {
+      uint8_t command;
+      uint8_t length;
+      uint8_t data[8];
+      void clear()
+      {
+        command = 0x00;
+      }
+    } crossfire;
+  };
 };
 
 extern Fifo<LuaTelemetryPacket, 16> * luaInputTelemetryFifo;

radio/src/telemetry/frsky_sport.cpp

@@ -201,14 +201,14 @@ void sportSendLuaPacket(LuaTelemetryPacket & packet)
 
   uint16_t crc = 0;
   for (uint8_t i=0; i<7; i++) {
-    if (packet.raw[i] == 0x7E || packet.raw[i] == 0x7D) {
+    if (packet.sport.raw[i] == 0x7E || packet.sport.raw[i] == 0x7D) {
       *ptr++ = 0x7D;
-      *ptr++ = 0x20 ^ packet.raw[i];
+      *ptr++ = 0x20 ^ packet.sport.raw[i];
     }
     else {
-      *ptr++ = packet.raw[i];
+      *ptr++ = packet.sport.raw[i];
     }
-    crc += packet.raw[i];         //0-1FF
+    crc += packet.sport.raw[i];         //0-1FF
     crc += crc >> 8;   //0-100
     crc &= 0x00ff;
   }
@@ -313,7 +313,12 @@ void processSportPacket(uint8_t * packet)
         else if (id >= DIY_FIRST_ID && id <= DIY_LAST_ID) {
 #if defined(LUA)
           if (luaInputTelemetryFifo) {
-            luaInputTelemetryFifo->push(LuaTelemetryPacket(physicalId, primId, id, data));
+            LuaTelemetryPacket luaPacket;
+            luaPacket.sport.physicalId = physicalId;
+            luaPacket.sport.primId = primId;
+            luaPacket.sport.dataId = id;
+            luaPacket.sport.value = data;
+            luaInputTelemetryFifo->push(luaPacket);
           }
 #endif
         }
@@ -326,7 +331,12 @@ void processSportPacket(uint8_t * packet)
 #if defined(LUA)
   else if (primId == 0x32) {
     if (luaInputTelemetryFifo) {
-      luaInputTelemetryFifo->push(LuaTelemetryPacket(physicalId, primId, id, data));
+      LuaTelemetryPacket luaPacket;
+      luaPacket.sport.physicalId = physicalId;
+      luaPacket.sport.primId = primId;
+      luaPacket.sport.dataId = id;
+      luaPacket.sport.value = data;
+      luaInputTelemetryFifo->push(luaPacket);
     }
   }
 #endif

radio/src/telemetry/telemetry.cpp

@@ -421,6 +421,7 @@ void telemetryInit(uint8_t protocol)
 #if defined(CROSSFIRE)
   else if (protocol == PROTOCOL_PULSES_CROSSFIRE) {
     telemetryPortInit(CROSSFIRE_BAUDRATE);
+    luaOutputTelemetryPacket.crossfire.clear();
     telemetryPortSetDirectionOutput();
   }
 #endif
@@ -430,6 +431,7 @@ void telemetryInit(uint8_t protocol)
   }
   else {
     telemetryPortInit(FRSKY_SPORT_BAUDRATE);
+    luaOutputTelemetryPacket.sport.clear();
   }
 
 #if defined(REVX) && !defined(SIMU)
@@ -455,5 +457,5 @@ NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm)
 
 #if defined(LUA)
 Fifo<LuaTelemetryPacket, 16> * luaInputTelemetryFifo = NULL;
-LuaTelemetryPacket luaOutputTelemetryPacket = { 0x7E, 0x0, 0x0, 0x0 };
+LuaTelemetryPacket luaOutputTelemetryPacket;
 #endif