Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-16 04:45:17 +03:00
Code Issues Wiki Activity

S.PORT Push reworking

Browse source
This commit is contained in:
Bertrand Songis 2019-03-28 18:43:07 +01:00
parent 0feceff635
commit 95fe5c59bb
12 changed files with 99 additions and 241 deletions
Download patch file Download diff file Expand all files Collapse all files

14
radio/sdcard/FrSky-utilities/FrSky SBEC.lua
View file

@ -55,9 +55,9 @@ end
local settingsFields = {
{"SBEC OUTPUT (V)", VALUE, 0x80, nil, 50, 84 },
--{"Physical ID", VALUE, 0x01, nil, 0, 26 },
--{"Application IDgroup", VALUE, 0x0D, nil, 0, 15 },
--{"Data rate(*100ms)", VALUE, 0x22, nil, 1, 255 },
{"Physical ID", VALUE, 0x01, nil, 0, 26 },
{"Application IDgroup", VALUE, 0x0D, nil, 0, 15 },
{"Data rate(*100ms)", VALUE, 0x22, nil, 1, 255 },
}
-- Change display attribute to current field
@ -156,19 +156,19 @@ local function redrawFieldsPage()
end
local function telemetryRead(fieldx)
return sportTelemetryPush(0x08, 0x30, 0x0e50, fieldx)
return sportTelemetryPush(0, 0x30, 0x0e50, fieldx)
end
local function telemetryIdle(field)
return sportTelemetryPush(0x08,0x21,0x0e50,field)
return sportTelemetryPush(0, 0x21, 0x0e50, field)
end
local function telemetryUnIdle(field)
return sportTelemetryPush(0x08,0x20,0x0e50,field)
return sportTelemetryPush(0, 0x20, 0x0e50, field)
end
local function telemetryWrite(fieldx, valuex)
return sportTelemetryPush(0x08, 0x31, 0x0e50, fieldx + valuex*256)
return sportTelemetryPush(0, 0x31, 0x0e50, fieldx + valuex*256)
end
local telemetryPopTimeout = 0

8
radio/src/io/frsky_sport.cpp
View file

@ -20,12 +20,9 @@
#include "opentx.h"
bool isTelemetryOutputBufferAvailable()
{
return (outputTelemetryBuffer.size == 0 && outputTelemetryBuffer.trigger == 0x7E);
}
#warning "This need to be adapted"
void sportOutputPushByte(uint8_t byte)
/*void sportOutputPushByte(uint8_t byte)
{
if (byte == 0x7E || byte == 0x7D) {
outputTelemetryBuffer.push(0x7D);
@ -52,3 +49,4 @@ void sportOutputPushPacket(SportTelemetryPacket * packet)
outputTelemetryBuffer.setTrigger(packet->raw[0]); // physicalId
outputTelemetryBuffer.setDestination(SPORT_MODULE);
}
*/

3
radio/src/io/frsky_sport.h
View file

@ -34,9 +34,6 @@ PACK(union SportTelemetryPacket
};
uint8_t raw[8];
});
bool isTelemetryOutputBufferAvailable();
void sportOutputPushPacket(SportTelemetryPacket * packet);
#endif
#if defined(STM32)

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

