Mirror/edgetx
1
0
Fork 0
mirror of https://github.com/EdgeTX/edgetx.git synced 2025-07-25 01:05:08 +03:00
Code Activity

FrSky S.PORT upstream (#3426)

Browse source 
FrSky S.PORT upstream
This commit is contained in:
Bertrand Songis 2016-04-29 18:44:25 +02:00
parent 56f59196ea
commit bd87028fd5
8 changed files with 303 additions and 45 deletions
Download patch file Download diff file Expand all files Collapse all files

183
radio/sdcard/taranis/TEMPLATES/S6R.lua Normal file
View file

@ -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 }

46
radio/src/lua/api_general.cpp
View file

@ -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;
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<LuaTelemetryValue, 16>();
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

2
radio/src/targets/taranis/bootloader/CMakeLists.txt
View file

@ -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")

10
radio/src/targets/taranis/telemetry_driver.cpp
View file

@ -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;
}

29
radio/src/telemetry/frsky.h
View file

@ -419,6 +419,7 @@ void frskyDProcessPacket(uint8_t *packet);
// FrSky S.PORT Protocol
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() { };
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<LuaTelemetryValue, 16> * luaOutputTelemetryFifo;
extern Fifo<LuaTelemetryPacket, 16> * luaInputTelemetryFifo;
extern LuaTelemetryPacket luaOutputTelemetryPacket;
void sportSendLuaPacket(LuaTelemetryPacket & packet);
#endif
#endif // _FRSKY_H_

49
radio/src/telemetry/frsky_sport.cpp
View file

@ -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];
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)
@ -369,8 +404,6 @@ void blankPacket(uint8_t *packet)
memset(packet+2, 0, 6);
}
uint8_t sportUpdatePacket[16] __DMA;
void writePacket(uint8_t * packet)
{
uint8_t * ptr = sportUpdatePacket;

4
radio/src/telemetry/telemetry.cpp
View file

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

11
radio/src/tests/frsky.cpp
View file

@ -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)