Crossfire: add barometer altitude sensor

EdgeTX · May 7, 2022 · 7806d5f · 7806d5f
1 parent 60fbe5e
commit 7806d5f
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 0 deletions.
diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp
@@ -53,6 +53,7 @@ const CrossfireSensor crossfireSensors[] = {
   {ATTITUDE_ID,    2, STR_SENSOR_YAW,           UNIT_RADIANS,           3},
   {FLIGHT_MODE_ID, 0, STR_SENSOR_FLIGHT_MODE,   UNIT_TEXT,              0},
   {CF_VARIO_ID,    0, STR_SENSOR_VSPD,          UNIT_METERS_PER_SECOND, 2},
+  {BARO_ALT_ID,    0, STR_SENSOR_ALT,           UNIT_METERS,            2},
   {0,              0, "UNKNOWN",          UNIT_RAW,               0},
 };
 
@@ -74,6 +75,8 @@ const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
     return crossfireSensors[ATTITUDE_PITCH_INDEX+subId];
   else if (id == FLIGHT_MODE_ID)
     return crossfireSensors[FLIGHT_MODE_INDEX];
+  else if (id == BARO_ALT_ID)
+    return crossfireSensors[BARO_ALTITUDE_INDEX];
   else
     return crossfireSensors[UNKNOWN_INDEX];
 }
@@ -146,6 +149,21 @@ void processCrossfireTelemetryFrame(uint8_t module)
         processCrossfireTelemetryValue(GPS_SATELLITES_INDEX, value);
       break;
 
+    case BARO_ALT_ID:
+      if (getCrossfireTelemetryValue<2>(3, value, module)) {
+        if (value & 0x8000) {
+          // Altitude in meters
+          value &= ~(0x8000);
+          value *= 100; // cm
+        } else {
+          // Altitude in decimeters + 10000dm
+          value -= 10000;
+          value *= 10;
+        }
+        processCrossfireTelemetryValue(BARO_ALTITUDE_INDEX, value);
+      }
+      break;
+
     case LINK_ID:
       for (unsigned int i=0; i<=TX_SNR_INDEX; i++) {
         if (getCrossfireTelemetryValue<1>(3+i, value, module)) {

diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h
@@ -34,6 +34,7 @@
 #define GPS_ID                         0x02
 #define CF_VARIO_ID                    0x07
 #define BATTERY_ID                     0x08
+#define BARO_ALT_ID                    0x09
 #define LINK_ID                        0x14
 #define CHANNELS_ID                    0x16
 #define LINK_RX_ID                     0x1C
@@ -89,6 +90,7 @@ enum CrossfireSensorIndexes {
   ATTITUDE_YAW_INDEX,
   FLIGHT_MODE_INDEX,
   VERTICAL_SPEED_INDEX,
+  BARO_ALTITUDE_INDEX,
   UNKNOWN_INDEX,
 };