From 67fb0859501469729dba0582751d5876aa2a9b39 Mon Sep 17 00:00:00 2001
From: Michal Herko <michal@herko.it>
Date: Wed, 12 Feb 2025 16:56:28 +0000
Subject: [PATCH] add barometer altitude to crsf telemetry

---
 src/main/rx/crsf.h        |  2 ++
 src/main/telemetry/crsf.c | 38 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 40 insertions(+)

diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
index 69777a0390..c5136e3b7a 100755
--- a/src/main/rx/crsf.h
+++ b/src/main/rx/crsf.h
@@ -45,6 +45,7 @@ enum {
     CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
     CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
+    CRSF_FRAME_BAROMETER_ALTITUDE_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,
@@ -86,6 +87,7 @@ typedef enum {
     CRSF_FRAMETYPE_GPS = 0x02,
     CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
+    CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
     CRSF_FRAMETYPE_ATTITUDE = 0x1E,
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 8b7f289db2..3a804485f2 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -258,6 +258,33 @@ static void crsfFrameBatterySensor(sbuf_t *dst)
     crsfSerialize8(dst, batteryRemainingPercentage);
 }
 
+const int32_t ALT_MIN_DM = 10000;
+const int32_t ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM;
+const int32_t ALT_MAX_DM = 0x7ffe * 10 - 5;
+
+/*
+0x09 Barometer altitude and vertical speed
+Payload:
+uint16_t    altitude_packed ( dm - 10000 )
+*/
+static void crsfBarometerAltitude(sbuf_t *dst)
+{
+    int32_t altitude_dm = lrintf(getEstimatedActualPosition(Z) / 10);
+    uint16_t altitude_packed;
+    if (altitude_dm < -ALT_MIN_DM) {
+        altitude_packed = 0;
+    } else if (altitude_dm > ALT_MAX_DM) {
+        altitude_packed = 0xfffe;
+    } else if (altitude_dm < ALT_THRESHOLD_DM) {
+        altitude_packed = altitude_dm + ALT_MIN_DM;
+    } else {
+        altitude_packed = ((altitude_dm + 5) / 10) | 0x8000;
+    }
+    sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE);
+    crsfSerialize16(dst, altitude_packed);
+}
+
 typedef enum {
     CRSF_ACTIVE_ANTENNA1 = 0,
     CRSF_ACTIVE_ANTENNA2 = 1
@@ -415,6 +442,7 @@ typedef enum {
     CRSF_FRAME_FLIGHT_MODE_INDEX,
     CRSF_FRAME_GPS_INDEX,
     CRSF_FRAME_VARIO_SENSOR_INDEX,
+    CRSF_FRAME_BAROMETER_ALTITUDE_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
@@ -481,6 +509,11 @@ static void processCrsf(void)
         crsfFrameVarioSensor(dst);
         crsfFinalize(dst);
     }
+    if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfBarometerAltitude(dst);
+        crsfFinalize(dst);
+    }
 #endif
     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
@@ -514,6 +547,11 @@ void initCrsfTelemetry(void)
     if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
         crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
     }
+#endif
+#ifdef USE_BARO
+    if (sensors(SENSOR_BARO)) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX);
+    }
 #endif
     crsfScheduleCount = (uint8_t)index;
 }