AP_InertialSensor: expose TCal class for use in SITL

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -681,11 +681,15 @@ private:
     uint8_t imu_kill_mask;
 
 #if HAL_INS_TEMPERATURE_CAL_ENABLE
+public:
+    // TCal class is public for use by SITL
     class TCal {
     public:
         static const struct AP_Param::GroupInfo var_info[];
         void correct_accel(float temperature, float cal_temp, Vector3f &accel) const;
         void correct_gyro(float temperature, float cal_temp, Vector3f &accel) const;
+        void sitl_apply_accel(float temperature, Vector3f &accel) const;
+        void sitl_apply_gyro(float temperature, Vector3f &accel) const;
     private:
         AP_Int8 enable;
         AP_Float temp_min;
@@ -698,6 +702,7 @@ private:
         void correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const;
         Vector3f polynomial_eval(float temperature, const AP_Vector3f coeff[3]) const;
     };
+private:
     TCal tcal[INS_MAX_INSTANCES];
 
     // temperature that last calibration was run at

libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp

@@ -44,6 +44,20 @@ static float calculate_noise(float noise, float noise_variation) {
     return noise * (1.0f + noise_variation * rand_float());
 }
 
+float AP_InertialSensor_SITL::get_temperature(void)
+{
+    if (!is_zero(sitl->imu_temp_fixed)) {
+        // user wants fixed temperature
+        return sitl->imu_temp_fixed;
+    }
+    // follow a curve with given start, end and time constant
+    const float tsec = AP_HAL::millis() * 0.001f;
+    const float T0 = sitl->imu_temp_start;
+    const float T1 = sitl->imu_temp_end;
+    const float tconst = sitl->imu_temp_tconst;
+    return T1 - (T1 - T0) * expf(-tsec / tconst);
+}
+
 /*
   generate an accelerometer sample
  */
@@ -51,6 +65,9 @@ void AP_InertialSensor_SITL::generate_accel()
 {
     Vector3f accel_accum;
     uint8_t nsamples = enable_fast_sampling(accel_instance) ? 4 : 1;
+
+    float T = get_temperature();
+
     for (uint8_t j = 0; j < nsamples; j++) {
 
         // add accel bias and noise
@@ -143,6 +160,8 @@ void AP_InertialSensor_SITL::generate_accel()
 
         Vector3f accel = Vector3f(xAccel, yAccel, zAccel);
 
+        sitl->imu_tcal[gyro_instance].sitl_apply_accel(T, accel);
+
         _notify_new_accel_sensor_rate_sample(accel_instance, accel);
 
         accel_accum += accel;
@@ -152,7 +171,7 @@ void AP_InertialSensor_SITL::generate_accel()
     _rotate_and_correct_accel(accel_instance, accel_accum);
     _notify_new_accel_raw_sample(accel_instance, accel_accum, AP_HAL::micros64());
 
-    _publish_temperature(accel_instance, 23);
+    _publish_temperature(accel_instance, get_temperature());
 }
 
 /*
@@ -224,6 +243,8 @@ void AP_InertialSensor_SITL::generate_gyro()
 
         Vector3f gyro = Vector3f(p, q, r);
 
+        sitl->imu_tcal[gyro_instance].sitl_apply_gyro(get_temperature(), gyro);
+
         // add in gyro scaling
         Vector3f scale = sitl->gyro_scale;
         gyro.x *= (1 + scale.x * 0.01f);

libraries/AP_InertialSensor/AP_InertialSensor_SITL.h

@@ -28,6 +28,7 @@ private:
     float gyro_drift(void);
     void generate_accel();
     void generate_gyro();
+    float get_temperature(void);
 
     SITL::SITL *sitl;
 

libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp

@@ -194,9 +194,12 @@ void AP_InertialSensor::TCal::correct_sensor(float temperature, float cal_temp,
     cal_temp = constrain_float(cal_temp, temp_min, temp_max);
 
     const float tmid = (temp_max + temp_min)*0.5;
+    if (tmid <= 0) {
+        return;
+    }
 
     // get the polynomial correction for the difference between the
-    // current temperature and the temperature we are at now
+    // current temperature and the mid temperature
     v -= polynomial_eval(temperature-tmid, coeff);
 
     // we need to add the correction for the temperature
@@ -216,4 +219,18 @@ void AP_InertialSensor::TCal::correct_gyro(float temperature, float cal_temp, Ve
     correct_sensor(temperature, cal_temp, gyro_coeff, gyro);
 }
 
+void AP_InertialSensor::TCal::sitl_apply_accel(float temperature, Vector3f &accel) const
+{
+    Vector3f v;
+    correct_sensor(temperature, 0.5*(temp_max+temp_min), accel_coeff, v);
+    accel -= v;
+}
+
+void AP_InertialSensor::TCal::sitl_apply_gyro(float temperature, Vector3f &gyro) const
+{
+    Vector3f v;
+    correct_sensor(temperature, 0.5*(temp_max+temp_min), gyro_coeff, v);
+    gyro -= v;
+}
+
 #endif // HAL_INS_TEMPERATURE_CAL_ENABLE