Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-24 16:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: correct compilation if INS_TEMPERATURE_CAL_ENABLE is off

Browse Source
This commit is contained in:
Peter Barker 2022-01-28 22:25:37 +11:00 committed by Andrew Tridgell
parent 2ea33324a0
commit 558f6babc4
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -46,6 +46,7 @@ static float calculate_noise(float noise, float noise_variation) {
float AP_InertialSensor_SITL::get_temperature(void) float AP_InertialSensor_SITL::get_temperature(void)
{ {
#if HAL_INS_TEMPERATURE_CAL_ENABLE
if (!is_zero(sitl->imu_temp_fixed)) { if (!is_zero(sitl->imu_temp_fixed)) {
// user wants fixed temperature // user wants fixed temperature
return sitl->imu_temp_fixed; return sitl->imu_temp_fixed;
@ -60,6 +61,9 @@ float AP_InertialSensor_SITL::get_temperature(void)
const float T1 = sitl->imu_temp_end; const float T1 = sitl->imu_temp_end;
const float tconst = sitl->imu_temp_tconst; const float tconst = sitl->imu_temp_tconst;
return T1 - (T1 - T0) * expf(-tsec / tconst); return T1 - (T1 - T0) * expf(-tsec / tconst);
#else
return 20.0f;
#endif
} }
/* /*
@ -70,8 +74,6 @@ void AP_InertialSensor_SITL::generate_accel()
Vector3f accel_accum; Vector3f accel_accum;
uint8_t nsamples = enable_fast_sampling(accel_instance) ? 4 : 1; uint8_t nsamples = enable_fast_sampling(accel_instance) ? 4 : 1;
float T = get_temperature();
for (uint8_t j = 0; j < nsamples; j++) { for (uint8_t j = 0; j < nsamples; j++) {
Vector3f accel = Vector3f(sitl->state.xAccel, Vector3f accel = Vector3f(sitl->state.xAccel,
@ -173,7 +175,10 @@ void AP_InertialSensor_SITL::generate_accel()
accel.x = accel.y = accel.z = sitl->accel_fail[accel_instance]; accel.x = accel.y = accel.z = sitl->accel_fail[accel_instance];
} }
#if HAL_INS_TEMPERATURE_CAL_ENABLE
const float T = get_temperature();
sitl->imu_tcal[gyro_instance].sitl_apply_accel(T, accel); sitl->imu_tcal[gyro_instance].sitl_apply_accel(T, accel);
#endif
_notify_new_accel_sensor_rate_sample(accel_instance, accel); _notify_new_accel_sensor_rate_sample(accel_instance, accel);
@ -256,7 +261,9 @@ void AP_InertialSensor_SITL::generate_gyro()
Vector3f gyro = Vector3f(p, q, r); Vector3f gyro = Vector3f(p, q, r);
#if HAL_INS_TEMPERATURE_CAL_ENABLE
sitl->imu_tcal[gyro_instance].sitl_apply_gyro(get_temperature(), gyro); sitl->imu_tcal[gyro_instance].sitl_apply_gyro(get_temperature(), gyro);
#endif
// add in gyro scaling // add in gyro scaling
Vector3f scale = sitl->gyro_scale[gyro_instance]; Vector3f scale = sitl->gyro_scale[gyro_instance];