diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index c479d844122..2be2ac6f254 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -739,6 +739,7 @@ public: void reset(float temperature); float start_temp; float start_tmax; + uint32_t last_save_ms; TCal &tcal; uint8_t instance(void) const { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index f58cdd6158e..c88f5f4ccc1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -739,9 +739,10 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a() _notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64()); // read temperature every 10th sample - if (_temp_counter++ % 10 == 0) { + if (_temp_counter == 10) { int16_t traw; const uint8_t regtemp = OUT_TEMP_L_XM | 0xC0; + _temp_counter = 0; if (_dev_accel->transfer(®temp, 1, (uint8_t *)&traw, sizeof(traw))) { _temperature = _temp_filter.apply(traw * 0.125 + 25); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp index b8cc10802ff..beabfea343e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp @@ -63,7 +63,7 @@ const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = { // @Param: TMAX // @DisplayName: Temperature calibration max - // @Description: The maximum temperature that the calibration is valid for + // @Description: The maximum temperature that the calibration is valid for. This must be at least 10 degrees above TMIN for calibration // @Range: -70 80 // @Units: degC // @User: Advanced @@ -326,9 +326,10 @@ void AP_InertialSensor::TCal::Learn::add_sample(const Vector3f &sample, float te if (temperature >= start_tmax) { // we've reached the target temperature finish_calibration(temperature); - } else { + } else if (now - last_save_ms > 15000) { // save partial calibration, so if user stops the cal part // way then they still have a useful calibration + last_save_ms = now; save_calibration(st.last_temp); } }