SITL: use TCal class for temp adjustment in SITL

libraries/SITL/SITL.cpp

@@ -76,6 +76,7 @@ const AP_Param::GroupInfo SITL::var_info[] = {
     AP_GROUPINFO("ADSB_TX",       51, SITL,  adsb_tx, 0),
     AP_GROUPINFO("SPEEDUP",       52, SITL,  speedup, -1),
     AP_GROUPINFO("IMU_POS",       53, SITL,  imu_pos_offset, 0),
+    AP_SUBGROUPEXTENSION("",      54, SITL,  var_ins),
     AP_GROUPINFO("SONAR_POS",     55, SITL,  rngfnd_pos_offset, 0),
     AP_GROUPINFO("FLOW_POS",      56, SITL,  optflow_pos_offset, 0),
     AP_GROUPINFO("ACC2_BIAS",     57, SITL,  accel2_bias, 0),
@@ -368,8 +369,19 @@ const AP_Param::GroupInfo SITL::var_sfml_joystick[] = {
     AP_GROUPINFO("SF_JS_AXIS8",    9, SITL,  sfml_joystick_axis[7], sf::Joystick::Axis::PovY),
     AP_GROUPEND
 };
-
 #endif //SFML_JOYSTICK
+
+// INS SITL parameters
+const AP_Param::GroupInfo SITL::var_ins[] = {
+    AP_GROUPINFO("IMUT_START",             1, SITL, imu_temp_start,  25),
+    AP_GROUPINFO("IMUT_END",               2, SITL, imu_temp_end, 45),
+    AP_GROUPINFO("IMUT_TCONST",            3, SITL, imu_temp_tconst, 300),
+    AP_GROUPINFO("IMUT_FIXED",             4, SITL, imu_temp_fixed, 0),
+    AP_SUBGROUPINFO(imu_tcal[0], "IMUT1_", 5, SITL, AP_InertialSensor::TCal),
+    AP_SUBGROUPINFO(imu_tcal[1], "IMUT2_", 6, SITL, AP_InertialSensor::TCal),
+    AP_SUBGROUPINFO(imu_tcal[2], "IMUT3_", 7, SITL, AP_InertialSensor::TCal),
+    AP_GROUPEND
+};
     
 /* report SITL state via MAVLink SIMSTATE*/
 void SITL::simstate_send(mavlink_channel_t chan)

libraries/SITL/SITL.h

@@ -9,6 +9,7 @@
 #include <AP_Baro/AP_Baro.h>
 #include <AP_Common/Location.h>
 #include <AP_Compass/AP_Compass.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 #include "SIM_Buzzer.h"
 #include "SIM_Gripper_EPM.h"
 #include "SIM_Gripper_Servo.h"
@@ -98,6 +99,7 @@ public:
         AP_Param::setup_object_defaults(this, var_info3);
         AP_Param::setup_object_defaults(this, var_gps);
         AP_Param::setup_object_defaults(this, var_mag);
+        AP_Param::setup_object_defaults(this, var_ins);
 #ifdef SFML_JOYSTICK
         AP_Param::setup_object_defaults(this, var_sfml_joystick);
 #endif // SFML_JOYSTICK
@@ -152,6 +154,7 @@ public:
     static const struct AP_Param::GroupInfo var_info3[];
     static const struct AP_Param::GroupInfo var_gps[];
     static const struct AP_Param::GroupInfo var_mag[];
+    static const struct AP_Param::GroupInfo var_ins[];
 #ifdef SFML_JOYSTICK
     static const struct AP_Param::GroupInfo var_sfml_joystick[];
 #endif //SFML_JOYSTICK
@@ -446,6 +449,13 @@ public:
     // get the apparent wind speed and direction as set by external physics backend
     float get_apparent_wind_dir(){return state.wind_vane_apparent.direction;}
     float get_apparent_wind_spd(){return state.wind_vane_apparent.speed;}
+
+    // IMU temperature calibration params
+    AP_Float imu_temp_start;
+    AP_Float imu_temp_end;
+    AP_Float imu_temp_tconst;
+    AP_Float imu_temp_fixed;
+    AP_InertialSensor::TCal imu_tcal[INS_MAX_INSTANCES];
 };
 
 } // namespace SITL