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:
co-authored by
3djc
Peter Feerick
parent
eaabb2ce97
commit
c93695eeaa
@@ -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]);
|
||||
}
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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 {
|
||||
|
||||
Reference in New Issue
Block a user