From c952e58edb027f9fce63bfa2c9b4b184243389c2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 Mar 2017 22:09:16 +1100
Subject: [PATCH] AP_InertialSensor: fixed invensense driver temp reading

different parts have quite different zero offsets in temperature
---
 .../AP_InertialSensor_Invensense.cpp          | 28 +++++++++++++++++--
 .../AP_InertialSensor_Invensense.h            |  3 ++
 2 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
index 292eea933c..5c2857bd34 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
@@ -406,6 +406,30 @@ void AP_InertialSensor_Invensense::start()
         adev = DEVTYPE_ACC_MPU6000;
         break;
     }
+
+    /*
+      setup temperature sensitivity and offset. This varies
+      considerably between parts
+     */
+    switch (_mpu_type) {
+    case Invensense_MPU9250:
+        temp_zero = 21;
+        temp_sensitivity = 1.0/340;
+        break;
+
+    case Invensense_MPU6000:
+    case Invensense_MPU6500:
+        temp_zero = 36.53;
+        temp_sensitivity = 1.0/340;
+        break;
+
+    case Invensense_ICM20608:
+    case Invensense_ICM20602:
+        temp_zero = 25;
+        temp_sensitivity = 1.0/326.8; 
+        break;
+    }
+
     _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev));
     _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev));
 
@@ -553,7 +577,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
             _fifo_reset();
             return false;
         }
-        float temp = t2/340.0f + 36.53f;
+        float temp = t2 * temp_sensitivity + temp_zero;
         
         gyro = Vector3f(int16_val(data, 5),
                         int16_val(data, 4),
@@ -643,7 +667,7 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u
     }
 
     if (ret) {
-        float temp = (static_cast<float>(tsum)/n_samples)/340.0f + 36.53f;
+        float temp = (static_cast<float>(tsum)/n_samples)*temp_sensitivity + temp_zero;
         _temp_filtered = _temp_filter.apply(temp);
     }
     
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
index 1b2eeec628..16e23b85e0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
@@ -106,6 +106,9 @@ private:
 
     uint16_t _error_count;
 
+    float temp_sensitivity = 1.0/340; // degC/LSB
+    float temp_zero = 36.53; // degC
+    
     float _temp_filtered;
     float _accel_scale;
     LowPassFilter2pFloat _temp_filter;