Moved telemetry processing to module interface
This commit is contained in:
committed by
Raphael Coeffic
parent
39c61408e7
commit
246840f065
+168
-46
@@ -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,
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user