mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
AP_InertialSensor: review fixes
This commit is contained in:
parent
af18e0c755
commit
6c8a57c2d7
libraries/AP_InertialSensor
@ -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 {
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user