diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index d26bd5d6d0..439be83d22 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -19,7 +19,10 @@ /* FRSKY Telemetry library */ + #include "AP_Frsky_Telem.h" + +#include #include #include @@ -683,6 +686,9 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void) { uint32_t ap_status; + // IMU temperature: offset -19, 0 means temp =< 19°, 63 means temp => 82° + uint8_t imu_temp = (uint8_t) roundf(constrain_float(AP::ins().get_temperature(0), AP_IMU_TEMP_MIN, AP_IMU_TEMP_MAX) - AP_IMU_TEMP_MIN); + // control/flight mode number (limit to 31 (0x1F) since the value is stored on 5 bits) ap_status = (uint8_t)((_ap.control_mode+1) & AP_CONTROL_MODE_LIMIT); // simple/super simple modes flags @@ -695,6 +701,8 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void) ap_status |= (uint8_t)(AP_Notify::flags.failsafe_battery)<