@ -422,15 +422,14 @@ When called without parameters, it will only return the status of the output buf
static int luaSportTelemetryPush(lua_State * L)
{
if (lua_gettop(L) == 0) {
lua_pushboolean(L, isTelemetryOutputBufferAvailable());
lua_pushboolean(L, outputTelemetryBuffer.isAvailable());
}
else if (isTelemetryOutputBufferAvailable()) {
SportTelemetryPacket packet;
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);
else if (outputTelemetryBuffer.isAvailable()) {
outputTelemetryBuffer.sport.physicalId = getDataId(luaL_checkunsigned(L, 1)); // ignored
outputTelemetryBuffer.sport.primId = luaL_checkunsigned(L, 2);
outputTelemetryBuffer.sport.dataId = luaL_checkunsigned(L, 3);
outputTelemetryBuffer.sport.value = luaL_checkunsigned(L, 4);
outputTelemetryBuffer.setDestination(TELEMETRY_ENDPOINT_ANY);
lua_pushboolean(L, true);
}
else {
@ -439,62 +438,6 @@ static int luaSportTelemetryPush(lua_State * L)
return 1;
}
#if defined(PXX2)
/*luadoc
@function pxx2TelemetryPush()
This functions allows for sending SPORT telemetry data toward the receiver,
and more generally, to anything connected SPORT bus on the receiver or transmitter.
When called without parameters, it will only return the status of the output buffer without sending anything.
@param module module index
@param receiver receiver index
@param sensorId physical sensor ID
@param frameId frame ID
@param dataId data ID
@param value value
@retval boolean data queued in output buffer or not.
@status current Introduced in 2.3.0
*/
static int luaPXX2TelemetryPush(lua_State * L)
{
if (lua_gettop(L) == 0) {
lua_pushboolean(L, isTelemetryOutputBufferAvailable());
}
else if (isTelemetryOutputBufferAvailable()) {
SportTelemetryPacket packet;
uint8_t module = luaL_checkunsigned(L, 1);
uint8_t receiver = luaL_checkunsigned(L, 2);
uint8_t rx_uid = g_model.moduleData[module].pxx2.getReceiverSlot(receiver);
if (rx_uid > 0) {
// TODO add more controls (module started, module = PXX2, etc.)
packet.physicalId = getDataId(luaL_checkunsigned(L, 3));
packet.primId = luaL_checkunsigned(L, 4);
packet.dataId = luaL_checkunsigned(L, 5);
packet.value = luaL_checkunsigned(L, 6);
// TODO we could avoid this new copy
pushPXX2TelemetryPacket(module, rx_uid - 1, &packet);
lua_pushboolean(L, true);
}
else {
lua_pushboolean(L, false);
}
}
else {
lua_pushboolean(L, false);
}
return 1;
}
#endif
#if defined(CROSSFIRE)
/*luadoc
@function crossfireTelemetryPop()
@ -1366,10 +1309,6 @@ const luaL_Reg opentxLib[] = {
{ "sportTelemetryPop", luaSportTelemetryPop },
{ "sportTelemetryPush", luaSportTelemetryPush },
{ "setTelemetryValue", luaSetTelemetryValue },
#if defined(PXX2)
{ "sportTelemetryPop", luaSportTelemetryPop },
{ "pxx2TelemetryPush", luaPXX2TelemetryPush },
#endif
#if defined(CROSSFIRE)
{ "crossfireTelemetryPop", luaCrossfireTelemetryPop },
{ "crossfireTelemetryPush", luaCrossfireTelemetryPush },

13
radio/src/pulses/pxx2.cpp
View file

@ -122,28 +122,28 @@ void Pxx2Pulses::setupChannelsFrame(uint8_t module)
void Pxx2Pulses::setupTelemetryFrame(uint8_t module)
{
addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_TELEMETRY);
for (uint8_t i = 0; i < outputTelemetryBuffer.size; i++) {
Pxx2Transport::addByte(outputTelemetryBuffer.destination.rxUid);
for (uint8_t i = 0; i < sizeof(SportTelemetryPacket); i++) {
Pxx2Transport::addByte(outputTelemetryBuffer.data[i]);
}
outputTelemetryBuffer.reset();
}
void Pxx2Pulses::setupHardwareInfoFrame(uint8_t module)
{
if (reusableBuffer.hardwareAndSettings.modules[module].current <= reusableBuffer.hardwareAndSettings.modules[module].maximum) {
if (reusableBuffer.hardwareAndSettings.modules[module].timeout == 0) {
if (reusableBuffer.hardwareAndSettings.modules[module].current <= reusableBuffer.hardwareAndSettings.modules[module].maximum) {
addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_HW_INFO);
Pxx2Transport::addByte(reusableBuffer.hardwareAndSettings.modules[module].current);
reusableBuffer.hardwareAndSettings.modules[module].timeout = 20;
reusableBuffer.hardwareAndSettings.modules[module].current++;
}
else {
reusableBuffer.hardwareAndSettings.modules[module].timeout--;
moduleSettings[module].mode = MODULE_MODE_NORMAL;
setupChannelsFrame(module);
}
}
else {
moduleSettings[module].mode = MODULE_MODE_NORMAL;
reusableBuffer.hardwareAndSettings.modules[module].timeout--;
setupChannelsFrame(module);
}
}
@ -319,8 +319,9 @@ void Pxx2Pulses::setupFrame(uint8_t module)
setupShareMode(module);
break;
default:
if (outputTelemetryBuffer.size > 0 && outputTelemetryBuffer.destination == module) {
if (outputTelemetryBuffer.destination.module == module) {
setupTelemetryFrame(module);
outputTelemetryBuffer.reset();
}
else {
setupChannelsFrame(module);

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

@ -158,8 +158,9 @@ extern "C" void TELEMETRY_USART_IRQHandler(void)
#if defined(LUA)
if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT) {
static uint8_t prevdata;
if (prevdata == 0x7E && outputTelemetryBuffer.size > 0 && data == outputTelemetryBuffer.trigger && outputTelemetryBuffer.destination == SPORT_MODULE) {
sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size);
if (prevdata == 0x7E && outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_SPORT && data == outputTelemetryBuffer.sport.physicalId) {
#warning "bytestuffing + buffer send"
// sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size);
}
prevdata = data;
}

70
radio/src/telemetry/frsky.cpp
View file

@ -20,76 +20,6 @@
#include "opentx.h"
bool checkPXX2PacketCRC(const uint8_t * packet)
{
// TODO ...
return true;
// TRACE("checkPXX2PacketCRC(): checksum error ");
// DUMP(packet, FRSKY_SPORT_PACKET_SIZE);
}
void createFrSkyPXX2Sensor(uint16_t type, uint8_t value){
uint8_t buffer[5] = {0x18, 0x10, 0, 0, 0};
buffer[2] = type & 0xF;
buffer[3] = type >> 8;
buffer[4] = value;
sportProcessTelemetryPacketWithoutCrc(buffer);
}
void processFrskyPXX2Data(uint8_t data)
{
static uint8_t dataState = STATE_DATA_IDLE;
switch (dataState) {
case STATE_DATA_IDLE:
if (data == START_STOP) {
telemetryRxBufferCount = 0;
dataState = STATE_DATA_START;
}
break;
case STATE_DATA_START:
if (telemetryRxBufferCount == 0 && data > 0x0C) {
// wrong length
telemetryRxBufferCount = 0;
dataState = STATE_DATA_IDLE;
}
else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
if (telemetryRxBuffer[0] + 3 /* 1 byte for length, 2 bytes for CRC */ == telemetryRxBufferCount) {
if (checkPXX2PacketCRC(telemetryRxBuffer)) {
R9ModuleStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
uint8_t R9Region = (telemetryRxBuffer[3] & 0x30) >> 4;
if (R9Region != 0x04) {
g_model.moduleData[EXTERNAL_MODULE].r9m.region = R9Region;
}
if (telemetryRxBuffer[2] & 0x80) {
createFrSkyPXX2Sensor(RSSI_ID, telemetryRxBuffer[2] & 0x7f);
}
else {
createFrSkyPXX2Sensor(BATT_ID, telemetryRxBuffer[2] & 0x7f);
}
if((telemetryRxBuffer[3] & 0x04) == 0) {
createFrSkyPXX2Sensor(R9_PWR_ID, (telemetryRxBuffer[3] & 0x03));
}
if (telemetryRxBuffer[0] == 0x0C) {
sportProcessTelemetryPacketWithoutCrc(telemetryRxBuffer + 5 /* LEN, TYPE, RSSI/BAT, TP/SS/FW_T, FW_VER, Data ID */);
}
}
telemetryRxBufferCount = 0;
dataState = STATE_DATA_IDLE;
}
}
else {
// overflow guard
telemetryRxBufferCount = 0;
dataState = STATE_DATA_IDLE;
}
break;
}
}
void processFrskyTelemetryData(uint8_t data)
{
#if defined(PCBSKY9X) && defined(BLUETOOTH)

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

@ -284,7 +284,7 @@ void frskyDProcessPacket(const uint8_t *packet);
// FrSky S.PORT Telemetry Protocol
void sportProcessTelemetryPacket(const uint8_t * packet);
void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet);
void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet);
void telemetryWakeup();
void telemetryReset();
@ -315,6 +315,5 @@ extern TelemetryData telemetryData;
bool pushFrskyTelemetryData(uint8_t data); // returns true when end of frame detected
void processFrskyTelemetryData(uint8_t data);
void processFrskyPXX2Data(uint8_t data);
#endif // _FRSKY_H_

