mirror of
https://github.com/opentx/opentx.git
synced 2025-07-15 04:15:26 +03:00
Bsongis/crossfire refactoring (#3613)
* Crossfire refactoring * Crossfire script now displaying fields * Crossfire script now displaying text selection values
This commit is contained in:
parent
ab58151f2a
commit
53b51cac94
16 changed files with 289 additions and 178 deletions
|
@ -57,17 +57,19 @@ local function drawDevicePage(page)
|
||||||
-- end
|
-- end
|
||||||
|
|
||||||
for index = 1, 7, 1 do
|
for index = 1, 7, 1 do
|
||||||
field = page["fields"][pageOffset+index]
|
field = page.fields[pageOffset+index]
|
||||||
if field == nil then
|
if field == nil then
|
||||||
break
|
break
|
||||||
end
|
end
|
||||||
|
|
||||||
if field["name"] == nil then
|
if field.name == nil then
|
||||||
lcd.drawText(0, 1+8*index, "...")
|
lcd.drawText(0, 1+8*index, "...")
|
||||||
else
|
else
|
||||||
attr = current == (pageOffset+index) and ((edit == true and BLINK or 0) + INVERS) or 0
|
attr = current == (pageOffset+index) and ((edit == true and BLINK or 0) + INVERS) or 0
|
||||||
|
lcd.drawText(0, 1+8*index, field.name)
|
||||||
lcd.drawText(0, 1+8*index, field["name"])
|
if field.functions ~= nil then
|
||||||
|
field.functions.display(field, 1+8*index, attr)
|
||||||
|
end
|
||||||
|
|
||||||
-- if field[4] == nil then
|
-- if field[4] == nil then
|
||||||
-- lcd.drawText(COLUMN_2, 1+8*index, "---", attr)
|
-- lcd.drawText(COLUMN_2, 1+8*index, "---", attr)
|
||||||
|
@ -88,19 +90,19 @@ local function createPage(id, name, fields_count)
|
||||||
newpage = {
|
newpage = {
|
||||||
id = id,
|
id = id,
|
||||||
name = name,
|
name = name,
|
||||||
state = 0,
|
pagetimeout = 0,
|
||||||
timeout = 0,
|
fields = {},
|
||||||
fields = {}
|
fieldstimeout = 0,
|
||||||
}
|
}
|
||||||
for i=1, fields_count do
|
for i=1, fields_count do
|
||||||
newpage["fields"][i] = { name=nil }
|
newpage.fields[i] = { name=nil }
|
||||||
end
|
end
|
||||||
return newpage
|
return newpage
|
||||||
end
|
end
|
||||||
|
|
||||||
local function getPage(name)
|
local function getPage(name)
|
||||||
for i=1, #pages do
|
for i=1, #pages do
|
||||||
if pages[i]["name"] == name then
|
if pages[i].name == name then
|
||||||
return pages[i]
|
return pages[i]
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
|
@ -118,21 +120,74 @@ local function parseDeviceInfoMessage(data)
|
||||||
fields_count = data[i+13]
|
fields_count = data[i+13]
|
||||||
pg = getPage(name)
|
pg = getPage(name)
|
||||||
if pg == nil then
|
if pg == nil then
|
||||||
pg = createPage(id, name, fields_count)
|
pg = createPage(id, name, fields_count-1) -- TODO fields_count should be different
|
||||||
pages[#pages + 1] = pg
|
pages[#pages + 1] = pg
|
||||||
end
|
end
|
||||||
pg["timeout"] = time + 3000 -- 30s
|
pg.pagetimeout = time + 3000 -- 30s
|
||||||
end
|
end
|
||||||
|
|
||||||
|
function split(inputstr)
|
||||||
|
local t={}; i=1
|
||||||
|
for str in string.gmatch(inputstr, "([^;]+)") do
|
||||||
|
t[i] = str
|
||||||
|
i = i + 1
|
||||||
|
end
|
||||||
|
return t
|
||||||
|
end
|
||||||
|
|
||||||
|
local function fieldTextSelectionLoad(field, data, offset)
|
||||||
|
values = ""
|
||||||
|
while data[offset] ~= 0 do
|
||||||
|
values = values .. string.char(data[offset])
|
||||||
|
offset = offset + 1
|
||||||
|
end
|
||||||
|
field.values = split(values)
|
||||||
|
offset = offset + 1
|
||||||
|
field.value = data[offset]
|
||||||
|
end
|
||||||
|
|
||||||
|
local function fieldTextSelectionDisplay(field, y, attr)
|
||||||
|
lcd.drawText(COLUMN_2, y, field.values[field.value+1], attr)
|
||||||
|
end
|
||||||
|
|
||||||
|
local types_functions = {
|
||||||
|
nil,
|
||||||
|
nil,
|
||||||
|
nil,
|
||||||
|
nil,
|
||||||
|
nil,
|
||||||
|
nil,
|
||||||
|
nil,
|
||||||
|
nil,
|
||||||
|
nil,
|
||||||
|
{ load=fieldTextSelectionLoad, display=fieldTextSelectionDisplay },
|
||||||
|
nil,
|
||||||
|
nil
|
||||||
|
}
|
||||||
|
|
||||||
local function parseParameterInfoMessage(data)
|
local function parseParameterInfoMessage(data)
|
||||||
index = data[3]
|
index = data[3]
|
||||||
|
field = pages[page].fields[index]
|
||||||
|
field.parent = data[4]
|
||||||
|
field.type = data[5]
|
||||||
|
field.functions = types_functions[field.type+1]
|
||||||
|
parent = field.parent
|
||||||
name = ""
|
name = ""
|
||||||
|
while parent ~= 0 do
|
||||||
|
name = name .. " "
|
||||||
|
parent = pages[page].fields[parent].parent
|
||||||
|
end
|
||||||
i = 6
|
i = 6
|
||||||
while data[i] ~= 0 do
|
while data[i] ~= 0 do
|
||||||
name = name .. string.char(data[i])
|
name = name .. string.char(data[i])
|
||||||
i = i + 1
|
i = i + 1
|
||||||
end
|
end
|
||||||
pages[page]["fields"][index+1]["name"] = name
|
i = i + 1
|
||||||
|
field.name = name
|
||||||
|
if field.functions ~= nil then
|
||||||
|
field.functions.load(field, data, i)
|
||||||
|
end
|
||||||
|
pages[page].fieldstimeout = 0
|
||||||
end
|
end
|
||||||
|
|
||||||
local telemetryPopTimeout = 0
|
local telemetryPopTimeout = 0
|
||||||
|
@ -148,9 +203,14 @@ local function refreshNext()
|
||||||
devicesRefreshTimeout = time + 1000 -- 10s
|
devicesRefreshTimeout = time + 1000 -- 10s
|
||||||
end
|
end
|
||||||
crossfireTelemetryPush(0x28, { 0x00, 0xEA })
|
crossfireTelemetryPush(0x28, { 0x00, 0xEA })
|
||||||
elseif page <= #pages and pages[page]["state"] == 0 and time > pages[page]["timeout"] then
|
elseif page <= #pages and time > pages[page].fieldstimeout then
|
||||||
crossfireTelemetryPush(0x2A, { pages[page]["id"], 0xEA })
|
for i=1, #pages[page].fields do
|
||||||
pages[page]["timeout"] = time + 100 -- 1s
|
field = pages[page].fields[i]
|
||||||
|
if field.name == nil then
|
||||||
|
crossfireTelemetryPush(0x2C, { pages[page].id, 0xEA, i })
|
||||||
|
pages[page].fieldstimeout = time + 200 -- 2s
|
||||||
|
end
|
||||||
|
end
|
||||||
end
|
end
|
||||||
elseif command == 0x29 then
|
elseif command == 0x29 then
|
||||||
parseDeviceInfoMessage(data)
|
parseDeviceInfoMessage(data)
|
||||||
|
@ -260,7 +320,7 @@ end
|
||||||
|
|
||||||
local function runDevicePage(index, event)
|
local function runDevicePage(index, event)
|
||||||
lcd.clear()
|
lcd.clear()
|
||||||
lcd.drawScreenTitle(pages[index]["name"], index, #pages)
|
lcd.drawScreenTitle(pages[index].name, index, #pages)
|
||||||
drawDevicePage(pages[index])
|
drawDevicePage(pages[index])
|
||||||
return 0
|
return 0
|
||||||
end
|
end
|
||||||
|
|
|
@ -73,6 +73,22 @@ class Fifo
|
||||||
while (!isEmpty()) {};
|
while (!isEmpty()) {};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint32_t size()
|
||||||
|
{
|
||||||
|
return (N + widx - ridx) & (N-1);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool probe(T & element)
|
||||||
|
{
|
||||||
|
if (isEmpty()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
element = fifo[ridx];
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
T fifo[N];
|
T fifo[N];
|
||||||
volatile uint32_t widx;
|
volatile uint32_t widx;
|
||||||
|
|
|
@ -299,18 +299,21 @@ bool luaFindFieldByName(const char * name, LuaField & field, unsigned int flags)
|
||||||
static int luaSportTelemetryPop(lua_State * L)
|
static int luaSportTelemetryPop(lua_State * L)
|
||||||
{
|
{
|
||||||
if (!luaInputTelemetryFifo) {
|
if (!luaInputTelemetryFifo) {
|
||||||
luaInputTelemetryFifo = new Fifo<LuaTelemetryPacket, LUA_TELEMETRY_FIFO_SIZE>();
|
luaInputTelemetryFifo = new Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE>();
|
||||||
if (!luaInputTelemetryFifo) {
|
if (!luaInputTelemetryFifo) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
LuaTelemetryPacket packet;
|
if (luaInputTelemetryFifo->size() >= sizeof(SportTelemetryPacket)) {
|
||||||
if (luaInputTelemetryFifo->pop(packet)) {
|
SportTelemetryPacket packet;
|
||||||
lua_pushnumber(L, packet.sport.physicalId);
|
for (uint8_t i=0; i<sizeof(packet); i++) {
|
||||||
lua_pushnumber(L, packet.sport.primId);
|
luaInputTelemetryFifo->pop(packet.raw[i]);
|
||||||
lua_pushnumber(L, packet.sport.dataId);
|
}
|
||||||
lua_pushunsigned(L, packet.sport.value);
|
lua_pushnumber(L, packet.physicalId);
|
||||||
|
lua_pushnumber(L, packet.primId);
|
||||||
|
lua_pushnumber(L, packet.dataId);
|
||||||
|
lua_pushunsigned(L, packet.value);
|
||||||
return 4;
|
return 4;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -329,36 +332,41 @@ uint8_t getDataId(uint8_t physicalId)
|
||||||
|
|
||||||
static int luaSportTelemetryPush(lua_State * L)
|
static int luaSportTelemetryPush(lua_State * L)
|
||||||
{
|
{
|
||||||
if (luaOutputTelemetryPacket.sport.physicalId != 0x7E) {
|
if (isSportOutputBufferAvailable()) {
|
||||||
lua_pushboolean(L, false);
|
SportTelemetryPacket packet;
|
||||||
return 1;
|
packet.physicalId = getDataId(luaL_checkunsigned(L, 1));
|
||||||
|
packet.primId = luaL_checkunsigned(L, 2);
|
||||||
|
packet.dataId = luaL_checkunsigned(L, 3);
|
||||||
|
packet.value = luaL_checkunsigned(L, 4);
|
||||||
|
sportOutputPushPacket(packet);
|
||||||
|
lua_pushboolean(L, true);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
lua_pushboolean(L, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
luaOutputTelemetryPacket.sport.physicalId = getDataId(luaL_checkunsigned(L, 1));
|
|
||||||
luaOutputTelemetryPacket.sport.primId = luaL_checkunsigned(L, 2);
|
|
||||||
luaOutputTelemetryPacket.sport.dataId = luaL_checkunsigned(L, 3);
|
|
||||||
luaOutputTelemetryPacket.sport.value = luaL_checkunsigned(L, 4);
|
|
||||||
|
|
||||||
lua_pushboolean(L, true);
|
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int luaCrossfireTelemetryPop(lua_State * L)
|
static int luaCrossfireTelemetryPop(lua_State * L)
|
||||||
{
|
{
|
||||||
if (!luaInputTelemetryFifo) {
|
if (!luaInputTelemetryFifo) {
|
||||||
luaInputTelemetryFifo = new Fifo<LuaTelemetryPacket, LUA_TELEMETRY_FIFO_SIZE>();
|
luaInputTelemetryFifo = new Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE>();
|
||||||
if (!luaInputTelemetryFifo) {
|
if (!luaInputTelemetryFifo) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
LuaTelemetryPacket packet;
|
uint8_t length, data;
|
||||||
if (luaInputTelemetryFifo->pop(packet)) {
|
if (luaInputTelemetryFifo->probe(length) && luaInputTelemetryFifo->size() >= uint32_t(length)) {
|
||||||
lua_pushnumber(L, packet.crossfire.command);
|
// length value includes the length field
|
||||||
|
luaInputTelemetryFifo->pop(length);
|
||||||
|
luaInputTelemetryFifo->pop(data); // command
|
||||||
|
lua_pushnumber(L, data);
|
||||||
lua_newtable(L);
|
lua_newtable(L);
|
||||||
for (int i=0; i<packet.crossfire.length; i++) {
|
for (uint8_t i=1; i<length-1; i++) {
|
||||||
lua_pushinteger(L, i+1);
|
luaInputTelemetryFifo->pop(data);
|
||||||
lua_pushinteger(L, packet.crossfire.data[i]);
|
lua_pushinteger(L, i);
|
||||||
|
lua_pushinteger(L, data);
|
||||||
lua_settable(L, -3);
|
lua_settable(L, -3);
|
||||||
}
|
}
|
||||||
return 2;
|
return 2;
|
||||||
|
@ -369,20 +377,24 @@ static int luaCrossfireTelemetryPop(lua_State * L)
|
||||||
|
|
||||||
static int luaCrossfireTelemetryPush(lua_State * L)
|
static int luaCrossfireTelemetryPush(lua_State * L)
|
||||||
{
|
{
|
||||||
if (luaOutputTelemetryPacket.crossfire.command != 0x00) {
|
if (isCrossfireOutputBufferAvailable()) {
|
||||||
|
uint8_t command = luaL_checkunsigned(L, 1);
|
||||||
|
luaL_checktype(L, 2, LUA_TTABLE);
|
||||||
|
uint8_t length = luaL_len(L, 2);
|
||||||
|
telemetryOutputPushByte(MODULE_ADDRESS);
|
||||||
|
telemetryOutputPushByte(2 + length); // 1(COMMAND) + data length + 1(CRC)
|
||||||
|
telemetryOutputPushByte(command); // COMMAND
|
||||||
|
for (int i=0; i<length; i++) {
|
||||||
|
lua_rawgeti(L, 2, i+1);
|
||||||
|
telemetryOutputPushByte(luaL_checkunsigned(L, -1));
|
||||||
|
}
|
||||||
|
telemetryOutputPushByte(crc8(outputTelemetryBuffer+2, 1 + length));
|
||||||
|
telemetryOutputSetTrigger(command);
|
||||||
|
lua_pushboolean(L, true);
|
||||||
|
}
|
||||||
|
else {
|
||||||
lua_pushboolean(L, false);
|
lua_pushboolean(L, false);
|
||||||
return 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);
|
|
||||||
}
|
|
||||||
luaOutputTelemetryPacket.crossfire.command = luaL_checkunsigned(L, 1);
|
|
||||||
|
|
||||||
lua_pushboolean(L, true);
|
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -2,7 +2,7 @@
|
||||||
* Copyright (C) OpenTX
|
* Copyright (C) OpenTX
|
||||||
*
|
*
|
||||||
* Based on code named
|
* Based on code named
|
||||||
* th9x - http://code.google.com/p/th9x
|
* th9x - http://code.google.com/p/th9x
|
||||||
* er9x - http://code.google.com/p/er9x
|
* er9x - http://code.google.com/p/er9x
|
||||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||||
*
|
*
|
||||||
|
@ -46,19 +46,3 @@ uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses)
|
||||||
*buf++ = crc8(crc_start, 23);
|
*buf++ = crc8(crc_start, 23);
|
||||||
return buf - frame;
|
return buf - frame;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(LUA)
|
|
||||||
uint8_t createCrossfireRequestFrame(uint8_t * frame, LuaTelemetryPacket * telemetryPacket)
|
|
||||||
{
|
|
||||||
uint8_t * buf = frame;
|
|
||||||
*buf++ = MODULE_ADDRESS;
|
|
||||||
*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);
|
|
||||||
return buf - frame;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
|
@ -32,10 +32,6 @@ TrainerPulsesData trainerPulsesData __DMA;
|
||||||
uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses);
|
uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(CROSSFIRE) && defined(LUA)
|
|
||||||
uint8_t createCrossfireRequestFrame(uint8_t * frame, LuaTelemetryPacket * telemetry);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
uint8_t getRequiredProtocol(uint8_t port)
|
uint8_t getRequiredProtocol(uint8_t port)
|
||||||
{
|
{
|
||||||
uint8_t required_protocol;
|
uint8_t required_protocol;
|
||||||
|
@ -187,13 +183,11 @@ void setupPulses(uint8_t port)
|
||||||
uint8_t * crossfire = modulePulsesData[port].crossfire.pulses;
|
uint8_t * crossfire = modulePulsesData[port].crossfire.pulses;
|
||||||
uint8_t len;
|
uint8_t len;
|
||||||
#if defined(LUA)
|
#if defined(LUA)
|
||||||
if (luaOutputTelemetryPacket.crossfire.command) {
|
if (outputTelemetryBufferTrigger != 0x00 && outputTelemetryBufferSize > 0) {
|
||||||
len = createCrossfireRequestFrame(crossfire, &luaOutputTelemetryPacket);
|
memcpy(crossfire, outputTelemetryBuffer, outputTelemetryBufferSize);
|
||||||
luaOutputTelemetryPacket.crossfire.clear();
|
len = outputTelemetryBufferSize;
|
||||||
LOG_TELEMETRY_WRITE_START();
|
outputTelemetryBufferTrigger = 0x00;
|
||||||
for (uint32_t i=0; i<len; i++) {
|
outputTelemetryBufferSize = 0;
|
||||||
LOG_TELEMETRY_WRITE_BYTE(crossfire[i]);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -140,7 +140,7 @@
|
||||||
#define LED_GPIO_PIN GPIO_Pin_5 // PI.05
|
#define LED_GPIO_PIN GPIO_Pin_5 // PI.05
|
||||||
|
|
||||||
// Serial Port (DEBUG)
|
// Serial Port (DEBUG)
|
||||||
#define SERIAL_RCC_AHB1Periph RCC_AHB1Periph_GPIOB
|
#define SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA1)
|
||||||
#define SERIAL_RCC_APB1Periph RCC_APB1Periph_USART3
|
#define SERIAL_RCC_APB1Periph RCC_APB1Periph_USART3
|
||||||
#define SERIAL_GPIO GPIOB
|
#define SERIAL_GPIO GPIOB
|
||||||
#define SERIAL_GPIO_PIN_TX GPIO_Pin_10 // PB.10
|
#define SERIAL_GPIO_PIN_TX GPIO_Pin_10 // PB.10
|
||||||
|
|
|
@ -405,7 +405,12 @@ void checkTrainerSettings(void);
|
||||||
#include "fifo.h"
|
#include "fifo.h"
|
||||||
#include "dmafifo.h"
|
#include "dmafifo.h"
|
||||||
|
|
||||||
|
#if defined(CROSSFIRE)
|
||||||
|
#define TELEMETRY_FIFO_SIZE 128
|
||||||
|
#else
|
||||||
#define TELEMETRY_FIFO_SIZE 64
|
#define TELEMETRY_FIFO_SIZE 64
|
||||||
|
#endif
|
||||||
|
|
||||||
extern Fifo<uint8_t, TELEMETRY_FIFO_SIZE> telemetryFifo;
|
extern Fifo<uint8_t, TELEMETRY_FIFO_SIZE> telemetryFifo;
|
||||||
extern DMAFifo<32> serial2RxFifo;
|
extern DMAFifo<32> serial2RxFifo;
|
||||||
extern Fifo<uint8_t, 32> sbusFifo;
|
extern Fifo<uint8_t, 32> sbusFifo;
|
||||||
|
|
|
@ -396,7 +396,7 @@
|
||||||
#define TRAINER_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1)
|
#define TRAINER_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1)
|
||||||
|
|
||||||
// Serial Port
|
// Serial Port
|
||||||
#define SERIAL_RCC_AHB1Periph RCC_AHB1Periph_GPIOB
|
#define SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA1)
|
||||||
#define SERIAL_RCC_APB1Periph RCC_APB1Periph_USART3
|
#define SERIAL_RCC_APB1Periph RCC_APB1Periph_USART3
|
||||||
#define SERIAL_GPIO GPIOB
|
#define SERIAL_GPIO GPIOB
|
||||||
#define SERIAL_GPIO_PIN_TX GPIO_Pin_10 // PB.10
|
#define SERIAL_GPIO_PIN_TX GPIO_Pin_10 // PB.10
|
||||||
|
|
|
@ -118,6 +118,10 @@ extern "C" void TELEMETRY_DMA_TX_IRQHandler(void)
|
||||||
if (DMA_GetITStatus(TELEMETRY_DMA_Stream_TX, TELEMETRY_DMA_TX_FLAG_TC)) {
|
if (DMA_GetITStatus(TELEMETRY_DMA_Stream_TX, TELEMETRY_DMA_TX_FLAG_TC)) {
|
||||||
DMA_ClearITPendingBit(TELEMETRY_DMA_Stream_TX, TELEMETRY_DMA_TX_FLAG_TC);
|
DMA_ClearITPendingBit(TELEMETRY_DMA_Stream_TX, TELEMETRY_DMA_TX_FLAG_TC);
|
||||||
TELEMETRY_USART->CR1 |= USART_CR1_TCIE;
|
TELEMETRY_USART->CR1 |= USART_CR1_TCIE;
|
||||||
|
if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) {
|
||||||
|
outputTelemetryBufferSize = 0;
|
||||||
|
outputTelemetryBufferTrigger = 0x7E;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -147,9 +151,8 @@ extern "C" void TELEMETRY_USART_IRQHandler(void)
|
||||||
#if defined(LUA)
|
#if defined(LUA)
|
||||||
if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) {
|
if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) {
|
||||||
static uint8_t prevdata;
|
static uint8_t prevdata;
|
||||||
if (prevdata == 0x7E && data == luaOutputTelemetryPacket.sport.physicalId) {
|
if (prevdata == 0x7E && outputTelemetryBufferSize > 0 && data == outputTelemetryBufferTrigger) {
|
||||||
sportSendLuaPacket(luaOutputTelemetryPacket);
|
sportSendBuffer(outputTelemetryBuffer, outputTelemetryBufferSize);
|
||||||
luaOutputTelemetryPacket.sport.clear();
|
|
||||||
}
|
}
|
||||||
prevdata = data;
|
prevdata = data;
|
||||||
}
|
}
|
||||||
|
|
|
@ -196,27 +196,30 @@ void processCrossfireTelemetryFrame()
|
||||||
#if defined(LUA)
|
#if defined(LUA)
|
||||||
default:
|
default:
|
||||||
if (luaInputTelemetryFifo) {
|
if (luaInputTelemetryFifo) {
|
||||||
LuaTelemetryPacket luaPacket;
|
for (uint8_t i=1; i<telemetryRxBufferCount-1; i++) {
|
||||||
luaPacket.crossfire.command = id;
|
// destination address and CRC are skipped
|
||||||
luaPacket.crossfire.length = telemetryRxBuffer[1]-2;
|
luaInputTelemetryFifo->push(telemetryRxBuffer[i]);
|
||||||
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;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool isCrossfireOutputBufferAvailable()
|
||||||
|
{
|
||||||
|
return outputTelemetryBufferSize == 0;
|
||||||
|
}
|
||||||
|
|
||||||
void processCrossfireTelemetryData(uint8_t data)
|
void processCrossfireTelemetryData(uint8_t data)
|
||||||
{
|
{
|
||||||
if (telemetryRxBufferCount == 0 && data != 0x00) {
|
if (telemetryRxBufferCount == 0 && data != RADIO_ADDRESS) {
|
||||||
|
TRACE("processCrossfirePacket(): address 0x%02X error", data);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (telemetryRxBufferCount == 1 && (data < 2 || data > TELEMETRY_RX_PACKET_SIZE-2)) {
|
if (telemetryRxBufferCount == 1 && (data < 2 || data > TELEMETRY_RX_PACKET_SIZE-2)) {
|
||||||
|
TRACE("processCrossfirePacket(): length 0x%02X error", data);
|
||||||
telemetryRxBufferCount = 0;
|
telemetryRxBufferCount = 0;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -225,8 +228,7 @@ void processCrossfireTelemetryData(uint8_t data)
|
||||||
telemetryRxBuffer[telemetryRxBufferCount++] = data;
|
telemetryRxBuffer[telemetryRxBufferCount++] = data;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
TRACE("processCrossfirePacket(): length error ");
|
TRACE("processCrossfirePacket(): array size error, %d", telemetryRxBufferCount);
|
||||||
DUMP(telemetryRxBuffer, TELEMETRY_RX_PACKET_SIZE);
|
|
||||||
telemetryRxBufferCount = 0;
|
telemetryRxBufferCount = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -23,6 +23,7 @@
|
||||||
|
|
||||||
// Device address
|
// Device address
|
||||||
#define BROADCAST_ADDRESS 0x00
|
#define BROADCAST_ADDRESS 0x00
|
||||||
|
#define RADIO_ADDRESS 0xEA
|
||||||
#define MODULE_ADDRESS 0xEE
|
#define MODULE_ADDRESS 0xEE
|
||||||
|
|
||||||
// Frame id
|
// Frame id
|
||||||
|
@ -38,5 +39,6 @@
|
||||||
|
|
||||||
void processCrossfireTelemetryData(uint8_t data);
|
void processCrossfireTelemetryData(uint8_t data);
|
||||||
void crossfireSetDefault(int index, uint8_t id, uint8_t subId);
|
void crossfireSetDefault(int index, uint8_t id, uint8_t subId);
|
||||||
|
bool isCrossfireOutputBufferAvailable();
|
||||||
|
|
||||||
#endif // _CROSSFIRE_H_
|
#endif // _CROSSFIRE_H_
|
||||||
|
|
|
@ -484,49 +484,18 @@ void frskyUpdateCells();
|
||||||
|
|
||||||
void processFrskyTelemetryData(uint8_t data);
|
void processFrskyTelemetryData(uint8_t data);
|
||||||
|
|
||||||
#if defined(LUA)
|
PACK(union SportTelemetryPacket
|
||||||
struct LuaTelemetryPacket
|
|
||||||
{
|
{
|
||||||
LuaTelemetryPacket()
|
struct {
|
||||||
{
|
uint8_t physicalId;
|
||||||
}
|
uint8_t primId;
|
||||||
|
uint16_t dataId;
|
||||||
union {
|
uint32_t value;
|
||||||
struct {
|
|
||||||
uint8_t physicalId;
|
|
||||||
union {
|
|
||||||
struct {
|
|
||||||
uint8_t primId;
|
|
||||||
uint16_t dataId;
|
|
||||||
uint32_t value;
|
|
||||||
uint8_t crc;
|
|
||||||
};
|
|
||||||
uint8_t raw[8];
|
|
||||||
};
|
|
||||||
void clear()
|
|
||||||
{
|
|
||||||
physicalId = 0x7E;
|
|
||||||
}
|
|
||||||
} sport;
|
|
||||||
|
|
||||||
#if defined(CROSSFIRE)
|
|
||||||
struct {
|
|
||||||
uint8_t command;
|
|
||||||
uint8_t length;
|
|
||||||
uint8_t data[32];
|
|
||||||
void clear()
|
|
||||||
{
|
|
||||||
command = 0x00;
|
|
||||||
}
|
|
||||||
} crossfire;
|
|
||||||
#endif
|
|
||||||
};
|
};
|
||||||
};
|
uint8_t raw[8];
|
||||||
|
});
|
||||||
|
|
||||||
#define LUA_TELEMETRY_FIFO_SIZE 8
|
bool isSportOutputBufferAvailable();
|
||||||
extern Fifo<LuaTelemetryPacket, LUA_TELEMETRY_FIFO_SIZE> * luaInputTelemetryFifo;
|
void sportOutputPushPacket(SportTelemetryPacket & packet);
|
||||||
extern LuaTelemetryPacket luaOutputTelemetryPacket;
|
|
||||||
void sportSendLuaPacket(LuaTelemetryPacket & packet);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif // _FRSKY_H_
|
#endif // _FRSKY_H_
|
||||||
|
|
|
@ -189,33 +189,38 @@ void processSportPacket(uint16_t id, uint8_t subId, uint8_t instance, uint32_t d
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t sportUpdatePacket[16] __DMA;
|
void sportOutputPushByte(uint8_t byte)
|
||||||
|
|
||||||
#if defined(LUA)
|
|
||||||
// TODO merge it with S.PORT update function when finished
|
|
||||||
void sportSendLuaPacket(LuaTelemetryPacket & packet)
|
|
||||||
{
|
{
|
||||||
uint8_t * ptr = sportUpdatePacket;
|
if (byte == 0x7E || byte == 0x7D) {
|
||||||
// *ptr++ = 0x7E;
|
telemetryOutputPushByte(0x7D);
|
||||||
// *ptr++ = 0x1A;
|
telemetryOutputPushByte(0x20 ^ byte);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
telemetryOutputPushByte(byte);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isSportOutputBufferAvailable()
|
||||||
|
{
|
||||||
|
return (outputTelemetryBufferSize == 0 && outputTelemetryBufferTrigger == 0x7E);
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO merge it with S.PORT update function when finished
|
||||||
|
void sportOutputPushPacket(SportTelemetryPacket & packet)
|
||||||
|
{
|
||||||
uint16_t crc = 0;
|
uint16_t crc = 0;
|
||||||
for (uint8_t i=0; i<7; i++) {
|
|
||||||
if (packet.sport.raw[i] == 0x7E || packet.sport.raw[i] == 0x7D) {
|
for (uint8_t i=1; i<sizeof(packet); i++) {
|
||||||
*ptr++ = 0x7D;
|
uint8_t byte = packet.raw[i];
|
||||||
*ptr++ = 0x20 ^ packet.sport.raw[i];
|
sportOutputPushByte(byte);
|
||||||
}
|
crc += byte; // 0-1FF
|
||||||
else {
|
crc += crc >> 8; // 0-100
|
||||||
*ptr++ = packet.sport.raw[i];
|
|
||||||
}
|
|
||||||
crc += packet.sport.raw[i]; //0-1FF
|
|
||||||
crc += crc >> 8; //0-100
|
|
||||||
crc &= 0x00ff;
|
crc &= 0x00ff;
|
||||||
}
|
}
|
||||||
*ptr++ = 0xFF-crc;
|
|
||||||
sportSendBuffer(sportUpdatePacket, ptr-sportUpdatePacket);
|
telemetryOutputPushByte(0xFF-crc);
|
||||||
|
telemetryOutputSetTrigger(packet.raw[0]); // physicalId
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
void processSportPacket(uint8_t * packet)
|
void processSportPacket(uint8_t * packet)
|
||||||
{
|
{
|
||||||
|
@ -313,12 +318,14 @@ void processSportPacket(uint8_t * packet)
|
||||||
else if (id >= DIY_FIRST_ID && id <= DIY_LAST_ID) {
|
else if (id >= DIY_FIRST_ID && id <= DIY_LAST_ID) {
|
||||||
#if defined(LUA)
|
#if defined(LUA)
|
||||||
if (luaInputTelemetryFifo) {
|
if (luaInputTelemetryFifo) {
|
||||||
LuaTelemetryPacket luaPacket;
|
SportTelemetryPacket luaPacket;
|
||||||
luaPacket.sport.physicalId = physicalId;
|
luaPacket.physicalId = physicalId;
|
||||||
luaPacket.sport.primId = primId;
|
luaPacket.primId = primId;
|
||||||
luaPacket.sport.dataId = id;
|
luaPacket.dataId = id;
|
||||||
luaPacket.sport.value = data;
|
luaPacket.value = data;
|
||||||
luaInputTelemetryFifo->push(luaPacket);
|
for (uint8_t i=0; i<sizeof(luaPacket); i++) {
|
||||||
|
luaInputTelemetryFifo->push(luaPacket.raw[i]);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
@ -331,12 +338,14 @@ void processSportPacket(uint8_t * packet)
|
||||||
#if defined(LUA)
|
#if defined(LUA)
|
||||||
else if (primId == 0x32) {
|
else if (primId == 0x32) {
|
||||||
if (luaInputTelemetryFifo) {
|
if (luaInputTelemetryFifo) {
|
||||||
LuaTelemetryPacket luaPacket;
|
SportTelemetryPacket luaPacket;
|
||||||
luaPacket.sport.physicalId = physicalId;
|
luaPacket.physicalId = physicalId;
|
||||||
luaPacket.sport.primId = primId;
|
luaPacket.primId = primId;
|
||||||
luaPacket.sport.dataId = id;
|
luaPacket.dataId = id;
|
||||||
luaPacket.sport.value = data;
|
luaPacket.value = data;
|
||||||
luaInputTelemetryFifo->push(luaPacket);
|
for (uint8_t i=0; i<sizeof(luaPacket); i++) {
|
||||||
|
luaInputTelemetryFifo->push(luaPacket.raw[i]);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -417,9 +426,10 @@ void blankPacket(uint8_t * packet)
|
||||||
memset(packet+2, 0, 6);
|
memset(packet+2, 0, 6);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// TODO merge this function
|
||||||
void writePacket(uint8_t * packet)
|
void writePacket(uint8_t * packet)
|
||||||
{
|
{
|
||||||
uint8_t * ptr = sportUpdatePacket;
|
uint8_t * ptr = outputTelemetryBuffer;
|
||||||
*ptr++ = 0x7E;
|
*ptr++ = 0x7E;
|
||||||
*ptr++ = 0xFF;
|
*ptr++ = 0xFF;
|
||||||
packet[7] = crc16(packet, 7);
|
packet[7] = crc16(packet, 7);
|
||||||
|
@ -432,7 +442,7 @@ void writePacket(uint8_t * packet)
|
||||||
*ptr++ = packet[i];
|
*ptr++ = packet[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
sportSendBuffer(sportUpdatePacket, ptr-sportUpdatePacket);
|
sportSendBuffer(outputTelemetryBuffer, ptr-outputTelemetryBuffer);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool sportUpdatePowerOn(ModuleIndex module)
|
bool sportUpdatePowerOn(ModuleIndex module)
|
||||||
|
|
|
@ -410,7 +410,8 @@ void telemetryInit(uint8_t protocol)
|
||||||
else if (protocol == PROTOCOL_PULSES_CROSSFIRE) {
|
else if (protocol == PROTOCOL_PULSES_CROSSFIRE) {
|
||||||
telemetryPortInit(CROSSFIRE_BAUDRATE);
|
telemetryPortInit(CROSSFIRE_BAUDRATE);
|
||||||
#if defined(LUA)
|
#if defined(LUA)
|
||||||
luaOutputTelemetryPacket.crossfire.clear();
|
outputTelemetryBufferSize = 0;
|
||||||
|
outputTelemetryBufferTrigger = 0;
|
||||||
#endif
|
#endif
|
||||||
telemetryPortSetDirectionOutput();
|
telemetryPortSetDirectionOutput();
|
||||||
}
|
}
|
||||||
|
@ -422,7 +423,8 @@ void telemetryInit(uint8_t protocol)
|
||||||
else {
|
else {
|
||||||
telemetryPortInit(FRSKY_SPORT_BAUDRATE);
|
telemetryPortInit(FRSKY_SPORT_BAUDRATE);
|
||||||
#if defined(LUA)
|
#if defined(LUA)
|
||||||
luaOutputTelemetryPacket.sport.clear();
|
outputTelemetryBufferSize = 0;
|
||||||
|
outputTelemetryBufferTrigger = 0x7E;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -467,7 +469,10 @@ void logTelemetryWriteByte(uint8_t data)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
uint8_t outputTelemetryBuffer[TELEMETRY_OUTPUT_FIFO_SIZE] __DMA;
|
||||||
|
uint8_t outputTelemetryBufferSize = 0;
|
||||||
|
uint8_t outputTelemetryBufferTrigger = 0;
|
||||||
|
|
||||||
#if defined(LUA)
|
#if defined(LUA)
|
||||||
Fifo<LuaTelemetryPacket, LUA_TELEMETRY_FIFO_SIZE> * luaInputTelemetryFifo = NULL;
|
Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo = NULL;
|
||||||
LuaTelemetryPacket luaOutputTelemetryPacket;
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -62,7 +62,7 @@ extern uint8_t telemetryState;
|
||||||
#define TELEMETRY_TIMEOUT10ms 100 // 1 second
|
#define TELEMETRY_TIMEOUT10ms 100 // 1 second
|
||||||
|
|
||||||
#if defined(CROSSFIRE)
|
#if defined(CROSSFIRE)
|
||||||
#define TELEMETRY_RX_PACKET_SIZE 40
|
#define TELEMETRY_RX_PACKET_SIZE 64
|
||||||
#else
|
#else
|
||||||
#define TELEMETRY_RX_PACKET_SIZE 19 // 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
|
#define TELEMETRY_RX_PACKET_SIZE 19 // 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
|
||||||
#endif
|
#endif
|
||||||
|
@ -158,4 +158,24 @@ void logTelemetryWriteByte(uint8_t data);
|
||||||
#define LOG_TELEMETRY_WRITE_BYTE(data)
|
#define LOG_TELEMETRY_WRITE_BYTE(data)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define TELEMETRY_OUTPUT_FIFO_SIZE 16
|
||||||
|
extern uint8_t outputTelemetryBuffer[TELEMETRY_OUTPUT_FIFO_SIZE] __DMA;
|
||||||
|
extern uint8_t outputTelemetryBufferSize;
|
||||||
|
extern uint8_t outputTelemetryBufferTrigger;
|
||||||
|
|
||||||
|
inline void telemetryOutputPushByte(uint8_t byte)
|
||||||
|
{
|
||||||
|
outputTelemetryBuffer[outputTelemetryBufferSize++] = byte;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void telemetryOutputSetTrigger(uint8_t byte)
|
||||||
|
{
|
||||||
|
outputTelemetryBufferTrigger = byte;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if defined(LUA)
|
||||||
|
#define LUA_TELEMETRY_INPUT_FIFO_SIZE 256
|
||||||
|
extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
|
||||||
|
#endif
|
||||||
|
|
||||||
#endif // _TELEMETRY_H_
|
#endif // _TELEMETRY_H_
|
||||||
|
|
|
@ -10,6 +10,21 @@ import sys
|
||||||
lineNumber = 0
|
lineNumber = 0
|
||||||
crossfireDataBuff = []
|
crossfireDataBuff = []
|
||||||
|
|
||||||
|
crossfire_types = [
|
||||||
|
"UINT8",
|
||||||
|
"INT8",
|
||||||
|
"UINT16",
|
||||||
|
"INT16",
|
||||||
|
"UINT32",
|
||||||
|
"INT32",
|
||||||
|
"UINT64",
|
||||||
|
"INT64",
|
||||||
|
"FLOAT",
|
||||||
|
"TEXT_SELECTION",
|
||||||
|
"STRING",
|
||||||
|
"FOLDER"
|
||||||
|
]
|
||||||
|
|
||||||
def dump(data, maxLen=None):
|
def dump(data, maxLen=None):
|
||||||
if maxLen and len(data) > maxLen:
|
if maxLen and len(data) > maxLen:
|
||||||
data = data[:maxLen]
|
data = data[:maxLen]
|
||||||
|
@ -43,10 +58,22 @@ def ParsePingDevices(_):
|
||||||
return '[Ping Devices]'
|
return '[Ping Devices]'
|
||||||
|
|
||||||
def ParseDevice(payload):
|
def ParseDevice(payload):
|
||||||
return '[Device] "%s" %d parameters' % ("".join([chr(c) for c in payload[2:-14]]), payload[-1])
|
return '[Device] 0x%02x "%s" %d parameters' % (payload[1], "".join([chr(c) for c in payload[2:-14]]), payload[-1])
|
||||||
|
|
||||||
def ParseFieldsRequest(payload):
|
def ParseFieldsRequest(payload):
|
||||||
return '[Fields request] Device=0x%02x' % payload[0]
|
return '[Fields request]'
|
||||||
|
|
||||||
|
def ParseFieldRequest(payload):
|
||||||
|
return '[Field request] device=0x%02x field=%d' % (payload[1], payload[2])
|
||||||
|
|
||||||
|
def ParseField(payload):
|
||||||
|
name = ""
|
||||||
|
i = 5
|
||||||
|
while payload[i] != 0:
|
||||||
|
name += chr(payload[i])
|
||||||
|
i += 1
|
||||||
|
i += 1
|
||||||
|
return '[Field] %s device=0x%02x field=%d parent=%d type=%s' % (name, payload[1], payload[2], payload[3], crossfire_types[payload[4]])
|
||||||
|
|
||||||
parsers = (
|
parsers = (
|
||||||
(0x02, ParseGPS),
|
(0x02, ParseGPS),
|
||||||
|
@ -57,6 +84,8 @@ parsers = (
|
||||||
(0x28, ParsePingDevices),
|
(0x28, ParsePingDevices),
|
||||||
(0x29, ParseDevice),
|
(0x29, ParseDevice),
|
||||||
(0x2a, ParseFieldsRequest),
|
(0x2a, ParseFieldsRequest),
|
||||||
|
(0x2b, ParseField),
|
||||||
|
(0x2c, ParseFieldRequest),
|
||||||
)
|
)
|
||||||
|
|
||||||
def ParsePacket(packet):
|
def ParsePacket(packet):
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue