AP_Frsky_Telem: add IMU temperature reporting

libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@@ -19,7 +19,10 @@
 /* 
    FRSKY Telemetry library
 */
+
 #include "AP_Frsky_Telem.h"
+
+#include <AP_InertialSensor/AP_InertialSensor.h>
 #include <GCS_MAVLink/GCS.h>
 
 #include <stdio.h>
@@ -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)<<AP_BATT_FS_OFFSET;
     // bad ekf flag
     ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<<AP_EKF_FS_OFFSET;
+    // IMU temperature
+    ap_status |= imu_temp << AP_IMU_TEMP_OFFSET;
     return ap_status;
 }
 

libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

@@ -95,6 +95,9 @@ for FrSky SPort Passthrough
 #define AP_ARMED_OFFSET             8
 #define AP_BATT_FS_OFFSET           9
 #define AP_EKF_FS_OFFSET            10
+#define AP_IMU_TEMP_MIN             19.0f
+#define AP_IMU_TEMP_MAX             82.0f
+#define AP_IMU_TEMP_OFFSET          26
 // for home position related data
 #define HOME_ALT_OFFSET             12
 #define HOME_BEARING_LIMIT          0x7F