Moved telemetry processing to module interface

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
+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,
};