From 246840f065a66c914fbbd31482047ed831d68877 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Fri, 1 Jul 2022 12:57:34 +0200 Subject: [PATCH] Moved telemetry processing to module interface --- radio/src/hal/module_driver.h | 4 +- radio/src/pulses/afhds2.cpp | 12 +- radio/src/pulses/afhds2.h | 16 -- radio/src/pulses/afhds3.cpp | 222 +++++++++------------- radio/src/pulses/afhds3_transport.cpp | 60 +++++- radio/src/pulses/afhds3_transport.h | 13 +- radio/src/pulses/crossfire.cpp | 214 ++++++++++++++++----- radio/src/pulses/flysky.cpp | 33 ++-- radio/src/pulses/flysky.h | 2 +- radio/src/pulses/multi.cpp | 21 +- radio/src/pulses/pxx2.cpp | 177 ++++++++++++----- radio/src/telemetry/crossfire.cpp | 87 --------- radio/src/telemetry/crossfire.h | 2 +- radio/src/telemetry/frsky_pxx2.cpp | 48 +---- radio/src/telemetry/telemetry.cpp | 208 ++++---------------- radio/src/telemetry/telemetry.h | 2 +- radio/src/telemetry/telemetry_sensors.cpp | 5 - 17 files changed, 525 insertions(+), 601 deletions(-) diff --git a/radio/src/hal/module_driver.h b/radio/src/hal/module_driver.h index 6b5b21407..fddcb9ec2 100644 --- a/radio/src/hal/module_driver.h +++ b/radio/src/hal/module_driver.h @@ -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); +}; diff --git a/radio/src/pulses/afhds2.cpp b/radio/src/pulses/afhds2.cpp index 3242e867d..ac07d41ee 100644 --- a/radio/src/pulses/afhds2.cpp +++ b/radio/src/pulses/afhds2.cpp @@ -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, }; diff --git a/radio/src/pulses/afhds2.h b/radio/src/pulses/afhds2.h index 39a6e762b..5d978e263 100644 --- a/radio/src/pulses/afhds2.h +++ b/radio/src/pulses/afhds2.h @@ -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; diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 61e675065..a418b3f40 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -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 diff --git a/radio/src/pulses/afhds3_transport.cpp b/radio/src/pulses/afhds3_transport.cpp index 1ecff9865..cedf36e5e 100644 --- a/radio/src/pulses/afhds3_transport.cpp +++ b/radio/src/pulses/afhds3_transport.cpp @@ -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) { diff --git a/radio/src/pulses/afhds3_transport.h b/radio/src/pulses/afhds3_transport.h index 3d16611cd..3103e55b9 100644 --- a/radio/src/pulses/afhds3_transport.h +++ b/radio/src/pulses/afhds3_transport.h @@ -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); }; diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp index 2dc1c3b36..2c892f94a 100644 --- a/radio/src/pulses/crossfire.cpp +++ b/radio/src/pulses/crossfire.cpp @@ -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(¶ms); + auto state = &crossfireState[module]; + state->init(module, &intmodulePulsesData.crossfire, + &IntmoduleSerialDriver, IntmoduleSerialDriver.init(¶ms), 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, }; diff --git a/radio/src/pulses/flysky.cpp b/radio/src/pulses/flysky.cpp index 4c4a7252b..49ac5ad49 100644 --- a/radio/src/pulses/flysky.cpp +++ b/radio/src/pulses/flysky.cpp @@ -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(intmodulePulsesData.flysky.telemetry); - uint8_t dataLen = intmodulePulsesData.flysky.telemetry_index; - + afhds2Resp* resp = reinterpret_cast(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; } } } diff --git a/radio/src/pulses/flysky.h b/radio/src/pulses/flysky.h index 8051a1235..8683222b8 100644 --- a/radio/src/pulses/flysky.h +++ b/radio/src/pulses/flysky.h @@ -26,4 +26,4 @@ void setupPulsesAFHDS2(); void resetPulsesAFHDS2(); -void processInternalFlySkyTelemetryData(uint8_t byte); +void processInternalFlySkyTelemetryData(uint8_t byte, uint8_t* buffer, uint8_t* len); diff --git a/radio/src/pulses/multi.cpp b/radio/src/pulses/multi.cpp index e4b6e6b4c..56a6af860 100644 --- a/radio/src/pulses/multi.cpp +++ b/radio/src/pulses/multi.cpp @@ -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 diff --git a/radio/src/pulses/pxx2.cpp b/radio/src/pulses/pxx2.cpp index 07909c753..4d02dca72 100644 --- a/radio/src/pulses/pxx2.cpp +++ b/radio/src/pulses/pxx2.cpp @@ -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(¶ms); + 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(¶ms)); + + 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(¶ms); + 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 diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 7605c4a44..0a81e6d23 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -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 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 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 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]; diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index e4ed914c9..c8a4ba07e 100644 --- a/radio/src/telemetry/crossfire.h +++ b/radio/src/telemetry/crossfire.h @@ -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); diff --git a/radio/src/telemetry/frsky_pxx2.cpp b/radio/src/telemetry/frsky_pxx2.cpp index 7663b7507..1694b22e0 100644 --- a/radio/src/telemetry/frsky_pxx2.cpp +++ b/radio/src/telemetry/frsky_pxx2.cpp @@ -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); -} diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 342eabf08..c18b28ee4 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -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 * luaInputTelemetryFifo; #endif -void processPXX2TelemetryData(uint8_t data, uint8_t idx); +void processPXX2Frame(uint8_t idx, const uint8_t* frame); // Module pulse synchronization struct ModuleSyncStatus diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp index 7a0f5d528..357e71635 100644 --- a/radio/src/telemetry/telemetry_sensors.cpp +++ b/radio/src/telemetry/telemetry_sensors.cpp @@ -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: