From a0aec17d661938217aaacdb8bfec9b187e8fb260 Mon Sep 17 00:00:00 2001 From: 3djc Date: Sun, 25 May 2025 05:14:29 +0200 Subject: [PATCH] feat(telem): add support for latest CRSF/ELRS sensor definition (#6274) --- radio/src/telemetry/crossfire.cpp | 63 ++++++++++++++++++++++++++- radio/src/telemetry/crossfire.h | 10 ++++- radio/src/telemetry/telemetry.h | 6 +-- radio/src/tests/frsky.cpp | 16 ------- radio/src/translations/untranslated.h | 1 + 5 files changed, 74 insertions(+), 22 deletions(-) diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index cb3207faf..a27dedc3c 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -63,6 +63,10 @@ const CrossfireSensor crossfireSensors[] = { CS(FLIGHT_MODE_ID, 0, STR_SENSOR_FLIGHT_MODE, UNIT_TEXT, 0), CS(CF_VARIO_ID, 0, STR_SENSOR_VSPD, UNIT_METERS_PER_SECOND, 2), CS(BARO_ALT_ID, 0, STR_SENSOR_ALT, UNIT_METERS, 2), + CS(AIRSPEED_ID, 0, STR_SENSOR_ASPD, UNIT_KMH, 1), + CS(CF_RPM_ID, 0, STR_SENSOR_RPM, UNIT_RPMS, 0), + CS(TEMP_ID, 0, STR_SENSOR_TEMP, UNIT_DEGREE, 1), + CS(CELLS_ID, 0, STR_SENSOR_CELLS, UNIT_CELLS, 2), CS(0, 0, "UNKNOWN", UNIT_RAW, 0), }; // clang-format on @@ -89,6 +93,14 @@ const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId) return crossfireSensors[FLIGHT_MODE_INDEX]; else if (id == BARO_ALT_ID) return crossfireSensors[BARO_ALTITUDE_INDEX]; + else if (id == AIRSPEED_ID) + return crossfireSensors[AIRSPEED_INDEX]; + else if (id == CF_RPM_ID) + return crossfireSensors[CF_RPM_INDEX]; + else if (id == TEMP_ID) + return crossfireSensors[TEMP_INDEX]; + else if (id == CELLS_ID) + return crossfireSensors[CELLS_INDEX]; else return crossfireSensors[UNKNOWN_INDEX]; } @@ -172,6 +184,55 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer, processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value); break; + case AIRSPEED_ID: + if (getCrossfireTelemetryValue<2>(3, value, rxBuffer)) { + // Airspeed in 0.1 * km/h (hectometers/h) + // Converstion to KMH is done through PREC1 + processCrossfireTelemetryValue(AIRSPEED_INDEX, value); + } + break; + + case CF_RPM_ID: + { + getCrossfireTelemetryValue<1>(3, value, rxBuffer); + uint8_t sensorID = value; + for(uint8_t i = 0; i * 3 < (crsfPayloadLen - 4); i++) { + getCrossfireTelemetryValue<3>(4 + i * 3, value, rxBuffer); + const CrossfireSensor & sensor = crossfireSensors[CF_RPM_INDEX]; + setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id + (sensorID << 8), 0, i, + value, sensor.unit, sensor.precision); + } + break; + } + + case TEMP_ID: + { + getCrossfireTelemetryValue<1>(3, value, rxBuffer); + uint8_t sensorID = value; + for(uint8_t i = 0; i * 2 < (crsfPayloadLen - 4); i++) { + getCrossfireTelemetryValue<2>(4 + i * 2, value, rxBuffer); + const CrossfireSensor & sensor = crossfireSensors[TEMP_INDEX]; + setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id + (sensorID << 8), 0, i, + value, sensor.unit, sensor.precision); + } + break; + } + + case CELLS_ID: + { + getCrossfireTelemetryValue<1>(3, value, rxBuffer); + uint8_t sensorID = value; + + // We can handle only up to 8 cells + for(uint8_t i = 0; i * 2 < min(16, crsfPayloadLen - 4); i++) { + getCrossfireTelemetryValue<2>(4 + i * 2, value, rxBuffer); + const CrossfireSensor & sensor = crossfireSensors[CELLS_INDEX]; + setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id + (sensorID << 8), 0, 0, + i << 16 | value / 10, sensor.unit, sensor.precision); + } + break; + } + case LINK_ID: for (unsigned int i=0; i<=TX_SNR_INDEX; i++) { if (getCrossfireTelemetryValue<1>(3+i, value, rxBuffer)) { @@ -318,7 +379,7 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer, } } -void crossfireSetDefault(int index, uint8_t id, uint8_t subId) +void crossfireSetDefault(int index, uint16_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 8e7c55c1c..232f1d3b9 100644 --- a/radio/src/telemetry/crossfire.h +++ b/radio/src/telemetry/crossfire.h @@ -35,6 +35,10 @@ #define CF_VARIO_ID 0x07 #define BATTERY_ID 0x08 #define BARO_ALT_ID 0x09 +#define AIRSPEED_ID 0x0A +#define CF_RPM_ID 0x0C +#define TEMP_ID 0x0D +#define CELLS_ID 0x0E #define LINK_ID 0x14 #define CHANNELS_ID 0x16 #define LINK_RX_ID 0x1C @@ -94,6 +98,10 @@ enum CrossfireSensorIndexes { FLIGHT_MODE_INDEX, VERTICAL_SPEED_INDEX, BARO_ALTITUDE_INDEX, + AIRSPEED_INDEX, + CF_RPM_INDEX, + TEMP_INDEX, + CELLS_INDEX, UNKNOWN_INDEX, }; @@ -117,7 +125,7 @@ extern CrossfireModuleStatus crossfireModuleStatus[2]; void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer, uint8_t rxBufferCount); -void crossfireSetDefault(int index, uint8_t id, uint8_t subId); +void crossfireSetDefault(int index, uint16_t id, uint8_t subId); const uint32_t CROSSFIRE_BAUDRATES[] = { 115200, diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index 7ae2ec7bb..acf89dd13 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -110,10 +110,8 @@ PACK(struct CellValue void set(uint16_t newValue) { - if (newValue > 50) { - value = newValue; - state = 1; - } + value = newValue; + state = 1; } }); diff --git a/radio/src/tests/frsky.cpp b/radio/src/tests/frsky.cpp index 8703ffaf6..5a20d838e 100644 --- a/radio/src/tests/frsky.cpp +++ b/radio/src/tests/frsky.cpp @@ -321,22 +321,6 @@ TEST(FrSkySPORT, frskySetCellVoltage) EXPECT_EQ(telemetryItems[0].valueMax, 2071); } -TEST(FrSkySPORT, StrangeCellsBug) -{ - MODEL_RESET(); - TELEMETRY_RESET(); - telemetryStreaming = TELEMETRY_TIMEOUT10ms; - telemetryData.telemetryValid = 0x07; - allowNewSensors = true; - - uint8_t pkt[] = { 0x7E, 0x48, 0x10, 0x00, 0x03, 0x30, 0x15, 0x50, 0x81, 0xD5 }; - EXPECT_EQ(checkSportPacket(pkt+1), true); - sportProcessTelemetryPacket(0, pkt+1, sizeof(pkt) - 1); - EXPECT_EQ(telemetryItems[0].cells.count, 3); - EXPECT_EQ(telemetryItems[0].cells.values[0].value, 0); // now we ignore such low values - EXPECT_EQ(telemetryItems[0].cells.values[1].value, 413); -} - TEST(FrSkySPORT, frskySetCellVoltageTwoSensors) { uint8_t packet[FRSKY_SPORT_PACKET_SIZE]; diff --git a/radio/src/translations/untranslated.h b/radio/src/translations/untranslated.h index 0ca0711e4..95a44d5fb 100644 --- a/radio/src/translations/untranslated.h +++ b/radio/src/translations/untranslated.h @@ -118,6 +118,7 @@ #define STR_SENSOR_A4 "A4" #define STR_SENSOR_BATT "RxBt" #define STR_SENSOR_ALT "Alt" +#define STR_SENSOR_TEMP "Temp" #define STR_SENSOR_TEMP1 "Tmp1" #define STR_SENSOR_TEMP2 "Tmp2" #define STR_SENSOR_TEMP3 "Tmp3"