1
0
Fork 0
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:
Bertrand Songis 2016-06-18 22:16:50 +02:00 committed by GitHub
parent ab58151f2a
commit 53b51cac94
16 changed files with 289 additions and 178 deletions

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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