piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Moved telemetry processing to module interface

Browse Source
This commit is contained in:
Raphael Coeffic
2022-08-11 12:15:46 +02:00
committed by Raphael Coeffic
parent 39c61408e7
commit 246840f065
17 changed files with 525 additions and 601 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/hal/module_driver.h
+3 -1
View File
@@ -65,5 +65,7 @@ struct etx_module_driver_t {
// Fetch telemetry byte
int (*getByte)(void* context, uint8_t* data);
};
// Process input data byte (telemetry)
void (*processData)(void* context, uint8_t data, uint8_t* buffer, uint8_t* len);
};
radio/src/pulses/afhds2.cpp
+6 -6
View File
@@ -29,14 +29,8 @@
#include "pulses/pulses.h"
#include "telemetry/telemetry.h"
afhds2::afhds2() {
// TODO Auto-generated constructor stub
}
afhds2::~afhds2() {
// TODO Auto-generated destructor stub
}
const etx_serial_init afhds2SerialInitParams = {
.baudrate = INTMODULE_USART_AFHDS2_BAUDRATE,
@@ -98,6 +92,11 @@ static int afhds2GetByte(void* context, uint8_t* data)
return IntmoduleSerialDriver.getByte(context, data);
}
static void afhds2ProcessData(void*, uint8_t data, uint8_t* buffer, uint8_t* len)
{
processInternalFlySkyTelemetryData(data, buffer, len);
}
const etx_module_driver_t Afhds2InternalDriver = {
.protocol = PROTOCOL_CHANNELS_AFHDS2A,
.init = afhds2Init,
@@ -105,4 +104,5 @@ const etx_module_driver_t Afhds2InternalDriver = {
.setupPulses = afhds2SetupPulses,
.sendPulses = afhds2SendPulses,
.getByte = afhds2GetByte,
.processData = afhds2ProcessData,
};
radio/src/pulses/afhds2.h
-16
View File
@@ -40,24 +40,8 @@ struct FlySkySerialPulsesData {
uint8_t state;
uint8_t timeout;
uint8_t esc_state;
uint8_t telemetry[64];
uint8_t telemetry_index;
} __attribute__((__packed__));
enum AfhdsSpecialChars {
END = 0xC0, //Frame end
START = END,
ESC_END = 0xDC, //Escaped frame end - in case END occurs in fame then ESC ESC_END must be used
ESC = 0xDB, //Escaping character
ESC_ESC = 0xDD, //Escaping character in case ESC occurs in fame then ESC ESC_ESC must be used
};
class afhds2 {
public:
afhds2();
virtual ~afhds2();
};
extern const etx_serial_init afhds2SerialInitParams;
extern const etx_module_driver_t Afhds2InternalDriver;
radio/src/pulses/afhds3.cpp
+86 -136
View File
@@ -320,16 +320,23 @@ class ProtoState
* @param moduleIndex index of module one of INTERNAL_MODULE, EXTERNAL_MODULE
* @param resetFrameCount flag if current frame count should be reseted
*/
void init(uint8_t moduleIndex, bool resetFrameCount = true);
void init(uint8_t moduleIndex, void* buffer, const etx_serial_driver_t* drv);
void deinit() { trsp.deinit(); }
/**
* Fills DMA buffers with frame to be send depending on actual state
*/
* Fills DMA buffers with frame to be send depending on actual state
*/
void setupFrame();
uint8_t* getBuffer();
uint32_t getBufferSize();
/**
* Sends prepared buffers
*/
void sendFrame() { trsp.sendBuffer(); }
/**
* Fetch telemetry byte
*/
int getTelemetryByte(uint8_t* data) { return trsp.getTelemetryByte(data); }
/**
* Gets actual module status into provided buffer
@@ -347,6 +354,10 @@ class ProtoState
void setConfigFromModel();
private:
//friendship declaration - use for passing telemetry
friend void processTelemetryData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len);
void processTelemetryData(uint8_t data, uint8_t* buffer, uint8_t* len);
void parseData(uint8_t* rxBuffer, uint8_t rxBufferCount);
@@ -364,12 +375,6 @@ class ProtoState
void clearFrameData();
void processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
uint8_t& rxBufferCount, uint8_t maxSize);
//friendship declaration - use for passing telemetry
friend void processTelemetryData(uint8_t data, uint8_t module);
bool isConnected();
bool hasTelemetry();
@@ -385,6 +390,8 @@ class ProtoState
*/
ModuleState state;
bool modelIDSet;
/**
* Command count used for counting actual number of commands sent in run mode
*/
@@ -444,14 +451,10 @@ void getStatusString(uint8_t module, char* buffer)
}
//friends function that can access telemetry parsing method
void processTelemetryData(uint8_t data, uint8_t module)
void processTelemetryData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len)
{
uint8_t* rxBuffer = getTelemetryRxBuffer(module);
uint8_t& rxBufferCount = getTelemetryRxBufferCount(module);
uint8_t maxSize = TELEMETRY_RX_PACKET_SIZE;
protoState[module].processTelemetryData(data, rxBuffer, rxBufferCount,
maxSize);
auto p_state = (ProtoState*)ctx;
p_state->processTelemetryData(data, buffer, len);
}
void ProtoState::getStatusString(char* buffer) const
@@ -460,14 +463,14 @@ void ProtoState::getStatusString(char* buffer) const
: "Unknown");
}
void ProtoState::processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
uint8_t& rxBufferCount, uint8_t maxSize)
void ProtoState::processTelemetryData(uint8_t byte, uint8_t* buffer, uint8_t* len)
{
if (!trsp.processTelemetryData(byte, rxBuffer, rxBufferCount, maxSize))
uint8_t maxSize = TELEMETRY_RX_PACKET_SIZE;
if (!trsp.processTelemetryData(byte, buffer, *len, maxSize))
return;
parseData(rxBuffer, rxBufferCount);
rxBufferCount = 0;
parseData(buffer, *len);
*len = 0;
}
bool ProtoState::isConnected()
@@ -528,11 +531,23 @@ void ProtoState::setupFrame()
// if module is ready but not started
if (this->state == ModuleState::STATE_READY) {
auto mode = (uint8_t)MODULE_MODE_E::STANDBY;
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
trsp.sendFrame(MODULE_STATE, FRAME_TYPE::REQUEST_GET_DATA);
return;
}
if (!modelIDSet) {
if (this->state != ModuleState::STATE_STANDBY) {
auto mode = (uint8_t)MODULE_MODE_E::STANDBY;
trsp.sendFrame(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, &mode, 1);
return;
} else {
modelIDSet = true;
trsp.sendFrame(COMMAND::MODEL_ID, FRAME_TYPE::REQUEST_SET_EXPECT_DATA,
&g_model.header.modelId[module_index], 1);
return;
}
}
if (this->state == ModuleState::STATE_STANDBY) {
cmdCount = 0;
requestInfoAndRun(true);
@@ -581,35 +596,21 @@ void ProtoState::setupFrame()
}
}
uint8_t* ProtoState::getBuffer()
{
return trsp.getFrameBuffer();
}
uint32_t ProtoState::getBufferSize()
{
return trsp.getFrameSize();
}
void ProtoState::init(uint8_t moduleIndex, bool resetFrameCount)
void ProtoState::init(uint8_t moduleIndex, void* buffer, const etx_serial_driver_t* drv)
{
module_index = moduleIndex;
#if defined(INTERNAL_MODULE_AFHDS3)
if (moduleIndex == INTERNAL_MODULE) {
trsp.init(ByteTransport::Serial, &intmodulePulsesData.afhds3);
} else
#endif
{
#if defined(AFHDS3_EXT_UART)
trsp.init(ByteTransport::Serial, &extmodulePulsesData.afhds3);
#else
trsp.init(ByteTransport::Pulses, &extmodulePulsesData.afhds3);
#endif
// TODO: move bit/byte transport to serial driver
if (drv) {
trsp.init(ByteTransport::Serial, buffer, drv);
} else {
trsp.init(ByteTransport::Pulses, buffer, nullptr);
}
//clear local vars because it is member of union
moduleData = &g_model.moduleData[module_index];
state = ModuleState::STATE_NOT_READY;
modelIDSet = false;
clearFrameData();
}
@@ -649,8 +650,8 @@ void ProtoState::setState(ModuleState state)
void ProtoState::requestInfoAndRun(bool send)
{
// set model ID
trsp.enqueue(COMMAND::MODEL_ID, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
g_model.header.modelId[module_index]);
// trsp.enqueue(COMMAND::MODEL_ID, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
// g_model.header.modelId[module_index]);
// RUN
trsp.enqueue(COMMAND::MODULE_MODE, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, true,
@@ -968,78 +969,49 @@ uint8_t ProtoState::setFailSafe(int16_t* target)
return (uint8_t) (AFHDS3_MAX_CHANNELS);
}
#if defined(AFHDS3_EXT_UART)
static const etx_serial_init extmoduleUsartParams = {
.baudrate = AFHDS3_UART_BAUDRATE,
.parity = ETX_Parity_None,
.stop_bits = ETX_StopBits_One,
.word_length = ETX_WordLength_8,
.rx_enable = true,
};
static void* initExternal(uint8_t module)
{
#if defined(AFHDS3_EXT_UART)
const etx_serial_driver_t* drv = &ExtmoduleSerialDriver;
uint16_t period = AFHDS3_UART_COMMAND_TIMEOUT * 1000 /* us */;
#else
const etx_serial_driver_t* drv = nullptr;
uint16_t period = AFHDS3_SOFTSERIAL_COMMAND_TIMEOUT * 1000 /* us */;
telemetryPortInvertedInit(AFHDS3_SOFTSERIAL_BAUDRATE);
telemetryPortSetDirectionInput();
#endif
auto p_state = &protoState[module];
p_state->init(module, &extmodulePulsesData.afhds3, drv);
p_state->init(module);
void* ctx = ExtmoduleSerialDriver.init(&extmoduleUsartParams);
mixerSchedulerSetPeriod(module, AFHDS3_UART_COMMAND_TIMEOUT * 1000 /* us */);
mixerSchedulerSetPeriod(module, period);
EXTERNAL_MODULE_ON();
return ctx;
return p_state;
}
static void deinitExternal(void* context)
{
EXTERNAL_MODULE_OFF();
mixerSchedulerSetPeriod(EXTERNAL_MODULE, 0);
ExtmoduleSerialDriver.deinit(context);
auto p_state = (ProtoState*)context;
p_state->deinit();
}
static void sendPulsesExternal(void* context)
static void sendPulses(void* context)
{
auto p_state = &protoState[EXTERNAL_MODULE];
ExtmoduleSerialDriver.sendBuffer(context, p_state->getBuffer(), p_state->getBufferSize());
auto p_state = (ProtoState*)context;
p_state->sendFrame();
}
#else // !defined(AFHDS3_USART)
static void* initExternal(uint8_t module)
static void setupPulses(void* context, int16_t* channels, uint8_t nChannels)
{
auto p_state = &protoState[module];
p_state->init(module);
extmoduleSerialStart();
mixerSchedulerSetPeriod(module, AFHDS3_SOFTSERIAL_COMMAND_TIMEOUT * 1000 /* us */);
return nullptr;
}
static void deinitExternal(void* context)
{
(void)context;
extmoduleStop();
mixerSchedulerSetPeriod(EXTERNAL_MODULE, 0);
}
static void sendPulsesExternal(void* context)
{
(void)context;
auto p_state = &protoState[EXTERNAL_MODULE];
extmoduleSendNextFrameSoftSerial(p_state->getBuffer(), p_state->getBufferSize(), false);
}
#endif
static void setupPulsesExternal(void* context, int16_t* channels, uint8_t nChannels)
{
(void)context;
(void)channels;
(void)nChannels;
auto p_state = &protoState[EXTERNAL_MODULE];
auto p_state = (ProtoState*)context;
p_state->setupFrame();
}
@@ -1047,70 +1019,48 @@ etx_module_driver_t externalDriver = {
.protocol = PROTOCOL_CHANNELS_AFHDS3,
.init = initExternal,
.deinit = deinitExternal,
.setupPulses = setupPulsesExternal,
.sendPulses = sendPulsesExternal
.setupPulses = setupPulses,
.sendPulses = sendPulses,
.getByte = nullptr, // TODO
.processData = processTelemetryData,
};
#if defined(INTERNAL_MODULE_AFHDS3)
static const etx_serial_init intmoduleUsartParams = {
.baudrate = AFHDS3_UART_BAUDRATE,
.parity = ETX_Parity_None,
.stop_bits = ETX_StopBits_One,
.word_length = ETX_WordLength_8,
.rx_enable = true,
};
static void* initInternal(uint8_t module)
{
auto p_state = &protoState[module];
p_state->init(module);
void* ctx = IntmoduleSerialDriver.init(&intmoduleUsartParams);
p_state->init(module, &intmodulePulsesData, &IntmoduleSerialDriver);
mixerSchedulerSetPeriod(module, AFHDS3_UART_COMMAND_TIMEOUT * 1000 /* us */);
INTERNAL_MODULE_ON();
return ctx;
return p_state;
}
static void deinitInternal(void* context)
{
(void)context;
INTERNAL_MODULE_OFF();
mixerSchedulerSetPeriod(INTERNAL_MODULE, 0);
intmoduleStop();
}
static void setupPulsesInternal(void* context, int16_t* channels, uint8_t nChannels)
{
(void)context;
(void)channels;
(void)nChannels;
auto p_state = &protoState[INTERNAL_MODULE];
p_state->setupFrame();
}
static void sendPulsesInternal(void* context)
{
auto p_state = &protoState[INTERNAL_MODULE];
IntmoduleSerialDriver.sendBuffer(context, p_state->getBuffer(), p_state->getBufferSize());
auto p_state = (ProtoState*)context;
p_state->deinit();
}
static int getByteInternal(void* context, uint8_t* data)
{
return IntmoduleSerialDriver.getByte(context, data);
auto p_state = (ProtoState*)context;
return p_state->getTelemetryByte(data);
}
etx_module_driver_t internalDriver = {
.protocol = PROTOCOL_CHANNELS_AFHDS3,
.init = initInternal,
.deinit = deinitInternal,
.setupPulses = setupPulsesInternal,
.sendPulses = sendPulsesInternal,
.setupPulses = setupPulses,
.sendPulses = sendPulses,
.getByte = getByteInternal,
.processData = processTelemetryData,
};
#endif
radio/src/pulses/afhds3_transport.cpp
+52 -8
View File
@@ -23,6 +23,9 @@
#include "opentx_helpers.h"
#include "debug.h"
#include "extmodule_driver.h"
#include "board.h"
// timer is 2 MHz
#if defined(AFHDS3_SLOW)
// 1000000/57600 = 17,36 us
@@ -308,9 +311,35 @@ void CommandFifo::enqueue(COMMAND command, FRAME_TYPE frameType, bool useData,
}
}
void Transport::init(ByteTransport::Type t, void* buffer)
static const etx_serial_init _uartParams = {
.baudrate = AFHDS3_UART_BAUDRATE,
.parity = ETX_Parity_None,
.stop_bits = ETX_StopBits_One,
.word_length = ETX_WordLength_8,
.rx_enable = true,
};
void Transport::init(ByteTransport::Type t, void* buffer, const etx_serial_driver_t* drv)
{
trsp.init(t, buffer);
if (drv) {
uart_drv = drv;
uart_ctx = drv->init(&_uartParams);
} else {
uart_drv = nullptr;
uart_ctx = nullptr;
extmoduleSerialStart();
}
}
void Transport::deinit()
{
if (uart_drv) {
uart_drv->deinit(uart_ctx);
} else {
extmoduleStop();
}
}
void Transport::clear()
@@ -354,6 +383,15 @@ void Transport::enqueue(COMMAND command, FRAME_TYPE frameType, bool useData,
fifo.enqueue(command, frameType, useData, byteContent);
}
void Transport::sendBuffer()
{
if (uart_drv) {
uart_drv->sendBuffer(uart_ctx, (uint8_t*)trsp.trsp_buffer, trsp.getFrameSize());
} else {
extmoduleSendNextFrameSoftSerial((uint8_t*)trsp.trsp_buffer, trsp.getFrameSize());
}
}
bool Transport::processQueue()
{
// check waiting commands
@@ -420,13 +458,10 @@ bool Transport::handleReply(uint8_t* buffer, uint8_t len)
responseFrame->command, responseFrame->frameType,
responseFrame->frameNumber);
// TODO: send ACK immediately !!!
// trsp.putFrame(command, FRAME_TYPE::RESPONSE_ACK, nullptr, 0,
// responseFrame->frameNumber);
// return true;
fifo.enqueueACK((enum COMMAND)responseFrame->command,
responseFrame->frameNumber);
auto command = (enum COMMAND)responseFrame->command;
trsp.putFrame(command, FRAME_TYPE::RESPONSE_ACK, nullptr, 0,
responseFrame->frameNumber);
sendBuffer();
} else if (responseFrame->frameType == FRAME_TYPE::RESPONSE_DATA ||
responseFrame->frameType == FRAME_TYPE::RESPONSE_ACK) {
@@ -439,6 +474,15 @@ bool Transport::handleReply(uint8_t* buffer, uint8_t len)
return false;
}
int Transport::getTelemetryByte(uint8_t* data)
{
if (uart_drv) {
return uart_drv->getByte(uart_ctx, data);
} else {
return sportGetByte(data);
}
}
bool Transport::processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
uint8_t& rxBufferCount, uint8_t maxSize)
{
radio/src/pulses/afhds3_transport.h
+10 -3
View File
@@ -24,6 +24,8 @@
#include "afhds3_module.h"
#include "definitions.h"
#include "hal/serial_driver.h"
namespace afhds3
{
@@ -159,6 +161,9 @@ class Transport
IDLE
};
const etx_serial_driver_t* uart_drv;
void* uart_ctx;
FrameTransport trsp;
CommandFifo fifo;
@@ -182,7 +187,8 @@ class Transport
bool handleReply(uint8_t* buffer, uint8_t len);
public:
void init(ByteTransport::Type t, void* buffer);
void init(ByteTransport::Type t, void* buffer, const etx_serial_driver_t* drv);
void deinit();
void clear();
@@ -192,8 +198,7 @@ class Transport
void enqueue(COMMAND command, FRAME_TYPE frameType, bool useData = false,
uint8_t byteContent = 0);
uint8_t* getFrameBuffer() { return (uint8_t*)trsp.trsp_buffer; }
uint32_t getFrameSize() { return trsp.getFrameSize(); }
void sendBuffer();
/**
* Process retransmissions
@@ -208,6 +213,8 @@ class Transport
*/
bool processQueue();
int getTelemetryByte(uint8_t* data);
bool processTelemetryData(uint8_t byte, uint8_t* rxBuffer,
uint8_t& rxBufferCount, uint8_t maxSize);
};
radio/src/pulses/crossfire.cpp
+168 -46
View File
@@ -112,6 +112,145 @@ static void crossfireSetupMixerScheduler(uint8_t module)
}
}
static bool _checkFrameCRC(uint8_t* rxBuffer)
{
uint8_t len = rxBuffer[1];
uint8_t crc = crc8(&rxBuffer[2], len-1);
return (crc == rxBuffer[len+1]);
}
struct CrossfireState {
uint8_t module;
CrossfirePulsesData* data;
const etx_serial_driver_t* uart_drv;
void* uart_ctx;
uint8_t endpoint;
void init(uint8_t _module, CrossfirePulsesData* _data,
const etx_serial_driver_t* _drv, void* _ctx, uint8_t _endpoint)
{
module = _module;
data = _data;
uart_drv = _drv;
uart_ctx = _ctx;
endpoint = _endpoint;
}
void deinit() { uart_drv->deinit(uart_ctx); }
};
static CrossfireState crossfireState[NUM_MODULES];
static void crossfireSetupPulses(void* context, int16_t* channels, uint8_t nChannels)
{
auto state = (CrossfireState*)context;
crossfireSetupMixerScheduler(state->module);
setupPulsesCrossfire(state->module, state->data, state->endpoint, channels, nChannels);
}
static void crossfireSendPulses(void* context)
{
auto state = (CrossfireState*)context;
auto data = state->data;
if (state->uart_drv) {
auto drv = state->uart_drv;
auto ctx = state->uart_ctx;
drv->sendBuffer(ctx, data->pulses, data->length);
} else {
sportSendBuffer(data->pulses, data->length);
}
}
static int crossfireGetByte(void* context, uint8_t* data)
{
auto state = (CrossfireState*)context;
if (state->uart_drv) {
auto drv = state->uart_drv;
auto ctx = state->uart_ctx;
return drv->getByte(ctx, data);
} else {
return sportGetByte(data);
}
}
static bool _lenIsSane(uint8_t len)
{
// packet len must be at least 3 bytes (type+payload+crc) and 2 bytes < MAX (hdr+len)
return (len > 2 && len < TELEMETRY_RX_PACKET_SIZE-1);
}
static void _seekStart(uint8_t* buffer, uint8_t* len)
{
// Bad telemetry packets frequently are just truncated packets, with the start
// of a new packet contained in the data. This causes multiple packet drops as
// the parser tries to resync.
// Search through the rxBuffer for a sync byte, shift the contents if found
// and reduce rxBufferCount
for (uint8_t idx = 1; idx < *len; ++idx) {
uint8_t data = buffer[idx];
if (data == RADIO_ADDRESS || data == UART_SYNC) {
uint8_t remain = *len - idx;
// If there's at least 2 bytes, check the length for validity too
if (remain > 1 && !_lenIsSane(buffer[idx + 1])) continue;
// TRACE("Found 0x%02x with %u remain", data, remain);
// copy the data to the front of the buffer
for (uint8_t src = idx; src < *len; ++src) {
buffer[src - idx] = buffer[src];
}
*len = remain;
return;
} // if found sync
}
// Not found, clear the buffer
*len = 0;
}
static void crossfireProcessData(void* context, uint8_t data, uint8_t* buffer, uint8_t* len)
{
if (*len == 0 && data != RADIO_ADDRESS && data != UART_SYNC) {
TRACE("[XF] address 0x%02X error", data);
return;
}
if (*len == 1 && !_lenIsSane(data)) {
TRACE("[XF] length 0x%02X error", data);
*len = 0;
return;
}
if (*len < TELEMETRY_RX_PACKET_SIZE) {
buffer[(*len)++] = data;
}
else {
TRACE("[XF] array size %d error", *len);
*len = 0;
}
// rxBuffer[1] holds the packet length-2, check if the whole packet was received
while (*len > 4 && (buffer[1]+2) == *len) {
// TODO: module in context?
if (_checkFrameCRC(buffer)) {
#if defined(BLUETOOTH)
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(buffer, *len);
}
#endif
auto state = (CrossfireState*)context;
processCrossfireTelemetryFrame(state->module);
*len = 0;
}
else {
TRACE("[XF] CRC error ");
_seekStart(buffer, len); // adjusts len
}
}
}
#if defined(INTERNAL_MODULE_CRSF)
static const etx_serial_init intmoduleCrossfireInitParams = {
.baudrate = 0,
@@ -123,59 +262,56 @@ static const etx_serial_init intmoduleCrossfireInitParams = {
static void* crossfireInitInternal(uint8_t module)
{
(void)module;
// serial port setup
etx_serial_init params(intmoduleCrossfireInitParams);
params.baudrate = INT_CROSSFIRE_BAUDRATE;
INTERNAL_MODULE_ON();
return IntmoduleSerialDriver.init(&params);
auto state = &crossfireState[module];
state->init(module, &intmodulePulsesData.crossfire,
&IntmoduleSerialDriver, IntmoduleSerialDriver.init(&params), 0);
return state;
}
static void crossfireDeInitInternal(void* context)
{
(void)context;
INTERNAL_MODULE_OFF();
mixerSchedulerSetPeriod(INTERNAL_MODULE, 0);
intmoduleStop();
}
static void crossfireSetupPulsesInternal(void* context, int16_t* channels, uint8_t nChannels)
{
auto* p_data = &intmodulePulsesData.crossfire;
crossfireSetupMixerScheduler(INTERNAL_MODULE);
setupPulsesCrossfire(INTERNAL_MODULE, p_data, 0, channels, nChannels);
}
static void crossfireSendPulsesInternal(void* context)
{
IntmoduleSerialDriver.sendBuffer(context, intmodulePulsesData.crossfire.pulses,
intmodulePulsesData.crossfire.length);
}
static int crossfireGetByteInternal(void* context, uint8_t* data)
{
return IntmoduleSerialDriver.getByte(context, data);
auto state = (CrossfireState*)context;
state->deinit();
}
etx_module_driver_t CrossfireInternalDriver = {
.protocol = PROTOCOL_CHANNELS_CROSSFIRE,
.init = crossfireInitInternal,
.deinit = crossfireDeInitInternal,
.setupPulses = crossfireSetupPulsesInternal,
.sendPulses = crossfireSendPulsesInternal,
.getByte = crossfireGetByteInternal,
.setupPulses = crossfireSetupPulses,
.sendPulses = crossfireSendPulses,
.getByte = crossfireGetByte,
.processData = crossfireProcessData,
};
#endif
static void* crossfireInitExternal(uint8_t module)
{
telemetryInit(PROTOCOL_TELEMETRY_CROSSFIRE);
// telemetryInit(PROTOCOL_TELEMETRY_CROSSFIRE);
telemetryProtocol = PROTOCOL_TELEMETRY_CROSSFIRE;
telemetryPortInit(EXT_CROSSFIRE_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
#if defined(LUA)
outputTelemetryBuffer.reset();
#endif
telemetryPortSetDirectionOutput();
mixerSchedulerSetPeriod(module, CROSSFIRE_PERIOD);
EXTERNAL_MODULE_ON();
return nullptr;
auto state = &crossfireState[module];
state->init(module, &extmodulePulsesData.crossfire, nullptr, nullptr,
TELEMETRY_ENDPOINT_SPORT);
return state;
}
static void crossfireDeInitExternal(void* context)
@@ -187,26 +323,12 @@ static void crossfireDeInitExternal(void* context)
mixerSchedulerSetPeriod(EXTERNAL_MODULE, 0);
}
static void crossfireSetupPulsesExternal(void* context, int16_t* channels, uint8_t nChannels)
{
auto* p_data = &extmodulePulsesData.crossfire;
crossfireSetupMixerScheduler(EXTERNAL_MODULE);
setupPulsesCrossfire(EXTERNAL_MODULE, p_data, TELEMETRY_ENDPOINT_SPORT,
channels, nChannels);
}
static void crossfireSendPulsesExternal(void* context)
{
(void)context;
sportSendBuffer(extmodulePulsesData.crossfire.pulses,
extmodulePulsesData.crossfire.length);
}
etx_module_driver_t CrossfireExternalDriver = {
.protocol = PROTOCOL_CHANNELS_CROSSFIRE,
.init = crossfireInitExternal,
.deinit = crossfireDeInitExternal,
.setupPulses = crossfireSetupPulsesExternal,
.sendPulses = crossfireSendPulsesExternal
.setupPulses = crossfireSetupPulses,
.sendPulses = crossfireSendPulses,
.getByte = crossfireGetByte,
.processData = crossfireProcessData,
};
radio/src/pulses/flysky.cpp
+18 -15
View File
@@ -32,6 +32,14 @@
#define custom_log
#endif
enum AfhdsSpecialChars {
END = 0xC0, //Frame end
START = END,
ESC_END = 0xDC, //Escaped frame end - in case END occurs in fame then ESC ESC_END must be used
ESC = 0xDB, //Escaping character
ESC_ESC = 0xDD, //Escaping character in case ESC occurs in fame then ESC ESC_ESC must be used
};
// 0:OFF, 1:RF frame only, 2:TX frame only, 3:Both RF and TX frame
enum DEBUG_RF_FRAME_PRINT_E {
FRAME_PRINT_OFF = 0,
@@ -399,16 +407,13 @@ inline void debugFrame(const uint8_t* rxBuffer, uint8_t rxBufferCount)
TRACE("count [%d] data: %s", rxBufferCount, buffer);
}
inline void parseResponse()
inline void parseResponse(uint8_t* buffer, uint8_t dataLen)
{
afhds2Resp* resp =
reinterpret_cast<afhds2Resp*>(intmodulePulsesData.flysky.telemetry);
uint8_t dataLen = intmodulePulsesData.flysky.telemetry_index;
afhds2Resp* resp = reinterpret_cast<afhds2Resp*>(buffer);
if (resp->startByte != END || dataLen < 2) return;
dataLen -= 2;
if (!checkFlySkyFrameCrc(intmodulePulsesData.flysky.telemetry + 1, dataLen)) {
if (!checkFlySkyFrameCrc(buffer + 1, dataLen)) {
return;
}
@@ -533,7 +538,7 @@ bool isrxBufferMsgOK(void)
return isMsgOK && isFlySkyUsbDownload();
}
void processInternalFlySkyTelemetryData(uint8_t byte)
void processInternalFlySkyTelemetryData(uint8_t byte, uint8_t* buffer, uint8_t* len)
{
// #if !defined(SIMU)
// parseFlyskyData(&rxBuffer, byte);
@@ -559,9 +564,9 @@ void processInternalFlySkyTelemetryData(uint8_t byte)
// }
// usbDownloadTransmit(pt, rxBuffer.length + 5);
// }
if (byte == END && intmodulePulsesData.flysky.telemetry_index > 0) {
parseResponse();
intmodulePulsesData.flysky.telemetry_index = 0;
if (byte == END && *len > 0) {
parseResponse(buffer, *len);
*len = 0;
} else {
if (byte == ESC) {
intmodulePulsesData.flysky.esc_state = 1;
@@ -573,12 +578,10 @@ void processInternalFlySkyTelemetryData(uint8_t byte)
else if (byte == ESC_ESC)
byte = ESC;
}
intmodulePulsesData.flysky
.telemetry[intmodulePulsesData.flysky.telemetry_index++] = byte;
if (intmodulePulsesData.flysky.telemetry_index >=
sizeof(intmodulePulsesData.flysky.telemetry)) {
buffer[(*len)++] = byte;
if (*len >= TELEMETRY_RX_PACKET_SIZE) {
// TODO buffer is full, log an error?
intmodulePulsesData.flysky.telemetry_index = 0;
*len = 0;
}
}
}
radio/src/pulses/flysky.h
+1 -1
View File
@@ -26,4 +26,4 @@
void setupPulsesAFHDS2();
void resetPulsesAFHDS2();
void processInternalFlySkyTelemetryData(uint8_t byte);
void processInternalFlySkyTelemetryData(uint8_t byte, uint8_t* buffer, uint8_t* len);
radio/src/pulses/multi.cpp
+15 -6
View File
@@ -97,14 +97,17 @@ static void sendFailsafeChannels(uint8_t moduleIdx)
int16_t failsafeValue = g_model.failsafeChannels[i];
int pulseValue;
if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_HOLD || failsafeValue == FAILSAFE_CHANNEL_HOLD) {
if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_HOLD ||
failsafeValue == FAILSAFE_CHANNEL_HOLD) {
pulseValue = 2047;
}
else if (g_model.moduleData[moduleIdx].failsafeMode == FAILSAFE_NOPULSES || failsafeValue == FAILSAFE_CHANNEL_NOPULSE) {
} else if (g_model.moduleData[moduleIdx].failsafeMode ==
FAILSAFE_NOPULSES ||
failsafeValue == FAILSAFE_CHANNEL_NOPULSE) {
pulseValue = 0;
}
else {
failsafeValue += 2 * PPM_CH_CENTER(g_model.moduleData[moduleIdx].channelsStart + i) - 2 * PPM_CENTER;
} else {
failsafeValue +=
2 * PPM_CH_CENTER(g_model.moduleData[moduleIdx].channelsStart + i) -
2 * PPM_CENTER;
pulseValue = limit(1, (failsafeValue * 800 / 1000) + 1024, 2046);
}
@@ -270,6 +273,11 @@ static int multiGetByte(void* context, uint8_t* data)
return IntmoduleSerialDriver.getByte(context, data);
}
static void multiProcessData(void* context, uint8_t data, uint8_t* buffer, uint8_t* len)
{
processMultiTelemetryData(data, INTERNAL_MODULE);
}
#include "hal/module_driver.h"
const etx_module_driver_t MultiInternalDriver = {
@@ -279,6 +287,7 @@ const etx_module_driver_t MultiInternalDriver = {
.setupPulses = multiSetupPulses,
.sendPulses = multiSendPulses,
.getByte = multiGetByte,
.processData = multiProcessData,
};
#endif
radio/src/pulses/pxx2.cpp
+129 -48
View File
@@ -646,61 +646,139 @@ void Pxx2OtaUpdate::flashFirmware(const char * filename, ProgressHandler progres
resumePulses();
}
static void* pxx2InitInternal(uint8_t moduleType)
{
void* uart_ctx = IntmoduleSerialDriver.init(&pxx2SerialInitParams);
resetAccessAuthenticationCount();
struct PXX2State {
uint8_t module;
Pxx2Pulses* pulses;
const etx_serial_driver_t* uart_drv;
void* uart_ctx;
void init(uint8_t _module, Pxx2Pulses* _pulses, const etx_serial_driver_t* _drv, void* _ctx)
{
module = _module;
pulses = _pulses;
uart_drv = _drv;
uart_ctx = _ctx;
}
void deinit() { uart_drv->deinit(uart_ctx); }
};
static PXX2State pxx2State[NUM_MODULES];
static void* pxx2InitInternal(uint8_t module)
{
#if defined(INTMODULE_HEARTBEAT)
// use backup trigger (1 ms later)
init_intmodule_heartbeat();
#endif
mixerSchedulerSetPeriod(INTERNAL_MODULE, PXX2_PERIOD);
mixerSchedulerSetPeriod(module, PXX2_PERIOD);
INTERNAL_MODULE_ON();
return uart_ctx;
resetAccessAuthenticationCount();
auto state = &pxx2State[module];
state->init(module, &intmodulePulsesData.pxx2, &IntmoduleSerialDriver,
IntmoduleSerialDriver.init(&pxx2SerialInitParams));
return state;
}
static void pxx2DeInitInternal(void* context)
{
auto state = (PXX2State*)context;
state->deinit();
INTERNAL_MODULE_OFF();
IntmoduleSerialDriver.deinit(context);
#if defined(INTMODULE_HEARTBEAT)
stop_intmodule_heartbeat();
#endif
mixerSchedulerSetPeriod(INTERNAL_MODULE, 0);
mixerSchedulerSetPeriod(state->module, 0);
}
// TODO: move this to Pxx2Pulses
static bool pxx2InternalSendNextFrame = true;
static void pxx2SetupPulsesInternal(void* context, int16_t* channels, uint8_t nChannels)
{
(void)context;
auto state = (PXX2State*)context;
pxx2InternalSendNextFrame =
intmodulePulsesData.pxx2.setupFrame(INTERNAL_MODULE, channels, nChannels);
auto pulses = state->pulses;
auto module = state->module;
pxx2InternalSendNextFrame = pulses->setupFrame(module, channels, nChannels);
if (moduleState[INTERNAL_MODULE].mode == MODULE_MODE_SPECTRUM_ANALYSER ||
moduleState[INTERNAL_MODULE].mode == MODULE_MODE_POWER_METER) {
mixerSchedulerSetPeriod(INTERNAL_MODULE, PXX2_TOOLS_PERIOD);
if (moduleState[module].mode == MODULE_MODE_SPECTRUM_ANALYSER ||
moduleState[module].mode == MODULE_MODE_POWER_METER) {
mixerSchedulerSetPeriod(module, PXX2_TOOLS_PERIOD);
} else {
mixerSchedulerSetPeriod(INTERNAL_MODULE, PXX2_PERIOD);
mixerSchedulerSetPeriod(module, PXX2_PERIOD);
}
}
static void pxx2SendPulsesInternal(void* context)
{
if (pxx2InternalSendNextFrame) {
IntmoduleSerialDriver.sendBuffer(context, intmodulePulsesData.pxx2.getData(),
intmodulePulsesData.pxx2.getSize());
auto state = (PXX2State*)context;
auto drv = state->uart_drv;
auto ctx = state->uart_ctx;
auto pulses = state->pulses;
drv->sendBuffer(ctx, pulses->getData(), pulses->getSize());
}
}
static int pxx2GetByteInternal(void* context, uint8_t* data)
static int pxx2GetByte(void* context, uint8_t* data)
{
return IntmoduleSerialDriver.getByte(context, data);
auto state = (PXX2State*)context;
auto drv = state->uart_drv;
auto ctx = state->uart_ctx;
return drv->getByte(ctx, data);
}
static void pxx2ProcessData(void* context, uint8_t data, uint8_t* buffer, uint8_t* len)
{
if (*len == 0 && data != START_STOP) {
return;
}
if (*len == 1 && data > 40) {
*len = 0;
return;
}
if (*len < TELEMETRY_RX_PACKET_SIZE) {
buffer[(*len)++] = data;
}
else {
TRACE("[PXX2] array size %d error", *len);
*len = 0;
}
uint8_t frame_len = buffer[1];
/* 1 byte start + 2 bytes header + 2 bytes CRC = 5 */
if (*len < unsigned(frame_len + 5)) {
return;
}
uint16_t crc = 0xFFFF;
uint8_t* frame = buffer + 1;
for (uint32_t i = 1; i <= frame_len; i++) {
crc -= frame[i];
}
uint8_t crcHigh = frame[frame_len + 1];
uint8_t crcLow = frame[frame_len + 2];
if (((crc >> 8) != crcHigh) || ((crc & 0xFF) == crcLow)) {
TRACE("[PXX2] crc error");
*len = 0;
return;
}
auto state = (PXX2State*)context;
processPXX2Frame(state->module, frame);
}
#include "hal/module_driver.h"
const etx_module_driver_t Pxx2InternalDriver = {
@@ -709,42 +787,36 @@ const etx_module_driver_t Pxx2InternalDriver = {
.deinit = pxx2DeInitInternal,
.setupPulses = pxx2SetupPulsesInternal,
.sendPulses = pxx2SendPulsesInternal,
.getByte = pxx2GetByteInternal,
.getByte = pxx2GetByte,
.processData = pxx2ProcessData,
};
#if defined(EXTMODULE_USART)
#include "extmodule_serial_driver.h"
static const etx_serial_init pxx2ExtSerialInitParams = {
.baudrate = 0,
.parity = ETX_Parity_None,
.stop_bits = ETX_StopBits_One,
.word_length = ETX_WordLength_8,
.rx_enable = true,
};
static void* pxx2InitLowSpeed(uint8_t module)
static void* pxx2InitExternal(uint8_t module, uint32_t baudrate)
{
etx_serial_init params(pxx2ExtSerialInitParams);
params.baudrate = PXX2_LOWSPEED_BAUDRATE;
ExtmoduleSerialDriver.init(&params);
etx_serial_init params(pxx2SerialInitParams);
params.baudrate = baudrate;
mixerSchedulerSetPeriod(EXTERNAL_MODULE, PXX2_NO_HEARTBEAT_PERIOD);
mixerSchedulerSetPeriod(module, PXX2_NO_HEARTBEAT_PERIOD);
EXTERNAL_MODULE_ON();
return nullptr;
auto state = &pxx2State[module];
state->init(module, &extmodulePulsesData.pxx2, &ExtmoduleSerialDriver,
ExtmoduleSerialDriver.init(&params));
return state;
}
static void* pxx2InitExternal(uint8_t module)
static void* pxx2InitExtLowSpeed(uint8_t module)
{
etx_serial_init params(pxx2ExtSerialInitParams);
params.baudrate = PXX2_HIGHSPEED_BAUDRATE;
void* uart_ctx = ExtmoduleSerialDriver.init(&params);
return pxx2InitExternal(module, PXX2_LOWSPEED_BAUDRATE);
}
mixerSchedulerSetPeriod(EXTERNAL_MODULE, PXX2_NO_HEARTBEAT_PERIOD);
EXTERNAL_MODULE_ON();
return uart_ctx;
static void* pxx2InitExtHighSpeed(uint8_t module)
{
return pxx2InitExternal(module, PXX2_HIGHSPEED_BAUDRATE);
}
static void pxx2DeInitExternal(void* context)
@@ -756,29 +828,38 @@ static void pxx2DeInitExternal(void* context)
static void pxx2SetupPulsesExternal(void* context, int16_t* channels, uint8_t nChannels)
{
(void)context;
extmodulePulsesData.pxx2.setupFrame(EXTERNAL_MODULE, channels, nChannels);
auto state = (PXX2State*)context;
auto pulses = state->pulses;
auto module = state->module;
pulses->setupFrame(module, channels, nChannels);
}
static void pxx2SendPulsesExternal(void* context)
{
ExtmoduleSerialDriver.sendBuffer(context, extmodulePulsesData.pxx2.getData(),
extmodulePulsesData.pxx2.getSize());
auto state = (PXX2State*)context;
auto drv = state->uart_drv;
auto ctx = state->uart_ctx;
auto pulses = state->pulses;
drv->sendBuffer(ctx, pulses->getData(), pulses->getSize());
}
const etx_module_driver_t Pxx2ExternalDriver = {
.protocol = PROTOCOL_CHANNELS_PXX2_HIGHSPEED,
.init = pxx2InitExternal,
.init = pxx2InitExtHighSpeed,
.deinit = pxx2DeInitExternal,
.setupPulses = pxx2SetupPulsesExternal,
.sendPulses = pxx2SendPulsesExternal,
.getByte = pxx2GetByte,
.processData = pxx2ProcessData,
};
const etx_module_driver_t Pxx2LowSpeedExternalDriver = {
.protocol = PROTOCOL_CHANNELS_PXX2_LOWSPEED,
.init = pxx2InitLowSpeed,
.init = pxx2InitExtLowSpeed,
.deinit = pxx2DeInitExternal,
.setupPulses = pxx2SetupPulsesExternal,
.sendPulses = pxx2SendPulsesExternal,
.getByte = pxx2GetByte,
.processData = pxx2ProcessData,
};
#endif
radio/src/telemetry/crossfire.cpp
-87
View File
@@ -93,14 +93,6 @@ void processCrossfireTelemetryValue(uint8_t index, int32_t value)
value, sensor.unit, sensor.precision);
}
bool checkCrossfireTelemetryFrameCRC(uint8_t module)
{
uint8_t * rxBuffer = getTelemetryRxBuffer(module);
uint8_t len = rxBuffer[1];
uint8_t crc = crc8(&rxBuffer[2], len-1);
return (crc == rxBuffer[len+1]);
}
template<int N>
bool getCrossfireTelemetryValue(uint8_t index, int32_t & value, uint8_t module)
{
@@ -272,85 +264,6 @@ void processCrossfireTelemetryFrame(uint8_t module)
}
}
bool crossfireLenIsSane(uint8_t len)
{
// packet len must be at least 3 bytes (type+payload+crc) and 2 bytes < MAX (hdr+len)
return (len > 2 && len < TELEMETRY_RX_PACKET_SIZE-1);
}
void crossfireTelemetrySeekStart(uint8_t *rxBuffer, uint8_t &rxBufferCount)
{
// Bad telemetry packets frequently are just truncated packets, with the start
// of a new packet contained in the data. This causes multiple packet drops as
// the parser tries to resync.
// Search through the rxBuffer for a sync byte, shift the contents if found
// and reduce rxBufferCount
for (uint8_t idx=1; idx<rxBufferCount; ++idx) {
uint8_t data = rxBuffer[idx];
if (data == RADIO_ADDRESS || data == UART_SYNC) {
uint8_t remain = rxBufferCount - idx;
// If there's at least 2 bytes, check the length for validity too
if (remain > 1 && !crossfireLenIsSane(rxBuffer[idx+1]))
continue;
//TRACE("Found 0x%02x with %u remain", data, remain);
// copy the data to the front of the buffer
for (uint8_t src=idx; src<rxBufferCount; ++src) {
rxBuffer[src-idx] = rxBuffer[src];
}
rxBufferCount = remain;
return;
} // if found sync
}
// Not found, clear the buffer
rxBufferCount = 0;
}
void processCrossfireTelemetryData(uint8_t data, uint8_t module)
{
uint8_t * rxBuffer = getTelemetryRxBuffer(module);
uint8_t &rxBufferCount = getTelemetryRxBufferCount(module);
if (rxBufferCount == 0 && data != RADIO_ADDRESS && data != UART_SYNC) {
TRACE("[XF] address 0x%02X error", data);
return;
}
if (rxBufferCount == 1 && !crossfireLenIsSane(data)) {
TRACE("[XF] length 0x%02X error", data);
rxBufferCount = 0;
return;
}
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
}
else {
TRACE("[XF] array size %d error", rxBufferCount);
rxBufferCount = 0;
}
// rxBuffer[1] holds the packet length-2, check if the whole packet was received
while (rxBufferCount > 4 && (rxBuffer[1]+2) == rxBufferCount) {
if (checkCrossfireTelemetryFrameCRC(module)) {
#if defined(BLUETOOTH)
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(rxBuffer, rxBufferCount);
}
#endif
processCrossfireTelemetryFrame(module);
rxBufferCount = 0;
}
else {
TRACE("[XF] CRC error ");
crossfireTelemetrySeekStart(rxBuffer, rxBufferCount); // adjusts rxBufferCount
}
}
}
void crossfireSetDefault(int index, uint8_t id, uint8_t subId)
{
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
radio/src/telemetry/crossfire.h
+1 -1
View File
@@ -100,7 +100,7 @@ enum CrossfireFrames{
CRSF_FRAME_MODELID_SENT
};
void processCrossfireTelemetryData(uint8_t data, uint8_t module);
void processCrossfireTelemetryFrame(uint8_t module);
void crossfireSetDefault(int index, uint8_t id, uint8_t subId);
uint8_t createCrossfireModelIDFrame(uint8_t * frame);
radio/src/telemetry/frsky_pxx2.cpp
+1 -47
View File
@@ -395,7 +395,7 @@ static void processToolsFrame(uint8_t module, const uint8_t * frame)
}
}
static void processPXX2Frame(uint8_t module, const uint8_t * frame)
void processPXX2Frame(uint8_t module, const uint8_t * frame)
{
LOG_TELEMETRY_WRITE_START();
for (uint8_t i = 0; i < 1 + frame[0]; i++) {
@@ -420,49 +420,3 @@ static void processPXX2Frame(uint8_t module, const uint8_t * frame)
}
}
void processPXX2TelemetryData(uint8_t data, uint8_t idx)
{
uint8_t * rxBuffer = getTelemetryRxBuffer(idx);
uint8_t &rxBufferCount = getTelemetryRxBufferCount(idx);
if (rxBufferCount == 0 && data != START_STOP) {
return;
}
if (rxBufferCount == 1 && data > 40) {
rxBufferCount = 0;
return;
}
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
}
else {
TRACE("[PXX2] array size %d error", rxBufferCount);
rxBufferCount = 0;
}
uint8_t len = rxBuffer[1];
/* 1 byte start + 2 bytes header + 2 bytes CRC = 5 */
if (rxBufferCount < unsigned(len + 5)) {
return;
}
uint16_t crc = 0xFFFF;
uint8_t* frame = rxBuffer + 1;
for (uint32_t i = 1; i <= len; i++) {
crc -= frame[i];
}
uint8_t crcHigh = frame[len + 1];
uint8_t crcLow = frame[len + 2];
if (((crc >> 8) != crcHigh) || ((crc & 0xFF) == crcLow)) {
TRACE("[PXX2] crc error");
rxBufferCount = 0;
return;
}
processPXX2Frame(idx, frame);
}
radio/src/telemetry/telemetry.cpp
+34 -174
View File
@@ -177,13 +177,7 @@ void telemetryStop()
void processTelemetryData(uint8_t data)
{
#if defined(CROSSFIRE)
if (telemetryProtocol == PROTOCOL_TELEMETRY_CROSSFIRE) {
processCrossfireTelemetryData(data, EXTERNAL_MODULE);
return;
}
#endif
// TODO: move these to new interface
#if defined(GHOST)
if (telemetryProtocol == PROTOCOL_TELEMETRY_GHOST) {
processGhostTelemetryData(data);
@@ -198,32 +192,21 @@ void processTelemetryData(uint8_t data)
return;
}
// TODO: move these to new interface
#if defined(MULTIMODULE)
if (telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_IBUS) {
processFlySkyTelemetryData(data, telemetryRxBuffer, telemetryRxBufferCount);
return;
}
if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
processMultiTelemetryData(data, EXTERNAL_MODULE);
return;
}
#endif
#if defined(AFHDS2)
if(telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_NV14) {
processInternalFlySkyTelemetryData(data);
return;
#if defined(PXX1)
if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT ||
telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_D ||
telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) {
processFrskyTelemetryData(data);
}
#endif
#if defined(AFHDS3)
if (telemetryProtocol == PROTOCOL_TELEMETRY_AFHDS3) {
afhds3::processTelemetryData(data, EXTERNAL_MODULE);
return;
}
#endif
processFrskyTelemetryData(data);
}
inline bool isBadAntennaDetected()
@@ -241,6 +224,7 @@ inline bool isBadAntennaDetected()
}
#if defined(MULTI_PROTOLIST)
// TODO: FreeRTOS timer instead...
static inline void pollMultiProtolist(uint8_t idx)
{
if ((moduleState[idx].protocol == PROTOCOL_CHANNELS_MULTIMODULE) &&
@@ -250,89 +234,25 @@ static inline void pollMultiProtolist(uint8_t idx)
}
#endif
static inline void pollIntTelemetry(void (*processData)(uint8_t, uint8_t))
static inline void pollTelemetry(uint8_t module, const etx_module_driver_t* drv, void* ctx)
{
auto drv = getIntModuleDriver();
auto ctx = getIntModuleCtx();
if (!drv || !drv->getByte || !drv->processData) return;
uint8_t* rxBuffer = getTelemetryRxBuffer(module);
uint8_t& rxBufferCount = getTelemetryRxBufferCount(module);
if (!drv || !drv->getByte || !processData) return;
uint8_t data;
if (drv->getByte(ctx, &data) > 0) {
LOG_TELEMETRY_WRITE_START();
do {
telemetryMirrorSend(data);
processData(data, INTERNAL_MODULE);
drv->processData(ctx, data, rxBuffer, &rxBufferCount);
LOG_TELEMETRY_WRITE_BYTE(data);
} while (drv->getByte(ctx, &data) > 0);
}
}
#if defined(INTERNAL_MODULE_PXX2)
static void pollIntPXX2()
{
pollIntTelemetry(processPXX2TelemetryData);
}
#endif
#if defined(PXX2) && defined(EXTMODULE_USART)
static void pollExtPXX2()
{
auto drv = getExtModuleDriver();
auto ctx = getExtModuleCtx();
if (!drv || !drv->getByte) return;
uint8_t data;
if (drv->getByte(ctx, &data) > 0) {
LOG_TELEMETRY_WRITE_START();
do {
telemetryMirrorSend(data);
processPXX2TelemetryData(data, EXTERNAL_MODULE);
LOG_TELEMETRY_WRITE_BYTE(data);
} while (drv->getByte(ctx, &data) > 0);
}
}
#endif
#if defined(INTERNAL_MODULE_MULTI)
static void pollIntMulti()
{
pollIntTelemetry(processMultiTelemetryData);
#if defined(MULTI_PROTOLIST)
pollMultiProtolist(INTERNAL_MODULE);
#endif
}
#endif
#if defined(INTERNAL_MODULE_CRSF)
static void pollIntCrossfire()
{
pollIntTelemetry(processCrossfireTelemetryData);
}
#endif
#if defined(INTERNAL_MODULE_AFHDS2A)
static void processFlySkyTelemetryData(uint8_t data, uint8_t idx)
{
(void)idx;
processInternalFlySkyTelemetryData(data);
}
static void pollIntAFHDS2A()
{
pollIntTelemetry(processFlySkyTelemetryData);
}
#endif
#if defined(INTERNAL_MODULE_AFHDS3)
static void pollIntAFHDS3()
{
pollIntTelemetry(afhds3::processTelemetryData);
}
#endif
static void pollExtTelemetry()
static inline void pollExtTelemetryLegacy()
{
uint8_t data;
if (telemetryGetByte(&data)) {
@@ -343,63 +263,30 @@ static void pollExtTelemetry()
LOG_TELEMETRY_WRITE_BYTE(data);
} while (telemetryGetByte(&data));
}
#if defined(MULTI_PROTOLIST)
if (isModuleMultimodule(EXTERNAL_MODULE)) {
pollMultiProtolist(EXTERNAL_MODULE);
}
#endif
#if defined(PXX2) && defined(EXTMODULE_USART)
if (isModulePXX2(EXTERNAL_MODULE)) {
pollExtPXX2();
}
#endif
}
// TODO: this needs to be rewritten completely
// - telemetry polling needs to happen for each enabled module
void telemetryWakeup()
{
uint8_t requiredTelemetryProtocol = modelTelemetryProtocol();
auto int_drv = getIntModuleDriver();
if (int_drv) pollTelemetry(INTERNAL_MODULE, int_drv, getIntModuleCtx());
auto ext_drv = getExtModuleDriver();
if (ext_drv) pollTelemetry(EXTERNAL_MODULE, ext_drv, getExtModuleCtx());
// TODO: needs to be moved to protocol/module init
// as-is, it implies only ONE telemetry protocol
// enabled at the same time
uint8_t requiredTelemetryProtocol = modelTelemetryProtocol();
if (telemetryProtocol != requiredTelemetryProtocol) {
telemetryInit(requiredTelemetryProtocol);
}
// Poll internal modules
#if defined(INTERNAL_MODULE_PXX2)
if (isModuleISRM(INTERNAL_MODULE)) {
pollIntPXX2();
}
#endif
#if defined(INTERNAL_MODULE_MULTI)
if (isModuleMultimodule(INTERNAL_MODULE)) {
pollIntMulti();
}
#endif
#if defined(INTERNAL_MODULE_CRSF)
if (isModuleCrossfire(INTERNAL_MODULE)) {
pollIntCrossfire();
}
#endif
#if defined(INTERNAL_MODULE_AFHDS2A)
//! moduleUpdateActive(INTERNAL_MODULE) &&
if (isModuleAFHDS2A(INTERNAL_MODULE)) {
pollIntAFHDS2A();
}
#endif
#if defined(INTERNAL_MODULE_AFHDS3)
//! moduleUpdateActive(INTERNAL_MODULE) &&
if (isModuleAFHDS3(INTERNAL_MODULE)) {
pollIntAFHDS3();
}
#endif
// Poll external / S.PORT telemetry
// TODO: how to switch this OFF ???
pollExtTelemetry();
pollExtTelemetryLegacy();
for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
@@ -462,6 +349,7 @@ void telemetryWakeup()
if (telemetryState == TELEMETRY_KO) {
AUDIO_TELEMETRY_BACK();
#if defined(CROSSFIRE)
// TODO: move to crossfire code
if (isModuleCrossfire(EXTERNAL_MODULE)) {
moduleState[EXTERNAL_MODULE].counter = CRSF_FRAME_MODELID;
}
@@ -530,32 +418,22 @@ void telemetryInit(uint8_t protocol)
if (protocol == PROTOCOL_TELEMETRY_FRSKY_D) {
telemetryPortInit(FRSKY_D_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
}
#if defined(MULTIMODULE)
else if (protocol == PROTOCOL_TELEMETRY_MULTIMODULE || protocol == PROTOCOL_TELEMETRY_FLYSKY_IBUS) {
// The DIY Multi module always speaks 100000 baud regardless of the telemetry protocol in use
else if (protocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
// The DIY Multi module always speaks 100000 baud regardless of the
// telemetry protocol in use
telemetryPortInit(MULTIMODULE_BAUDRATE, TELEMETRY_SERIAL_8E2);
#if defined(LUA)
outputTelemetryBuffer.reset();
#endif
telemetryPortSetDirectionInput();
}
else if (protocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
// Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same since there is no real standard
} else if (protocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
// Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same
// since there is no real standard
telemetryPortInit(125000, TELEMETRY_SERIAL_DEFAULT);
}
#endif
#if defined(CROSSFIRE)
else if (protocol == PROTOCOL_TELEMETRY_CROSSFIRE) {
telemetryPortInit(EXT_CROSSFIRE_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
#if defined(LUA)
outputTelemetryBuffer.reset();
#endif
telemetryPortSetDirectionOutput();
}
#endif
#if defined(GHOST)
else if (protocol == PROTOCOL_TELEMETRY_GHOST) {
telemetryPortInit(GHOST_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
@@ -569,39 +447,19 @@ void telemetryInit(uint8_t protocol)
#if defined(AUX_SERIAL)
else if (protocol == PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY) {
telemetryPortInit(0, TELEMETRY_SERIAL_DEFAULT);
// Note: this is done in initSerialPorts()
//auxSerialTelemetryInit(PROTOCOL_TELEMETRY_FRSKY_D_SECONDARY);
}
#endif
#if defined(AFHDS3)
else if (protocol == PROTOCOL_TELEMETRY_AFHDS3) {
// TODO:
// telemetryPortInvertedInit(AFHDS3_BAUDRATE);
// telemetryPortSetDirectionInput();
}
#endif
#if defined(AFHDS2)
else if (protocol == PROTOCOL_TELEMETRY_FLYSKY_NV14) {
telemetryPortInit(INTMODULE_USART_AFHDS2_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
telemetryPortSetDirectionInput();
}
#endif
else if (protocol == PROTOCOL_TELEMETRY_DSMP) {
// soft serial
telemetryPortInvertedInit(115200);
}
else {
} else {
telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA);
#if defined(LUA)
outputTelemetryBuffer.reset();
#endif
}
}
#if defined(LOG_TELEMETRY) && !defined(SIMU)
extern FIL g_telemetryFile;
void logTelemetryWriteStart()
@@ -611,7 +469,9 @@ void logTelemetryWriteStart()
if (lastTime != newTime) {
struct gtm utm;
gettime(&utm);
f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0:", utm.tm_year+TM_YEAR_BASE, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0:",
utm.tm_year + TM_YEAR_BASE, utm.tm_mon + 1, utm.tm_mday,
utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
lastTime = newTime;
}
}
radio/src/telemetry/telemetry.h
+1 -1
View File
@@ -304,7 +304,7 @@ extern OutputTelemetryBuffer outputTelemetryBuffer __DMA;
extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
#endif
void processPXX2TelemetryData(uint8_t data, uint8_t idx);
void processPXX2Frame(uint8_t idx, const uint8_t* frame);
// Module pulse synchronization
struct ModuleSyncStatus
radio/src/telemetry/telemetry_sensors.cpp
-5
View File
@@ -568,11 +568,6 @@ int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, ui
break;
#endif
#if defined(MULTIMODULE) || defined(AFHDS3) || defined(AFHDS2)
case PROTOCOL_TELEMETRY_FLYSKY_IBUS:
flySkySetDefault(index, id, subId, instance);
break;
#endif
#if defined(AFHDS2) && defined(PCBNV14)
case PROTOCOL_TELEMETRY_FLYSKY_NV14: