perf: Process CRSF telemetry by frame (#4038)

Co-authored-by: 3djc <lesitewebdejc@hotmail.com>
Co-authored-by: Peter Feerick <peter.feerick@gmail.com>
This commit is contained in:
Raphael Coeffic
2023-10-19 13:31:30 +10:00
committed by GitHub
co-authored by 3djc Peter Feerick
parent eaabb2ce97
commit c93695eeaa
31 changed files with 418 additions and 169 deletions
+33 -34
View File
@@ -94,10 +94,10 @@ void processCrossfireTelemetryValue(uint8_t index, int32_t value)
value, sensor.unit, sensor.precision);
}
template<int N>
bool getCrossfireTelemetryValue(uint8_t index, int32_t & value, uint8_t module)
template <int N>
bool getCrossfireTelemetryValue(uint8_t index, int32_t& value,
uint8_t* rxBuffer)
{
uint8_t * rxBuffer = getTelemetryRxBuffer(module);
bool result = false;
uint8_t * byte = &rxBuffer[index];
value = (*byte & 0x80) ? -1 : 0;
@@ -111,11 +111,9 @@ bool getCrossfireTelemetryValue(uint8_t index, int32_t & value, uint8_t module)
return result;
}
void processCrossfireTelemetryFrame(uint8_t module)
void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
uint8_t rxBufferCount)
{
uint8_t * rxBuffer = getTelemetryRxBuffer(module);
uint8_t &rxBufferCount = getTelemetryRxBufferCount(module);
if (telemetryState == TELEMETRY_INIT &&
moduleState[module].counter != CRSF_FRAME_MODELID_SENT) {
moduleState[module].counter = CRSF_FRAME_MODELID;
@@ -126,27 +124,27 @@ void processCrossfireTelemetryFrame(uint8_t module)
int32_t value;
switch(id) {
case CF_VARIO_ID:
if (getCrossfireTelemetryValue<2>(3, value, module))
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer))
processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value);
break;
case GPS_ID:
if (getCrossfireTelemetryValue<4>(3, value, module))
if (getCrossfireTelemetryValue<4>(3, value, rxBuffer))
processCrossfireTelemetryValue(GPS_LATITUDE_INDEX, value/10);
if (getCrossfireTelemetryValue<4>(7, value, module))
if (getCrossfireTelemetryValue<4>(7, value, rxBuffer))
processCrossfireTelemetryValue(GPS_LONGITUDE_INDEX, value/10);
if (getCrossfireTelemetryValue<2>(11, value, module))
if (getCrossfireTelemetryValue<2>(11, value, rxBuffer))
processCrossfireTelemetryValue(GPS_GROUND_SPEED_INDEX, value);
if (getCrossfireTelemetryValue<2>(13, value, module))
if (getCrossfireTelemetryValue<2>(13, value, rxBuffer))
processCrossfireTelemetryValue(GPS_HEADING_INDEX, value);
if (getCrossfireTelemetryValue<2>(15, value, module))
if (getCrossfireTelemetryValue<2>(15, value, rxBuffer))
processCrossfireTelemetryValue(GPS_ALTITUDE_INDEX, value - 1000);
if (getCrossfireTelemetryValue<1>(17, value, module))
if (getCrossfireTelemetryValue<1>(17, value, rxBuffer))
processCrossfireTelemetryValue(GPS_SATELLITES_INDEX, value);
break;
case BARO_ALT_ID:
if (getCrossfireTelemetryValue<2>(3, value, module)) {
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer)) {
if (value & 0x8000) {
// Altitude in meters
value &= ~(0x8000);
@@ -160,13 +158,14 @@ void processCrossfireTelemetryFrame(uint8_t module)
}
// Length of TBS BARO_ALT has 4 payload bytes with just 2 bytes of altitude
// but support including VARIO if the declared payload length is 6 bytes or more
if (crsfPayloadLen > 5 && getCrossfireTelemetryValue<2>(5, value, module))
if (crsfPayloadLen > 5 &&
getCrossfireTelemetryValue<2>(5, value, rxBuffer))
processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value);
break;
case LINK_ID:
for (unsigned int i=0; i<=TX_SNR_INDEX; i++) {
if (getCrossfireTelemetryValue<1>(3+i, value, module)) {
if (getCrossfireTelemetryValue<1>(3+i, value, rxBuffer)) {
if (i == TX_POWER_INDEX) {
static const int32_t power_values[] = {0, 10, 25, 100, 500,
1000, 2000, 250, 50};
@@ -193,38 +192,38 @@ void processCrossfireTelemetryFrame(uint8_t module)
break;
case LINK_RX_ID:
if (getCrossfireTelemetryValue<1>(4, value, module))
if (getCrossfireTelemetryValue<1>(4, value, rxBuffer))
processCrossfireTelemetryValue(RX_RSSI_PERC_INDEX, value);
if (getCrossfireTelemetryValue<1>(7, value, module))
if (getCrossfireTelemetryValue<1>(7, value, rxBuffer))
processCrossfireTelemetryValue(TX_RF_POWER_INDEX, value);
break;
case LINK_TX_ID:
if (getCrossfireTelemetryValue<1>(4, value, module))
if (getCrossfireTelemetryValue<1>(4, value, rxBuffer))
processCrossfireTelemetryValue(TX_RSSI_PERC_INDEX, value);
if (getCrossfireTelemetryValue<1>(7, value, module))
if (getCrossfireTelemetryValue<1>(7, value, rxBuffer))
processCrossfireTelemetryValue(RX_RF_POWER_INDEX, value);
if (getCrossfireTelemetryValue<1>(8, value, module))
if (getCrossfireTelemetryValue<1>(8, value, rxBuffer))
processCrossfireTelemetryValue(TX_FPS_INDEX, value * 10);
break;
case BATTERY_ID:
if (getCrossfireTelemetryValue<2>(3, value, module))
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer))
processCrossfireTelemetryValue(BATT_VOLTAGE_INDEX, value);
if (getCrossfireTelemetryValue<2>(5, value, module))
if (getCrossfireTelemetryValue<2>(5, value, rxBuffer))
processCrossfireTelemetryValue(BATT_CURRENT_INDEX, value);
if (getCrossfireTelemetryValue<3>(7, value, module))
if (getCrossfireTelemetryValue<3>(7, value, rxBuffer))
processCrossfireTelemetryValue(BATT_CAPACITY_INDEX, value);
if (getCrossfireTelemetryValue<1>(10, value, module))
if (getCrossfireTelemetryValue<1>(10, value, rxBuffer))
processCrossfireTelemetryValue(BATT_REMAINING_INDEX, value);
break;
case ATTITUDE_ID:
if (getCrossfireTelemetryValue<2>(3, value, module))
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer))
processCrossfireTelemetryValue(ATTITUDE_PITCH_INDEX, value/10);
if (getCrossfireTelemetryValue<2>(5, value, module))
if (getCrossfireTelemetryValue<2>(5, value, rxBuffer))
processCrossfireTelemetryValue(ATTITUDE_ROLL_INDEX, value/10);
if (getCrossfireTelemetryValue<2>(7, value, module))
if (getCrossfireTelemetryValue<2>(7, value, rxBuffer))
processCrossfireTelemetryValue(ATTITUDE_YAW_INDEX, value/10);
break;
@@ -245,13 +244,13 @@ void processCrossfireTelemetryFrame(uint8_t module)
uint32_t update_interval;
int32_t offset;
if (getCrossfireTelemetryValue<4>(6, (int32_t &)update_interval,
module) &&
getCrossfireTelemetryValue<4>(10, offset, module)) {
rxBuffer) &&
getCrossfireTelemetryValue<4>(10, offset, rxBuffer)) {
// values are in 10th of micro-seconds
update_interval /= 10;
offset /= 10;
TRACE("[XF] Rate: %d, Lag: %d", update_interval, offset);
//TRACE("[XF] Rate: %d, Lag: %d", update_interval, offset);
getModuleSyncStatus(module).update(update_interval, offset);
}
}
@@ -259,8 +258,8 @@ void processCrossfireTelemetryFrame(uint8_t module)
#if defined(LUA)
default:
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(rxBufferCount-2) ) {
for (uint8_t i=1; i<rxBufferCount-1; i++) {
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(rxBufferCount - 2)) {
for (uint8_t i = 1; i < rxBufferCount - 1; i++) {
// destination address and CRC are skipped
luaInputTelemetryFifo->push(rxBuffer[i]);
}
+4 -2
View File
@@ -100,9 +100,11 @@ enum CrossfireFrames{
CRSF_FRAME_MODELID_SENT
};
void processCrossfireTelemetryFrame(uint8_t module);
void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
uint8_t rxBufferCount);
void crossfireSetDefault(int index, uint8_t id, uint8_t subId);
uint8_t createCrossfireModelIDFrame(uint8_t * frame);
uint8_t createCrossfireModelIDFrame(uint8_t* frame);
const uint32_t CROSSFIRE_BAUDRATES[] = {
115200,
+49 -29
View File
@@ -153,30 +153,10 @@ rxStatStruct *getRxStatLabels() {
return &rxStat;
}
// TODO: move to module port driver
//
// static int (*_telemetryGetByte)(void*, uint8_t*) = nullptr;
// static void* _telemetryGetByteCtx = nullptr;
// void telemetrySetGetByte(void* ctx, int (*fct)(void*, uint8_t*))
// {
// _telemetryGetByte = nullptr;
// _telemetryGetByteCtx = ctx;
// _telemetryGetByte = fct;
// }
// static bool telemetryGetByte(uint8_t* data)
// {
// auto _getByte = _telemetryGetByte;
// auto _ctx = _telemetryGetByteCtx;
// if (_getByte) {
// return _getByte(_ctx, data);
// }
// // return sportGetByte(data);
// return false;
// }
// This can only be changed when the mixer is not
// running as the priority of the timer task is
// lower.
volatile uint8_t _telemetryIsPolling = false;
static void (*telemetryMirrorSendByte)(void*, uint8_t) = nullptr;
static void* telemetryMirrorSendByteCtx = nullptr;
@@ -234,6 +214,51 @@ void telemetryStop()
}
}
}
static void _poll_frame(void *pvParameter1, uint32_t ulParameter2)
{
_telemetryIsPolling = true;
auto drv = (const etx_proto_driver_t*)pvParameter1;
auto module = (uint8_t)ulParameter2;
auto mod = pulsesGetModuleDriver(module);
if (!mod || !mod->drv || !mod->ctx || (drv != mod->drv))
return;
auto ctx = mod->ctx;
auto mod_st = (etx_module_state_t*)ctx;
auto serial_drv = modulePortGetSerialDrv(mod_st->rx);
auto serial_ctx = modulePortGetCtx(mod_st->rx);
if (!serial_drv || !serial_ctx || !serial_drv->copyRxBuffer)
return;
uint8_t frame[TELEMETRY_RX_PACKET_SIZE];
int frame_len = serial_drv->copyRxBuffer(serial_ctx, frame, TELEMETRY_RX_PACKET_SIZE);
if (frame_len > 0) {
LOG_TELEMETRY_WRITE_START();
for (int i = 0; i < frame_len; i++) {
telemetryMirrorSend(frame[i]);
LOG_TELEMETRY_WRITE_BYTE(frame[i]);
}
uint8_t* rxBuffer = getTelemetryRxBuffer(module);
uint8_t& rxBufferCount = getTelemetryRxBufferCount(module);
drv->processFrame(ctx, frame, frame_len, rxBuffer, &rxBufferCount);
}
_telemetryIsPolling = false;
}
void telemetryFrameTrigger_ISR(uint8_t module, const etx_proto_driver_t* drv)
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xTimerPendFunctionCallFromISR(_poll_frame, (void*)drv, module, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}
#endif
inline bool isBadAntennaDetected()
@@ -277,11 +302,6 @@ static inline void pollTelemetry(uint8_t module, const etx_proto_driver_t* drv,
}
}
// This can only be changed when the mixer is not
// running as the priority of the timer task is
// lower.
volatile uint8_t _telemetryIsPolling = false;
void telemetryWakeup()
{
_telemetryIsPolling = true;
+6
View File
@@ -81,6 +81,12 @@ void telemetryInterrupt10ms();
void telemetryStart();
void telemetryStop();
struct etx_proto_driver_t;
// Call from ISR to schedule telemetry frame
// processing for that module.
void telemetryFrameTrigger_ISR(uint8_t module, const etx_proto_driver_t* drv);
#define TELEMETRY_AVERAGE_COUNT 3
enum {