From 1c60417d03d134eafb4d19effcab6f17ba552e69 Mon Sep 17 00:00:00 2001
From: Jochen Anglett <info@zentechnology.de>
Date: Fri, 21 Sep 2018 08:22:04 +0200
Subject: [PATCH] AP_Frsky_Telem: add IMU temperature reporting

---
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 8 ++++++++
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.h   | 3 +++
 2 files changed, 11 insertions(+)

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 <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;
 }
 
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
index 238e8d54f9..1886711af3 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
+++ b/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