FrSky S.PORT upstream (#3426)

FrSky S.PORT upstream

radio/sdcard/taranis/TEMPLATES/S6R.lua

@@ -0,0 +1,183 @@
+local VALUE = 0
+local COMBO = 1
+
+local edit = false
+local current = 1
+local refreshState = 0
+local refreshIndex = 0
+local pageOffset = 0
+
+local fields = {
+  {"S6R functions:", COMBO, 0x9C, nil, { "Disable", "Enable" } },
+  {"Wing type:", COMBO, 0x80, nil, { "REA", "DELTA", "VTAIL" } },
+  {"", COMBO, 0x81, nil, { "Reverse", "Upright", "Upright reverse" } },
+  {"AIL direction:", COMBO, 0x82, nil, { "Normal", "Invers" }, { 0, 255 } },
+  {"ELE direction:", COMBO, 0x83, nil, { "Normal", "Invers" }, { 0, 255 } },
+  {"RUD direction:", COMBO, 0x84, nil, { "Normal", "Invers" }, { 0, 255 } },
+  {"AIL2 direction:", COMBO, 0x9A, nil, { "Normal", "Invers" }, { 0, 255 } },
+  {"ELE2 direction:", COMBO, 0x9B, nil, { "Normal", "Invers" }, { 0, 255 } },
+  {"AIL stabilize gain:", VALUE, 0x85, nil, 0, 200, "%"},
+  {"ELE stabilize gain:", VALUE, 0x86, nil, 0, 200, "%"},
+  {"RUD stabilize gain:", VALUE, 0x87, nil, 0, 200, "%"},
+  {"AIL auto level gain:", VALUE, 0x88, nil, 0, 200, "%"},
+  {"ELE auto level gain:", VALUE, 0x89, nil, 0, 200, "%"},
+  {"ELE upright gain:", VALUE, 0x8C, nil, 0, 200, "%"},
+  {"RUD upright gain:", VALUE, 0x8D, nil, 0, 200, "%"},
+  {"AIL crab gain:", VALUE, 0x8E, nil, 0, 200, "%"},
+  {"RUD crab gain:", VALUE, 0x90, nil, 0, 200, "%"},
+  {"AIL auto angle offset:", VALUE, 0x91, nil, -20, 20, "%", 0x6C},
+  {"ELE auto angle offset:", VALUE, 0x92, nil, -20, 20, "%", 0x6C},
+  {"ELE upright angle offset:", VALUE, 0x95, nil, -20, 20, "%", 0x6C},
+  {"RUD upright angle offset:", VALUE, 0x96, nil, -20, 20, "%", 0x6C},
+  {"AIL crab angle offset:", VALUE, 0x97, nil, -20, 20, "%", 0x6C},
+  {"RUD crab angle offset:", VALUE, 0x99, nil, -20, 20, "%", 0x6C},
+  -- {"Calib Accel. position:", COMBO, 0x9D, nil, { "Up", "Down", "Left", "Right", "Forward", "Back" } },
+}
+
+-- Change display attribute to current field
+local function addField(step)
+  field = fields[current]
+  if field[2] == VALUE then
+    min = field[5]
+    max = field[6]
+  elseif field[2] == COMBO then
+    min = 0
+    max = #(field[5]) - 1
+  end
+  if (step < 0 and field[4] > min) or (step > 0 and field[4] < max) then
+    field[4] = field[4] + step
+  end
+end
+
+-- Select the next or previous editable field
+local function selectField(step)
+  current = 1 + ((current + step - 1 + #fields) % #fields)
+  if current > 7 + pageOffset then
+    pageOffset = current - 7
+  elseif current <= pageOffset then
+    pageOffset = current - 1
+  end
+end
+
+local function drawProgressBar()
+  local width = (160 * refreshIndex) / #fields
+  lcd.drawRectangle(48, 0, 164, 8)
+  lcd.drawFilledRectangle(50, 2, width, 4);
+end
+
+-- Redraw the current page
+local function redrawPage()
+  lcd.clear()
+  lcd.drawScreenTitle("S8R", 0, 0)
+
+  if refreshIndex < #fields then
+    drawProgressBar()
+  end
+
+  for index = 1, 7, 1 do
+    field = fields[pageOffset+index]
+    attr = current == (pageOffset+index) and ((edit == true and BLINK or 0) + INVERS) or 0
+
+    -- display label
+    lcd.drawText(0, 1+8*index, field[1])
+
+    if field[4] == nil then
+      lcd.drawText(150, 1+8*index, "---", attr)
+    else
+      if field[2] == VALUE then
+        lcd.drawNumber(150, 1+8*index, field[4], LEFT + attr)
+      elseif field[2] == COMBO then
+        if field[4] >= 0 and field[4] < #(field[5]) then
+          lcd.drawText(150, 1+8*index, field[5][1+field[4]], attr)
+        end
+      end
+    end
+  end
+end
+
+local counter = 0
+local function refreshNext()
+  if refreshState == 0 then
+    if refreshIndex < #fields then
+      field = fields[refreshIndex + 1]
+      if telemetryPush(0x1A, 0x30, 0x0C30, field[3]) == true then
+        refreshState = 1
+        counter = 0
+      end
+    end
+  else
+    field = fields[refreshIndex + 1]
+    physicalId, primId, dataId, value = telemetryPop()
+    if physicalId == 0x1A and primId == 0x32 and dataId == 0x0C30 and value % 256 == field[3] then
+      value = math.floor(value / 256)
+      if field[2] == COMBO and #field == 6 then
+        for index = 1, #(field[6]), 1 do
+          if value == field[6][index] then
+            value = index - 1
+            break
+          end
+        end
+      elseif field[2] == VALUE and #field == 8 then
+        value = value - field[8] + field[5]
+      end
+      fields[refreshIndex + 1][4] = value
+      refreshIndex = refreshIndex + 1
+      refreshState = 0
+    elseif counter == 20 then
+      refreshState = 0
+    else
+      counter = counter + 1
+    end
+  end
+end
+
+local function updateField(field)
+  value = field[4]
+  if field[2] == COMBO and #field == 6 then
+    value = field[6][1+value]
+  elseif field[2] == VALUE and #field == 8 then
+    value = value + field[8] - field[5]
+  end
+  telemetryPush(0x1A, 0x31, 0x0C30, field[3] + value*256)
+end
+
+-- Init
+local function init()
+  current, edit, refreshState, refreshIndex = 1, false, 0, 0
+end
+
+-- Main
+local function run(event)
+  if event == nil then
+    error("Cannot be run as a model script!")
+    return 2
+  elseif event == EVT_EXIT_BREAK then                   -- exit script
+    return 2
+  elseif event == EVT_ENTER_BREAK then                  -- toggle editing/selecting current field
+    if fields[current][4] ~= nil then
+      edit = not edit
+      if edit == false then
+        updateField(fields[current])
+      end
+    end  
+    redrawPage()
+  elseif edit then
+    if event == EVT_PLUS_FIRST or event == EVT_PLUS_REPT then
+      addField(1)
+    elseif event == EVT_MINUS_FIRST or event == EVT_MINUS_REPT then
+      addField(-1)
+    end
+    redrawPage()
+  else
+    if event == EVT_MINUS_FIRST then
+      selectField(1)
+    elseif event == EVT_PLUS_FIRST then
+      selectField(-1) 
+    end
+    redrawPage()
+  end
+  refreshNext()
+  return 0
+end
+
+return { init=init, run=run }

radio/src/lua/api_general.cpp

@@ -296,40 +296,48 @@ bool luaFindFieldByName(const char * name, LuaField & field, unsigned int flags)
 static int luaTelemetryPop(lua_State *L)
 {
   if (!luaInputTelemetryFifo) {
-    luaInputTelemetryFifo = new Fifo<LuaTelemetryValue, 16>();
+    luaInputTelemetryFifo = new Fifo<LuaTelemetryPacket, 16>();
     if (!luaInputTelemetryFifo) {
       return 0;
     }
   }
 
-  LuaTelemetryValue value;
+  LuaTelemetryPacket packet;
-  if (luaInputTelemetryFifo->pop(value)) {
+  if (luaInputTelemetryFifo->pop(packet)) {
-    lua_pushnumber(L, value.id);
+    lua_pushnumber(L, packet.physicalId);
-    lua_pushunsigned(L, value.value);
+    lua_pushnumber(L, packet.primId);
-    return 2;
+    lua_pushnumber(L, packet.dataId);
+    lua_pushunsigned(L, packet.value);
+    return 4;
   }
 
   return 0;
 }
 
+#define BIT(x, index) (((x) >> index) & 0x01)
+uint8_t getDataId(uint8_t physicalId)
+{
+  uint8_t result = physicalId;
+  result += (BIT(physicalId, 0) ^ BIT(physicalId, 1) ^ BIT(physicalId, 2)) << 5;
+  result += (BIT(physicalId, 2) ^ BIT(physicalId, 3) ^ BIT(physicalId, 4)) << 6;
+  result += (BIT(physicalId, 0) ^ BIT(physicalId, 2) ^ BIT(physicalId, 4)) << 7;
+  return result;
+}
+
 static int luaTelemetryPush(lua_State *L)
 {
-  if (!luaOutputTelemetryFifo) {
+  if (luaOutputTelemetryPacket.physicalId != 0x7E) {
-    luaOutputTelemetryFifo = new Fifo<LuaTelemetryValue, 16>();
+    lua_pushboolean(L, false);
-    if (!luaOutputTelemetryFifo) {
+    return 1;
-      return 0;
-    }
   }
 
-  uint8_t id = luaL_checkunsigned(L, 1);
+  luaOutputTelemetryPacket.physicalId = getDataId(luaL_checkunsigned(L, 1));
-  uint32_t value = luaL_checkunsigned(L, 2);
+  luaOutputTelemetryPacket.primId = luaL_checkunsigned(L, 2);
+  luaOutputTelemetryPacket.dataId = luaL_checkunsigned(L, 3);
+  luaOutputTelemetryPacket.value = luaL_checkunsigned(L, 4);
 
-#if defined __GNUC__
+  lua_pushboolean(L, true);
-  // TODO remove this ifdef when we have updated to MSVC recent version
+  return 1;
-  luaOutputTelemetryFifo->push((LuaTelemetryValue){ id, value });
-#endif
-
-  return 0;
 }
 
 /*luadoc

radio/src/targets/taranis/bootloader/CMakeLists.txt

@@ -62,7 +62,7 @@ set(BOOTLOADER_SRC
   boot.cpp
   )
 
-remove_definitions(-DUSB_JOYSTICK -DUSB_SERIAL -DDEBUG)
+remove_definitions(-DUSB_JOYSTICK -DUSB_SERIAL -DLUA -DDEBUG)
 add_definitions(-DBOOT -DUSB_MASS_STORAGE)
 
 set(CMAKE_EXE_LINKER_FLAGS "-mcpu=${MCU} -mthumb -nostartfiles -lm -T${RADIO_SRC_DIRECTORY}/targets/taranis/stm32_ramboot.ld -Wl,-Map=bootloader.map,--cref,--no-warn-mismatch,--gc-sections")

radio/src/targets/taranis/telemetry_driver.cpp

@@ -144,6 +144,16 @@ extern "C" void TELEMETRY_USART_IRQHandler(void)
     }
     else {
       telemetryFifo.push(data);
+#if defined(LUA)
+      if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) {
+        static uint8_t prevdata;
+        if (prevdata == 0x7E && data == luaOutputTelemetryPacket.physicalId) {
+          sportSendLuaPacket(luaOutputTelemetryPacket);
+          luaOutputTelemetryPacket.physicalId = 0x7E;
+        }
+        prevdata = data;
+      }
+#endif
     }
     status = TELEMETRY_USART->SR;
   }

radio/src/telemetry/frsky.h

@@ -418,7 +418,8 @@ void frskyDSendNextAlarm();
 void frskyDProcessPacket(uint8_t *packet);
 
 // FrSky S.PORT Protocol
-void processSportPacket(uint8_t *packet);
+void processSportPacket(uint8_t * packet);
+
 #if defined(PCBTARANIS)
 void sportFirmwareUpdate(ModuleIndex module, const char *filename);
 #endif
@@ -483,14 +484,34 @@ void frskyUpdateCells();
 void processFrskyTelemetryData(uint8_t data);
 
 #if defined(LUA)
-struct LuaTelemetryValue
+struct LuaTelemetryPacket
 {
-  uint8_t id;
+  LuaTelemetryPacket() { };
-  uint32_t value;
+  LuaTelemetryPacket(uint8_t physicalId, uint8_t primId, uint16_t dataId, uint32_t value):
+    physicalId(physicalId),
+    primId(primId),
+    dataId(dataId),
+    value(value)
+    {
+    }
+
+  PACK(struct {
+    uint8_t physicalId;
+    union {
+      PACK(struct {
+        uint8_t primId;
+        uint16_t dataId;
+        uint32_t value;
+        uint8_t crc;
+      });
+      uint8_t raw[8];
+    };
+  });
 };
 
-extern Fifo<LuaTelemetryValue, 16> * luaInputTelemetryFifo;
+extern Fifo<LuaTelemetryPacket, 16> * luaInputTelemetryFifo;
-extern Fifo<LuaTelemetryValue, 16> * luaOutputTelemetryFifo;
+extern LuaTelemetryPacket luaOutputTelemetryPacket;
+void sportSendLuaPacket(LuaTelemetryPacket & packet);
 #endif
 
 #endif // _FRSKY_H_

radio/src/telemetry/frsky_sport.cpp

@@ -127,7 +127,7 @@ bool intPwr, extPwr;
 uint16_t servosState;
 uint16_t rboxState;
 
-void processSportUpdatePacket(uint8_t *packet)
+void processSportUpdatePacket(uint8_t * packet)
 {
   if (packet[0]==0x5E && packet[1]==0x50) {
     switch (packet[2]) {
@@ -190,11 +190,40 @@ void processSportPacket(uint16_t id, uint8_t subId, uint8_t instance, uint32_t d
   }
 }
 
+uint8_t sportUpdatePacket[16] __DMA;
+
+#if defined(LUA)
+// TODO merge it with S.PORT update function when finished
+void sportSendLuaPacket(LuaTelemetryPacket & packet)
+{
+  uint8_t * ptr = sportUpdatePacket;
+  // *ptr++ = 0x7E;
+  // *ptr++ = 0x1A;
+
+  uint16_t crc = 0;
+  for (uint8_t i=0; i<7; i++) {
+    if (packet.raw[i] == 0x7E || packet.raw[i] == 0x7D) {
+      *ptr++ = 0x7D;
+      *ptr++ = 0x20 ^ packet.raw[i];
+    }
+    else {
+      *ptr++ = packet.raw[i];
+    }
+    crc += packet.raw[i];         //0-1FF
+    crc += crc >> 8;   //0-100
+    crc &= 0x00ff;
+  }
+  *ptr++ = 0xFF-crc;
+  sportSendBuffer(sportUpdatePacket, ptr-sportUpdatePacket);
+}
+#endif
+
 void processSportPacket(uint8_t * packet)
 {
-  uint8_t  instance = (packet[0] & 0x1F) + 1;
+  uint8_t physicalId = packet[0] & 0x1F;
-  uint8_t  prim   = packet[1];
+  uint8_t primId = packet[1];
-  uint16_t id  = *((uint16_t *)(packet+2));
+  uint16_t id = *((uint16_t *)(packet+2));
+  uint32_t data = SPORT_DATA_S32(packet);
 
 #if defined(CPUSTM32) && !defined(SIMU)
   if (sportUpdateState != SPORT_IDLE) {
@@ -209,9 +238,8 @@ void processSportPacket(uint8_t * packet)
     return;
   }
 
-  if (prim == DATA_FRAME)  {
+  if (primId == DATA_FRAME) {
-    uint32_t data = SPORT_DATA_S32(packet);
+    uint8_t instance = physicalId + 1;
-
     if (id == RSSI_ID) {
       telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
       data = SPORT_DATA_U8(packet);
@@ -282,7 +310,7 @@ void processSportPacket(uint8_t * packet)
         else if (id >= DIY_FIRST_ID && id <= DIY_LAST_ID) {
 #if defined(LUA)
           if (luaInputTelemetryFifo) {
-            luaInputTelemetryFifo->push((LuaTelemetryValue){(uint8_t)id, data});
+            luaInputTelemetryFifo->push(LuaTelemetryPacket(physicalId, primId, id, data));
           }
 #endif
         }
@@ -292,6 +320,13 @@ void processSportPacket(uint8_t * packet)
       }
     }
   }
+#if defined(LUA)
+  else if (primId == 0x32) {
+    if (luaInputTelemetryFifo) {
+      luaInputTelemetryFifo->push(LuaTelemetryPacket(physicalId, primId, id, data));
+    }
+  }
+#endif
 }
 
 void frskySportSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
@@ -364,16 +399,14 @@ bool sportWaitState(SportUpdateState state, int timeout)
 #endif
 }
 
-void blankPacket(uint8_t *packet)
+void blankPacket(uint8_t * packet)
 {
   memset(packet+2, 0, 6);
 }
 
-uint8_t sportUpdatePacket[16] __DMA;
-
 void writePacket(uint8_t * packet)
 {
-  uint8_t *ptr = sportUpdatePacket;
+  uint8_t * ptr = sportUpdatePacket;
   *ptr++ = 0x7E;
   *ptr++ = 0xFF;
   packet[7] = crc16(packet, 7);

radio/src/telemetry/telemetry.cpp

@@ -454,6 +454,6 @@ NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm)
 }
 
 #if defined(LUA)
-Fifo<LuaTelemetryValue, 16> * luaInputTelemetryFifo = NULL;
+Fifo<LuaTelemetryPacket, 16> * luaInputTelemetryFifo = NULL;
-Fifo<LuaTelemetryValue, 16> * luaOutputTelemetryFifo = NULL;
+LuaTelemetryPacket luaOutputTelemetryPacket = { 0x7E, 0x0, 0x0, 0x0 };
 #endif

radio/src/tests/frsky.cpp

@@ -24,7 +24,7 @@ void frskyDProcessPacket(uint8_t *packet);
 
 #if defined(FRSKY_SPORT)
 bool checkSportPacket(uint8_t *packet);
-void processSportPacket(uint8_t *packet);
+void processSportPacket(uint8_t * packet);
 bool checkSportPacket(uint8_t *packet);
 void frskyCalculateCellStats(void);
 void displayVoltagesScreen();
@@ -173,14 +173,17 @@ TEST(FrSky, HubAltNegative)
   processHubPacket(BARO_ALT_AP_ID, 05);
   EXPECT_EQ(telemetryItems[0].value, 120);
 }
-#endif  // #if defined(FRSKY) && defined(CPUARM)
+#endif // defined(FRSKY) && defined(CPUARM)
 
 #if defined(FRSKY_SPORT)
 TEST(FrSkySPORT, checkCrc)
 {
-  // uint8_t pkt[] = { 0x7E, 0x98, 0x10, 0x10, 0x00, 0x7D, 0x5E, 0x02, 0x00, 0x00, 0x5F };
+  // Packet downstream
-  uint8_t pkt[] = { 0x7E, 0x98, 0x10, 0x10, 0x00, 0x7E, 0x02, 0x00, 0x00, 0x5F };
+  uint8_t pkt1[] = { 0x7E, 0x98, 0x10, 0x10, 0x00, 0x7E, 0x02, 0x00, 0x00, 0x5F };
-  EXPECT_EQ(checkSportPacket(pkt+1), true);
+  EXPECT_EQ(checkSportPacket(pkt1+1), true);
+  // Packet upstream
+  uint8_t pkt2[] = { 0x7E, 0x1C, 0x31, 0x00, 0x10, 0x85, 0x64, 0x00, 0x00, 0xD4 };
+  EXPECT_EQ(checkSportPacket(pkt2+1), true);
 }
 
 void setSportPacketCrc(uint8_t * packet)