Fix CRSF telemetry corruption from PR #11025

src/main/rx/crsf.h

@@ -46,6 +46,7 @@ enum {
     CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
     CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2,
+    CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE = 2,
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
     CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
@@ -88,6 +89,9 @@ typedef enum {
     CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
     CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09,
+    CRSF_FRAMETYPE_AIRSPEED_SENSOR = 0x0A,
+    CRSF_FRAMETYPE_RPM = 0x0C,
+    CRSF_FRAMETYPE_TEMP = 0x0D,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
     CRSF_FRAMETYPE_ATTITUDE = 0x1E,

src/main/telemetry/crsf.c

@@ -46,6 +46,7 @@
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"
+#include "flight/mixer.h"
 
 #include "io/gps.h"
 #include "io/serial.h"
@@ -56,7 +57,10 @@
 #include "rx/rx.h"
 
 #include "sensors/battery.h"
+#include "sensors/esc_sensor.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/sensors.h"
+#include "sensors/temperature.h"
 
 #include "telemetry/crsf.h"
 #include "telemetry/telemetry.h"
@@ -162,6 +166,16 @@ static void crsfSerialize16(sbuf_t *dst, uint16_t v)
     crsfSerialize8(dst, (uint8_t)v);
 }
 
+#ifdef USE_ESC_SENSOR
+static void crsfSerialize24(sbuf_t *dst, uint32_t v)
+{
+    // Use BigEndian format
+    crsfSerialize8(dst, (v >> 16));
+    crsfSerialize8(dst, (v >> 8));
+    crsfSerialize8(dst, (uint8_t)v);
+}
+#endif
+
 static void crsfSerialize32(sbuf_t *dst, uint32_t v)
 {
     // Use BigEndian format
@@ -296,6 +310,92 @@ static void crsfBarometerAltitude(sbuf_t *dst)
     crsfSerialize16(dst, altitude_packed);
 }
 
+#ifdef USE_PITOT
+/*
+0x0A Airspeed sensor
+Payload:
+int16      Air speed ( dm/s )
+*/
+static void crsfFrameAirSpeedSensor(sbuf_t *dst)
+{
+    // use sbufWrite since CRC does not include frame length
+    sbufWriteU8(dst, CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_AIRSPEED_SENSOR);
+    crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36 / 100));
+}
+#endif
+
+#ifdef USE_ESC_SENSOR
+/*
+0x0C RPM
+Payload:
+uint8_t    rpm_source_id;  // Identifies the source of the RPM data (e.g., 0 = Motor 1, 1 = Motor 2, etc.)
+int24_t    rpm_value[];     // 1 - 19 RPM values with negative ones representing the motor spinning in reverse
+*/
+static void crsfRpm(sbuf_t *dst)
+{
+    const uint8_t MAX_CRSF_RPM_VALUES = 19;  // CRSF protocol limit: 1-19 RPM values
+    uint8_t motorCount = getMotorCount();
+
+    if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
+        // Enforce protocol limit
+        if (motorCount > MAX_CRSF_RPM_VALUES) {
+            motorCount = MAX_CRSF_RPM_VALUES;
+        }
+
+        sbufWriteU8(dst, 1 + (motorCount * 3) + CRSF_FRAME_LENGTH_TYPE_CRC);
+        crsfSerialize8(dst, CRSF_FRAMETYPE_RPM);
+        // 0 = FC including all ESCs
+        crsfSerialize8(dst, 0);
+
+        for (uint8_t i = 0; i < motorCount; i++) {
+            const escSensorData_t *escState = getEscTelemetry(i);
+            crsfSerialize24(dst, (escState) ? escState->rpm : 0);
+        }
+    }
+}
+#endif
+
+/*
+0x0D TEMP
+Payload:
+uint8_t temp_source_id; // Identifies the source of the temperature data (e.g., 0 = FC including all ESCs, 1 = Ambient, etc.)
+int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of a degree) Celsius (e.g., 250 = 25.0°C, -50 = -5.0°C)
+*/
+static void crsfTemperature(sbuf_t *dst)
+{
+    const uint8_t MAX_CRSF_TEMPS = 20;  // Maximum temperatures per CRSF frame
+    uint8_t tempCount = 0;
+    int16_t temperatures[20];
+
+#ifdef USE_ESC_SENSOR
+    uint8_t motorCount = getMotorCount();
+    if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
+        for (uint8_t i = 0; i < motorCount && tempCount < MAX_CRSF_TEMPS; i++) {
+            const escSensorData_t *escState = getEscTelemetry(i);
+            temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE;
+        }
+    }
+#endif
+
+#ifdef USE_TEMPERATURE_SENSOR
+    for (uint8_t i = 0; i < MAX_TEMP_SENSORS && tempCount < MAX_CRSF_TEMPS; i++) {
+        int16_t value;
+        if (getSensorTemperature(i, &value))
+            temperatures[tempCount++] = value;
+    }
+#endif
+
+    if (tempCount > 0) {
+        sbufWriteU8(dst, 1 + (tempCount * 2) + CRSF_FRAME_LENGTH_TYPE_CRC);
+        crsfSerialize8(dst, CRSF_FRAMETYPE_TEMP);
+        // 0 = FC including all ESCs
+        crsfSerialize8(dst, 0);
+        for (uint8_t i = 0; i < tempCount; i++)
+            crsfSerialize16(dst, temperatures[i]);
+    }
+}
+
 typedef enum {
     CRSF_ACTIVE_ANTENNA1 = 0,
     CRSF_ACTIVE_ANTENNA2 = 1
@@ -465,11 +565,14 @@ typedef enum {
     CRSF_FRAME_GPS_INDEX,
     CRSF_FRAME_VARIO_SENSOR_INDEX,
     CRSF_FRAME_BAROMETER_ALTITUDE_INDEX,
+    CRSF_FRAME_TEMP_INDEX,
+    CRSF_FRAME_RPM_INDEX,
+    CRSF_FRAME_AIRSPEED_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
 static uint8_t crsfScheduleCount;
-static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
+static uint16_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 
 #if defined(USE_MSP_OVER_TELEMETRY)
 
@@ -506,7 +609,7 @@ static void processCrsf(void)
     }
 
     static uint8_t crsfScheduleIndex = 0;
-    const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
+    const uint16_t currentSchedule = crsfSchedule[crsfScheduleIndex];
 
     sbuf_t crsfPayloadBuf;
     sbuf_t *dst = &crsfPayloadBuf;
@@ -526,6 +629,20 @@ static void processCrsf(void)
         crsfFrameFlightMode(dst);
         crsfFinalize(dst);
     }
