diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 72165f174..e3168cda6 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -21,30 +21,31 @@ #include "opentx.h" const CrossfireSensor crossfireSensors[] = { - {LINK_ID, 0, ZSTR_RX_RSSI1, UNIT_DB, 0}, - {LINK_ID, 1, ZSTR_RX_RSSI2, UNIT_DB, 0}, - {LINK_ID, 2, ZSTR_RX_QUALITY, UNIT_PERCENT, 0}, - {LINK_ID, 3, ZSTR_RX_SNR, UNIT_DB, 0}, - {LINK_ID, 4, ZSTR_ANTENNA, UNIT_RAW, 0}, - {LINK_ID, 5, ZSTR_RF_MODE, UNIT_RAW, 0}, - {LINK_ID, 6, ZSTR_TX_POWER, UNIT_MILLIWATTS, 0}, - {LINK_ID, 7, ZSTR_TX_RSSI, UNIT_DB, 0}, - {LINK_ID, 8, ZSTR_TX_QUALITY, UNIT_PERCENT, 0}, - {LINK_ID, 9, ZSTR_TX_SNR, UNIT_DB, 0}, - {BATTERY_ID, 0, ZSTR_BATT, UNIT_VOLTS, 1}, - {BATTERY_ID, 1, ZSTR_CURR, UNIT_AMPS, 1}, - {BATTERY_ID, 2, ZSTR_CAPACITY, UNIT_MAH, 0}, - {GPS_ID, 0, ZSTR_GPS, UNIT_GPS_LATITUDE, 0}, - {GPS_ID, 0, ZSTR_GPS, UNIT_GPS_LONGITUDE, 0}, - {GPS_ID, 2, ZSTR_GSPD, UNIT_KMH, 1}, - {GPS_ID, 3, ZSTR_HDG, UNIT_DEGREE, 3}, - {GPS_ID, 4, ZSTR_ALT, UNIT_METERS, 0}, - {GPS_ID, 5, ZSTR_SATELLITES, UNIT_RAW, 0}, - {ATTITUDE_ID, 0, ZSTR_PITCH, UNIT_RADIANS, 3}, - {ATTITUDE_ID, 1, ZSTR_ROLL, UNIT_RADIANS, 3}, - {ATTITUDE_ID, 2, ZSTR_YAW, UNIT_RADIANS, 3}, - {FLIGHT_MODE_ID, 0, ZSTR_FLIGHT_MODE, UNIT_TEXT, 0}, - {0, 0, "UNKNOWN", UNIT_RAW, 0}, + {LINK_ID, 0, ZSTR_RX_RSSI1, UNIT_DB, 0}, + {LINK_ID, 1, ZSTR_RX_RSSI2, UNIT_DB, 0}, + {LINK_ID, 2, ZSTR_RX_QUALITY, UNIT_PERCENT, 0}, + {LINK_ID, 3, ZSTR_RX_SNR, UNIT_DB, 0}, + {LINK_ID, 4, ZSTR_ANTENNA, UNIT_RAW, 0}, + {LINK_ID, 5, ZSTR_RF_MODE, UNIT_RAW, 0}, + {LINK_ID, 6, ZSTR_TX_POWER, UNIT_MILLIWATTS, 0}, + {LINK_ID, 7, ZSTR_TX_RSSI, UNIT_DB, 0}, + {LINK_ID, 8, ZSTR_TX_QUALITY, UNIT_PERCENT, 0}, + {LINK_ID, 9, ZSTR_TX_SNR, UNIT_DB, 0}, + {BATTERY_ID, 0, ZSTR_BATT, UNIT_VOLTS, 1}, + {BATTERY_ID, 1, ZSTR_CURR, UNIT_AMPS, 1}, + {BATTERY_ID, 2, ZSTR_CAPACITY, UNIT_MAH, 0}, + {GPS_ID, 0, ZSTR_GPS, UNIT_GPS_LATITUDE, 0}, + {GPS_ID, 0, ZSTR_GPS, UNIT_GPS_LONGITUDE, 0}, + {GPS_ID, 2, ZSTR_GSPD, UNIT_KMH, 1}, + {GPS_ID, 3, ZSTR_HDG, UNIT_DEGREE, 3}, + {GPS_ID, 4, ZSTR_ALT, UNIT_METERS, 0}, + {GPS_ID, 5, ZSTR_SATELLITES, UNIT_RAW, 0}, + {ATTITUDE_ID, 0, ZSTR_PITCH, UNIT_RADIANS, 3}, + {ATTITUDE_ID, 1, ZSTR_ROLL, UNIT_RADIANS, 3}, + {ATTITUDE_ID, 2, ZSTR_YAW, UNIT_RADIANS, 3}, + {FLIGHT_MODE_ID, 0, ZSTR_FLIGHT_MODE, UNIT_TEXT, 0}, + {CF_VARIO_ID, 0, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2}, + {0, 0, "UNKNOWN", UNIT_RAW, 0}, }; const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId) @@ -55,6 +56,8 @@ const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId) return crossfireSensors[BATT_VOLTAGE_INDEX+subId]; else if (id == GPS_ID) return crossfireSensors[GPS_LATITUDE_INDEX+subId]; + else if (id == CF_VARIO_ID) + return crossfireSensors[VERTICAL_SPEED_INDEX]; else if (id == ATTITUDE_ID) return crossfireSensors[ATTITUDE_PITCH_INDEX+subId]; else if (id == FLIGHT_MODE_ID) @@ -102,6 +105,11 @@ void processCrossfireTelemetryFrame() uint8_t id = telemetryRxBuffer[2]; int32_t value; switch(id) { + case CF_VARIO_ID: + if (getCrossfireTelemetryValue<2>(3, value)) + processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value); + break; + case GPS_ID: if (getCrossfireTelemetryValue<4>(3, value)) processCrossfireTelemetryValue(GPS_LATITUDE_INDEX, value/10); diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index c627d580e..5f40b6f93 100644 --- a/radio/src/telemetry/crossfire.h +++ b/radio/src/telemetry/crossfire.h @@ -31,6 +31,7 @@ // Frame id #define GPS_ID 0x02 +#define CF_VARIO_ID 0x07 #define BATTERY_ID 0x08 #define LINK_ID 0x14 #define CHANNELS_ID 0x16 @@ -73,6 +74,7 @@ enum CrossfireSensorIndexes { ATTITUDE_ROLL_INDEX, ATTITUDE_YAW_INDEX, FLIGHT_MODE_INDEX, + VERTICAL_SPEED_INDEX, UNKNOWN_INDEX, };