[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

@@ -85,7 +85,7 @@ minor: 1
 rev: 7
 ```
 */
-static int luaGetVersion(lua_State *L)
+static int luaGetVersion(lua_State * L)
 {
   lua_pushstring(L, VERSION);
   lua_pushstring(L, RADIO_VERSION);
@@ -105,13 +105,13 @@ run time: 12.54 seconds, return value: 1254
 
 @status current Introduced in 2.0.0
 */
-static int luaGetTime(lua_State *L)
+static int luaGetTime(lua_State * L)
 {
   lua_pushunsigned(L, get_tmr10ms());
   return 1;
 }
 
-static void luaPushDateTime(lua_State *L, uint32_t year, uint32_t mon, uint32_t day,
+static void luaPushDateTime(lua_State * L, uint32_t year, uint32_t mon, uint32_t day,
                             uint32_t hour, uint32_t min, uint32_t sec)
 {
   lua_createtable(L, 0, 6);
@@ -136,7 +136,7 @@ Return current system date and time that is kept by the RTC unit
  * `min` (number) minutes
  * `sec` (number) seconds
 */
-static int luaGetDateTime(lua_State *L)
+static int luaGetDateTime(lua_State * L)
 {
   struct gtm utm;
   gettime(&utm);
@@ -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;
@@ -365,7 +409,7 @@ The list of valid sources is available:
 
 @status current Introduced in 2.0.8
 */
-static int luaGetFieldInfo(lua_State *L)
+static int luaGetFieldInfo(lua_State * L)
 {
   const char * what = luaL_checkstring(L, 1);
   LuaField field;
@@ -424,7 +468,7 @@ case the returned value is 0):
 While `Cels` sensor returns current values of all cells in a table, a `Cels+` or
 `Cels-` will return a single value - the maximum or minimum Cels value.
 */
-static int luaGetValue(lua_State *L)
+static int luaGetValue(lua_State * L)
 {
   int src = 0;
   if (lua_isnumber(L, 1)) {
@@ -458,7 +502,7 @@ is not specified (or contains invalid value), then the current flight mode data
 
 @status current Introduced in 2.1.7
 */
-static int luaGetFlightMode(lua_State *L)
+static int luaGetFlightMode(lua_State * L)
 {
   int mode = luaL_optinteger(L, 1, -1);
   if (mode < 0 || mode >= MAX_FLIGHT_MODES) {
@@ -482,7 +526,7 @@ to the path (example: for English language: `/SOUNDS/en` is appended)
 
 @status current Introduced in 2.0.0, changed in 2.1.0
 */
-static int luaPlayFile(lua_State *L)
+static int luaPlayFile(lua_State * L)
 {
   const char * filename = luaL_checkstring(L, 1);
   if (filename[0] != '/') {
@@ -575,7 +619,7 @@ OpenTX 2.1:
 | 24  | Seconds | 162 |
 
 */
-static int luaPlayNumber(lua_State *L)
+static int luaPlayNumber(lua_State * L)
 {
   int number = luaL_checkinteger(L, 1);
   int unit = luaL_checkinteger(L, 2);
@@ -597,7 +641,7 @@ Play a time value (text to speech)
 
 @status current Introduced in 2.1.0
 */
-static int luaPlayDuration(lua_State *L)
+static int luaPlayDuration(lua_State * L)
 {
   int duration = luaL_checkinteger(L, 1);
   bool playTime = (luaL_optinteger(L, 2, 0) != 0);
@@ -627,7 +671,7 @@ The valid range is from -127 to 127.
 
 @status current Introduced in 2.1.0
 */
-static int luaPlayTone(lua_State *L)
+static int luaPlayTone(lua_State * L)
 {
   int frequency = luaL_checkinteger(L, 1);
   int length = luaL_checkinteger(L, 2);
@@ -649,7 +693,7 @@ Stops key state machine.
 
 TODO table of events/masks
 */
-static int luaKillEvents(lua_State *L)
+static int luaKillEvents(lua_State * L)
 {
   uint8_t key = EVT_KEY_MASK(luaL_checkinteger(L, 1));
   // prevent killing maskable keys (only in telemetry scripts)
@@ -669,7 +713,7 @@ Returns gray value which can be used in LCD functions
 @retval (number) a value that represents amount of *greyness* (from 0 to 15)
 
 */
-static int luaGrey(lua_State *L)
+static int luaGrey(lua_State * L)
 {
   int index = luaL_checkinteger(L, 1);
   lua_pushunsigned(L, GREY(index));
@@ -694,7 +738,7 @@ Returns (some of) the general radio settings
 `language` and `voice` added int 2.2.0.
 
 */
-static int luaGetGeneralSettings(lua_State *L)
+static int luaGetGeneralSettings(lua_State * L)
 {
   lua_newtable(L);
   lua_pushtablenumber(L, "battMin", double(90+g_eeGeneral.vBatMin)/10);
@@ -731,7 +775,7 @@ Run function (key pressed)
 
 @status current Introduced in 2.0.0
 */
-static int luaPopupInput(lua_State *L)
+static int luaPopupInput(lua_State * L)
 {
   uint8_t event = luaL_checkinteger(L, 2);
   warningInputValue = luaL_checkinteger(L, 3);
@@ -765,7 +809,7 @@ Get stick that is assigned to a channel. See Default Channel Order in General Se
 
 @status current Introduced in 2.0.0
 */
-static int luaDefaultStick(lua_State *L)
+static int luaDefaultStick(lua_State * L)
 {
   uint8_t channel = luaL_checkinteger(L, 1);
   lua_pushinteger(L, channel_order(channel+1)-1);
@@ -785,7 +829,7 @@ Get channel assigned to stick. See Default Channel Order in General Settings
 
 @status current Introduced in 2.0.0
 */
-static int luaDefaultChannel(lua_State *L)
+static int luaDefaultChannel(lua_State * L)
 {
   uint8_t stick = luaL_checkinteger(L, 1);
   for (int i=1; i<=4; i++) {
@@ -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,24 +24,39 @@
 #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;
 
-  uint32_t bits = 0;
+  if (telemetryPacket->crossfire.command) {
-  uint8_t bitsavailable = 0;
+    *buf++ = 2 + telemetryPacket->crossfire.length; // 1(ID) + telemetryPacket->crossfire.length + 1(CRC)
-  for (int i=0; i<CROSSFIRE_CHANNELS_COUNT; i++) {
+    uint8_t * crc_start = buf;
-    uint32_t val = limit(0, CROSSFIRE_CH_CENTER + (((pulses[i]) * 4) / 5), 2*CROSSFIRE_CH_CENTER);
+    *buf++ = telemetryPacket->crossfire.command;
-    bits |= val << bitsavailable;
+    for (int i=0; i<telemetryPacket->crossfire.length; i++) {
-    bitsavailable += CROSSFIRE_CH_BITS;
+      *buf++ = telemetryPacket->crossfire.data[i];
-    while (bitsavailable >= 8) {
-      *buf++ = bits;
-      bits >>= 8;
-      bitsavailable -= 8;
     }
+    *buf++ = crc8(crc_start, telemetryPacket->crossfire.length + 1);
+    telemetryPacket->crossfire.clear();
   }
-  *buf = crc8(&frame[2], 23);
+  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++) {
+      uint32_t val = limit(0, CROSSFIRE_CH_CENTER + (((pulses[i]) * 4) / 5), 2*CROSSFIRE_CH_CENTER);
+      bits |= val << bitsavailable;
+      bitsavailable += CROSSFIRE_CH_BITS;
+      while (bitsavailable >= 8) {
+        *buf++ = bits;
+        bits >>= 8;
+        bitsavailable -= 8;
+      }
+    }
+    *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

@@ -422,7 +422,7 @@ void frskyDProcessPacket(uint8_t *packet);
 void processSportPacket(uint8_t * packet);
 
 #if defined(PCBTARANIS)
-void sportFirmwareUpdate(ModuleIndex module, const char *filename);
+void sportFirmwareUpdate(ModuleIndex module, const char * filename);
 #endif
 void telemetryWakeup();
 void telemetryReset();
@@ -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 {
-    uint8_t physicalId;
+    struct {
-    union {
+      uint8_t physicalId;
-      PACK(struct {
+      union {
-        uint8_t primId;
+        struct {
-        uint16_t dataId;
+          uint8_t primId;
-        uint32_t value;
+          uint16_t dataId;
-        uint8_t crc;
+          uint32_t value;
-      });
+          uint8_t crc;
-      uint8_t raw[8];
+        };
-    };
+        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
@@ -543,7 +553,7 @@ bool sportUpdateEnd()
   return sportWaitState(SPORT_COMPLETE, 2000);
 }
 
-void sportFirmwareUpdate(ModuleIndex module, const char *filename)
+void sportFirmwareUpdate(ModuleIndex module, const char * filename)
 {
   bool result = sportUpdatePowerOn(module);
   if (result)

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