From d5b511f4a0442872f0cd3c02409d8c0b1e89ba66 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 11 Jan 2021 08:08:00 +1100 Subject: [PATCH] AP_InertialSensor: fixed start timer for temperature in SITL when we are doing an autotest we want the temperature to start climbing when we first set SIM_IMUT_FIXED=0 --- libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp | 6 +++++- libraries/AP_InertialSensor/AP_InertialSensor_SITL.h | 1 + 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index 3fe711d242..2970751361 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -50,8 +50,12 @@ float AP_InertialSensor_SITL::get_temperature(void) // user wants fixed temperature return sitl->imu_temp_fixed; } + uint32_t now = AP_HAL::millis(); + if (temp_start_ms == 0) { + temp_start_ms = now; + } // follow a curve with given start, end and time constant - const float tsec = AP_HAL::millis() * 0.001f; + const float tsec = (AP_HAL::millis() - temp_start_ms) * 0.001f; const float T0 = sitl->imu_temp_start; const float T1 = sitl->imu_temp_end; const float tconst = sitl->imu_temp_tconst; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h index 6c8a990352..24c377869e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.h @@ -43,6 +43,7 @@ private: float accel_time; float gyro_motor_phase[12]; float accel_motor_phase[12]; + uint32_t temp_start_ms; static uint8_t bus_id; };