+#ifdef USE_ESC_SENSOR
+    if (currentSchedule & BV(CRSF_FRAME_RPM_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfRpm(dst);
+        crsfFinalize(dst);
+    }
+#endif
+#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR)
+    if (currentSchedule & BV(CRSF_FRAME_TEMP_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfTemperature(dst);
+        crsfFinalize(dst);
+    }
+#endif
 #ifdef USE_GPS
     if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) {
         crsfInitializeFrame(dst);
@@ -544,6 +661,13 @@ static void processCrsf(void)
         crsfBarometerAltitude(dst);
         crsfFinalize(dst);
     }
+#endif
+#ifdef USE_PITOT
+    if (currentSchedule & BV(CRSF_FRAME_AIRSPEED_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfFrameAirSpeedSensor(dst);
+        crsfFinalize(dst);
+    }
 #endif
     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
@@ -582,6 +706,40 @@ void initCrsfTelemetry(void)
     if (sensors(SENSOR_BARO)) {
         crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX);
     }
+#endif
+#ifdef USE_ESC_SENSOR
+    if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX);
+    }
+#endif
+#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR)
+    // Only schedule temperature frame if we have temperature sources available
+    bool hasTemperatureSources = false;
+#ifdef USE_ESC_SENSOR
+    if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
+        hasTemperatureSources = true;
+    }
+#endif
+#ifdef USE_TEMPERATURE_SENSOR
+    if (!hasTemperatureSources) {
+        // Check if any temperature sensors are configured
+        for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) {
+            int16_t value;
+            if (getSensorTemperature(i, &value)) {
+                hasTemperatureSources = true;
+                break;
+            }
+        }
+    }
+#endif
+    if (hasTemperatureSources) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX);
+    }
+#endif
+#ifdef USE_PITOT
+    if (sensors(SENSOR_PITOT)) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_AIRSPEED_INDEX);
+    }
 #endif
     crsfScheduleCount = (uint8_t)index;
 }