18
radio/src/telemetry/frsky_pxx2.cpp
View file

@ -166,7 +166,10 @@ void processBindFrame(uint8_t module, uint8_t * frame)
void processTelemetryFrame(uint8_t module, uint8_t * frame)
{
sportProcessTelemetryPacketWithoutCrc(&frame[4]);
TelemetryEndpoint origin;
origin.module = module;
origin.rxUid = frame[3] & 0x0F;
sportProcessTelemetryPacketWithoutCrc(origin.value, &frame[4]);
}
void processSpectrumAnalyserFrame(uint8_t module, uint8_t * frame)
@ -260,16 +263,3 @@ void processPXX2Frame(uint8_t module, uint8_t *frame)
break;
}
}
void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet)
{
// Flag0
outputTelemetryBuffer.push(rx_uid);
for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
uint8_t byte = packet->raw[i];
outputTelemetryBuffer.push(byte);
}
outputTelemetryBuffer.setDestination(module);
}

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

@ -147,19 +147,23 @@ void sportProcessTelemetryPacket(const uint8_t * packet)
return;
}
sportProcessTelemetryPacketWithoutCrc(packet);
sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet);
}
void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet)
{
uint8_t physicalId = packet[0] & 0x1F;
uint8_t primId = packet[1];
uint16_t id = *((uint16_t *)(packet+2));
uint16_t dataId = *((uint16_t *)(packet+2));
uint32_t data = SPORT_DATA_S32(packet);
if (primId == DATA_FRAME) {
if (outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_ANY && dataId == outputTelemetryBuffer.sport.dataId) {
outputTelemetryBuffer.sport.physicalId = packet[0];
outputTelemetryBuffer.destination.value = origin;
}
uint8_t instance = physicalId + 1;
if (id == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
if (dataId == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
data = SPORT_DATA_U8(packet);
if (data == 0)
@ -167,17 +171,17 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
else
telemetryData.rssi.set(data);
}
else if (id == R9_PWR_ID) {
else if (dataId == R9_PWR_ID) {
uint32_t r9pwr[] = {100, 200, 500, 1000};
data = r9pwr[SPORT_DATA_U8(packet) & 0x03];
}
else if (id == XJT_VERSION_ID) {
else if (dataId == XJT_VERSION_ID) {
telemetryData.xjtVersion = HUB_DATA_U16(packet);
if (!IS_RAS_VALUE_VALID()) {
telemetryData.swr.set(0x00);
}
}
else if (id == RAS_ID) {
else if (dataId == RAS_ID) {
if (IS_RAS_VALUE_VALID())
telemetryData.swr.set(SPORT_DATA_U8(packet));
else
@ -185,33 +189,33 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
}
if (TELEMETRY_STREAMING()/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) {
if ((id >> 8) == 0) {
if ((dataId >> 8) == 0) {
// The old FrSky IDs
processHubPacket(id, HUB_DATA_U16(packet));
processHubPacket(dataId, HUB_DATA_U16(packet));
}
else if (!IS_HIDDEN_TELEMETRY_VALUE(id)) {
if (id == ADC1_ID || id == ADC2_ID || id == BATT_ID || id == RAS_ID) {
else if (!IS_HIDDEN_TELEMETRY_VALUE(dataId)) {
if (dataId == ADC1_ID || dataId == ADC2_ID || dataId == BATT_ID || dataId == RAS_ID) {
data = SPORT_DATA_U8(packet);
}
if (id >= GPS_LONG_LATI_FIRST_ID && id <= GPS_LONG_LATI_LAST_ID) {
if (dataId >= GPS_LONG_LATI_FIRST_ID && dataId <= GPS_LONG_LATI_LAST_ID) {
int32_t value = (data & 0x3fffffff);
if (data & (1 << 30))
value = -value;
value = (value * 5) / 3; // min/10000 => deg/1000000
if (data & (1 << 31))
sportProcessTelemetryPacket(id, 0, instance, value, UNIT_GPS_LONGITUDE);
sportProcessTelemetryPacket(dataId, 0, instance, value, UNIT_GPS_LONGITUDE);
else
sportProcessTelemetryPacket(id, 0, instance, value, UNIT_GPS_LATITUDE);
sportProcessTelemetryPacket(dataId, 0, instance, value, UNIT_GPS_LATITUDE);
}
else if (id >= RBOX_BATT1_FIRST_ID && id <= RBOX_BATT2_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(id, 1, instance, data >> 16);
else if (dataId >= RBOX_BATT1_FIRST_ID && dataId <= RBOX_BATT2_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
}
else if (id >= RBOX_CNSP_FIRST_ID && id <= RBOX_CNSP_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(id, 1, instance, data >> 16);
else if (dataId >= RBOX_CNSP_FIRST_ID && dataId <= RBOX_CNSP_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
}
else if (id >= RBOX_STATE_FIRST_ID && id <= RBOX_STATE_LAST_ID) {
else if (dataId >= RBOX_STATE_FIRST_ID && dataId <= RBOX_STATE_LAST_ID) {
bool static isRB10 = false;
uint16_t newServosState;
@ -233,31 +237,31 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
}
servosState = newServosState;
rboxState = newRboxState;
sportProcessTelemetryPacket(id, 0, instance, servosState);
sportProcessTelemetryPacket(id, 1, instance, rboxState);
sportProcessTelemetryPacket(dataId, 0, instance, servosState);
sportProcessTelemetryPacket(dataId, 1, instance, rboxState);
}
else if (id >= ESC_POWER_FIRST_ID && id <= ESC_POWER_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(id, 1, instance, data >> 16);
else if (dataId >= ESC_POWER_FIRST_ID && dataId <= ESC_POWER_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0xffff);
sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
}
else if (id >= ESC_RPM_CONS_FIRST_ID && id <= ESC_RPM_CONS_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, 100 * (data & 0xffff));
sportProcessTelemetryPacket(id, 1, instance, data >> 16);
else if (dataId >= ESC_RPM_CONS_FIRST_ID && dataId <= ESC_RPM_CONS_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, 100 * (data & 0xffff));
sportProcessTelemetryPacket(dataId, 1, instance, data >> 16);
}
else if (id >= ESC_TEMPERATURE_FIRST_ID && id <= ESC_TEMPERATURE_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, data & 0x00ff);
else if (dataId >= ESC_TEMPERATURE_FIRST_ID && dataId <= ESC_TEMPERATURE_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, data & 0x00ff);
}
else if (id >= SBEC_POWER_FIRST_ID && id <= SBEC_POWER_LAST_ID) {
sportProcessTelemetryPacket(id, 0, instance, (data & 0xffff) / 10);
sportProcessTelemetryPacket(id, 1, instance, (data >> 16) / 10);
else if (dataId >= SBEC_POWER_FIRST_ID && dataId <= SBEC_POWER_LAST_ID) {
sportProcessTelemetryPacket(dataId, 0, instance, (data & 0xffff) / 10);
sportProcessTelemetryPacket(dataId, 1, instance, (data >> 16) / 10);
}
else if (id >= DIY_STREAM_FIRST_ID && id <= DIY_STREAM_LAST_ID) {
else if (dataId >= DIY_STREAM_FIRST_ID && dataId <= DIY_STREAM_LAST_ID) {
#if defined(LUA)
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) {
SportTelemetryPacket luaPacket;
luaPacket.physicalId = physicalId;
luaPacket.primId = primId;
luaPacket.dataId = id;
luaPacket.dataId = dataId;
luaPacket.value = data;
for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
luaInputTelemetryFifo->push(luaPacket.raw[i]);
@ -266,7 +270,7 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
#endif
}
else {
sportProcessTelemetryPacket(id, 0, instance, data);
sportProcessTelemetryPacket(dataId, 0, instance, data);
}
}
}
@ -277,7 +281,7 @@ void sportProcessTelemetryPacketWithoutCrc(const uint8_t * packet)
SportTelemetryPacket luaPacket;
luaPacket.physicalId = physicalId;
luaPacket.primId = primId;
luaPacket.dataId = id;
luaPacket.dataId = dataId;
luaPacket.value = data;
for (uint8_t i=0; i<sizeof(SportTelemetryPacket); i++) {
luaInputTelemetryFifo->push(luaPacket.raw[i]);

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

@ -63,11 +63,6 @@ void processTelemetryData(uint8_t data)
}
#endif
if (telemetryProtocol == PROTOCOL_TELEMETRY_PXX2) {
processFrskyPXX2Data(data);
return;
}
processFrskyTelemetryData(data);
}

42
radio/src/telemetry/telemetry.h
View file

@ -155,7 +155,17 @@ void logTelemetryWriteByte(uint8_t data);
#define LOG_TELEMETRY_WRITE_BYTE(data)
#endif
#define TELEMETRY_OUTPUT_FIFO_SIZE 20
#define TELEMETRY_ENDPOINT_ANY 0xFF
#define TELEMETRY_ENDPOINT_NONE 0xFE
#define TELEMETRY_ENDPOINT_SPORT 0xFD
union TelemetryEndpoint {
PACK(struct {
uint8_t module:4;
uint8_t rxUid:4;
});
uint8_t value;
};
class OutputTelemetryBuffer {
public:
@ -163,29 +173,24 @@ class OutputTelemetryBuffer {
reset();
}
void push(uint8_t byte) {
data[size++] = byte;
}
void setTrigger(uint8_t byte) {
trigger = byte;
}
void setDestination(uint8_t module) {
destination = module;
void setDestination(uint8_t value) {
destination.value = value;
}
void reset() {
size = 0;
trigger = 0x7E;
destination = 0xFF;
destination.value = TELEMETRY_ENDPOINT_NONE;
}
bool isAvailable() {
return destination.value == TELEMETRY_ENDPOINT_NONE;
}
public:
uint8_t data[TELEMETRY_OUTPUT_FIFO_SIZE];
uint8_t size;
uint8_t trigger;
uint8_t destination;
union {
SportTelemetryPacket sport;
uint8_t data[16];
};
TelemetryEndpoint destination;
};
extern OutputTelemetryBuffer outputTelemetryBuffer __DMA;
@ -202,6 +207,5 @@ extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
#endif
void processPXX2Frame(uint8_t module, uint8_t *frame);
void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet);
#endif // _TELEMETRY_H_