From bd87028fd58798003de4f2057516558e8fcca8ce Mon Sep 17 00:00:00 2001 From: Bertrand Songis Date: Fri, 29 Apr 2016 18:44:25 +0200 Subject: [PATCH] FrSky S.PORT upstream (#3426) FrSky S.PORT upstream --- radio/sdcard/taranis/TEMPLATES/S6R.lua | 183 ++++++++++++++++++ radio/src/lua/api_general.cpp | 46 +++-- .../targets/taranis/bootloader/CMakeLists.txt | 2 +- .../src/targets/taranis/telemetry_driver.cpp | 10 + radio/src/telemetry/frsky.h | 33 +++- radio/src/telemetry/frsky_sport.cpp | 57 ++++-- radio/src/telemetry/telemetry.cpp | 4 +- radio/src/tests/frsky.cpp | 13 +- 8 files changed, 303 insertions(+), 45 deletions(-) create mode 100644 radio/sdcard/taranis/TEMPLATES/S6R.lua diff --git a/radio/sdcard/taranis/TEMPLATES/S6R.lua b/radio/sdcard/taranis/TEMPLATES/S6R.lua new file mode 100644 index 000000000..035db1a3f --- /dev/null +++ b/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 } diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index 7d1a8b8a7..680c6e7c7 100644 --- a/radio/src/lua/api_general.cpp +++ b/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(); + luaInputTelemetryFifo = new Fifo(); if (!luaInputTelemetryFifo) { return 0; } } - LuaTelemetryValue value; - if (luaInputTelemetryFifo->pop(value)) { - lua_pushnumber(L, value.id); - lua_pushunsigned(L, value.value); - return 2; + LuaTelemetryPacket packet; + if (luaInputTelemetryFifo->pop(packet)) { + lua_pushnumber(L, packet.physicalId); + lua_pushnumber(L, packet.primId); + 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) { - luaOutputTelemetryFifo = new Fifo(); - if (!luaOutputTelemetryFifo) { - return 0; - } + if (luaOutputTelemetryPacket.physicalId != 0x7E) { + lua_pushboolean(L, false); + return 1; } - uint8_t id = luaL_checkunsigned(L, 1); - uint32_t value = luaL_checkunsigned(L, 2); + luaOutputTelemetryPacket.physicalId = getDataId(luaL_checkunsigned(L, 1)); + luaOutputTelemetryPacket.primId = luaL_checkunsigned(L, 2); + luaOutputTelemetryPacket.dataId = luaL_checkunsigned(L, 3); + luaOutputTelemetryPacket.value = luaL_checkunsigned(L, 4); -#if defined __GNUC__ - // TODO remove this ifdef when we have updated to MSVC recent version - luaOutputTelemetryFifo->push((LuaTelemetryValue){ id, value }); -#endif - - return 0; + lua_pushboolean(L, true); + return 1; } /*luadoc diff --git a/radio/src/targets/taranis/bootloader/CMakeLists.txt b/radio/src/targets/taranis/bootloader/CMakeLists.txt index f5152390c..86af4bbcd 100644 --- a/radio/src/targets/taranis/bootloader/CMakeLists.txt +++ b/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") diff --git a/radio/src/targets/taranis/telemetry_driver.cpp b/radio/src/targets/taranis/telemetry_driver.cpp index c6a37950c..efaf25754 100644 --- a/radio/src/targets/taranis/telemetry_driver.cpp +++ b/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; } diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index 1aa10394c..a405b3663 100644 --- a/radio/src/telemetry/frsky.h +++ b/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; - uint32_t value; + LuaTelemetryPacket() { }; + 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 * luaInputTelemetryFifo; -extern Fifo * luaOutputTelemetryFifo; +extern Fifo * luaInputTelemetryFifo; +extern LuaTelemetryPacket luaOutputTelemetryPacket; +void sportSendLuaPacket(LuaTelemetryPacket & packet); #endif #endif // _FRSKY_H_ diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index 27d19c87d..342a63741 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/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 prim = packet[1]; - uint16_t id = *((uint16_t *)(packet+2)); + uint8_t physicalId = packet[0] & 0x1F; + uint8_t primId = packet[1]; + 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) { - uint32_t data = SPORT_DATA_S32(packet); - + if (primId == DATA_FRAME) { + 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); diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index ade33dd47..5ead4e842 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -454,6 +454,6 @@ NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm) } #if defined(LUA) -Fifo * luaInputTelemetryFifo = NULL; -Fifo * luaOutputTelemetryFifo = NULL; +Fifo * luaInputTelemetryFifo = NULL; +LuaTelemetryPacket luaOutputTelemetryPacket = { 0x7E, 0x0, 0x0, 0x0 }; #endif diff --git a/radio/src/tests/frsky.cpp b/radio/src/tests/frsky.cpp index 3872e0ad6..d550ef40f 100644 --- a/radio/src/tests/frsky.cpp +++ b/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 }; - uint8_t pkt[] = { 0x7E, 0x98, 0x10, 0x10, 0x00, 0x7E, 0x02, 0x00, 0x00, 0x5F }; - EXPECT_EQ(checkSportPacket(pkt+1), true); + // Packet downstream + uint8_t pkt1[] = { 0x7E, 0x98, 0x10, 0x10, 0x00, 0x7E, 0x02, 0x00, 0x00, 0x5F }; + 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)