From 4d31a7320a1d2c38e2d742ae63c34f914febaa8f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
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 30d06b02b16c1..300125068d169 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -55,6 +55,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 7ab5b420d817e..38e0a66b8b0e0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -233,7 +233,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);