From e6ac2564617285e3186ec0763afbe21ec486efd4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Nov 2024 13:35:51 +1100 Subject: [PATCH] AP_InertialSensor: stop sensors converging if motors arm if the user arms within 30s of startup then stop the re-init of the sensors. This can give less accurate frequency as the sample rate may not have settled yet, but it is better than doing init of the filters while the vehicle may be flying also fix a 32 bit millis wrap --- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 9 +++++++++ libraries/AP_InertialSensor/AP_InertialSensor_Backend.h | 2 +- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 85c9ed8f34e35..b2f0d6eed9f7e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -54,6 +54,15 @@ void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t _imu._gyro_over_sampling[instance] = n; } +/* + while sensors are converging to get the true sample rate we re-init the notch filters. + stop doing this if the user arms + */ +bool AP_InertialSensor_Backend::sensors_converging() const +{ + return AP_HAL::millis64() < HAL_INS_CONVERGANCE_MS && !hal.util->get_soft_armed(); +} + /* update the sensor rate for FIFO sensors diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index e51d9362b80b5..948424c975291 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -212,7 +212,7 @@ class AP_InertialSensor_Backend void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__; // return true if the sensors are still converging and sampling rates could change significantly - bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; } + bool sensors_converging() const; // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset);