AP_InertialSensor: Added hook to read the temperature compensation enum and control temp compensation

Author: ohyaiamhere

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1848,14 +1848,18 @@ void AP_InertialSensor::_save_gyro_calibration()
         _gyro_offset(i).save();
         _gyro_id(i).save();
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
-        caltemp_gyro(i).save();
+        if (_backends[i]->externally_temperature_calibrated  == true) {
+            caltemp_gyro(i).save();
+        }
 #endif
     }
     for (uint8_t i=_gyro_count; i<INS_MAX_INSTANCES; i++) {
         _gyro_offset(i).set_and_save(Vector3f());
         _gyro_id(i).set_and_save(0);
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
-        caltemp_gyro(i).set_and_save_ifchanged(-300);
+        if (_backends[i]->externally_temperature_calibrated  == true) {
+            caltemp_gyro(i).set_and_save_ifchanged(-300);
+        }
 #endif
     }
 }
@@ -2298,6 +2302,9 @@ bool AP_InertialSensor::temperature_cal_running() const
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
         if (tcal(i).enable == AP_InertialSensor_TCal::Enable::LearnCalibration) {
+            if (_backends[i]->externally_temperature_calibrated  == true) {
+                return false;
+            }
             return true;
         }
     }
@@ -2394,13 +2401,17 @@ void AP_InertialSensor::_acal_save_calibrations()
             _accel_id(i).save();
             _accel_id_ok[i] = true;
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
-            caltemp_accel(i).set_and_save(get_temperature(i));
+            if (_backends[i]->externally_temperature_calibrated  == true) {
+                caltemp_accel(i).set_and_save(get_temperature(i));
+            }
 #endif
         } else {
             _accel_offset(i).set_and_save(Vector3f());
             _accel_scale(i).set_and_save(Vector3f());
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
-            caltemp_accel(i).set_and_save(-300);
+            if (_backends[i]->externally_temperature_calibrated  == true) {
+                caltemp_accel(i).set_and_save(-300);
+            }
 #endif
         }
     }
@@ -2411,7 +2422,9 @@ void AP_InertialSensor::_acal_save_calibrations()
         _accel_offset(i).set_and_save(Vector3f());
         _accel_scale(i).set_and_save(Vector3f());
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
-        caltemp_accel(i).set_and_save_ifchanged(-300);
+        if (_backends[i]->externally_temperature_calibrated  == true) {
+            caltemp_accel(i).set_and_save_ifchanged(-300);
+        }
 #endif
     }
 
@@ -2664,7 +2677,9 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal()
             _accel_id(k).save();
             _accel_id_ok[k] = true;
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
-            caltemp_accel(k).set_and_save(get_temperature(k));
+            if (_backends[k]->externally_temperature_calibrated  == true) {
+                caltemp_accel(k).set_and_save(get_temperature(k));
+            }
 #endif
         }
         for (uint8_t k=num_accels; k<INS_MAX_INSTANCES; k++) {
@@ -2795,8 +2810,12 @@ void AP_InertialSensor::send_uart_data(void)
 #if AP_EXTERNAL_AHRS_ENABLED
 void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt)
 {
+    TempCalibration = pkt.TempCalibration;
     for (uint8_t i = 0; i < _backend_count; i++) {
         _backends[i]->handle_external(pkt);
+        if(TempCalibration == AP_ExternalAHRS::TempCal::IsTempCalibrated){
+            _backends[i]->externally_temperature_calibrated = true;
+        }
     }
 }
 #endif // AP_EXTERNAL_AHRS_ENABLED

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -429,6 +429,7 @@ class AP_InertialSensor : AP_AccelCal_Client
 #if AP_EXTERNAL_AHRS_ENABLED
     // handle external AHRS data
     void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt);
+    AP_ExternalAHRS::TempCal TempCalibration;
 #endif
 
 #if HAL_INS_TEMPERATURE_CAL_ENABLE

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -41,7 +41,7 @@ class AP_InertialSensor_Backend
 public:
     AP_InertialSensor_Backend(AP_InertialSensor &imu);
     AP_InertialSensor_Backend(const AP_InertialSensor_Backend &that) = delete;
-
+    bool externally_temperature_calibrated = false;
     // we declare a virtual destructor so that drivers can
     // override with a custom destructor if need be.
     virtual ~AP_InertialSensor_Backend(void) {}