diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 7feaa28ee..3deb3319c 100644 --- 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 index 9afd6e264..e4ed914c9 100644 --- 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, };