From 30974def2b2ecae537264de352bc3f1596edfc3d Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Fri, 25 Oct 2024 14:18:33 +0200 Subject: [PATCH] pbio/imu: Save one off bias calibration. When the hub first boots, it is good to have an initial value for the bias to help suppress drift during the first nonstationary seconds. As an estimate, we can use the bias first saved on the previous boot. --- lib/pbio/src/imu.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index 55fc36f82..4c9dd0c95 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -400,6 +400,16 @@ static void pbio_imu_handle_stationary_data_func(const int32_t *gyro_data_sum, c // Update bias at decreasing rate. gyro_bias.values[i] = gyro_bias.values[i] * (1.0f - weight) + weight * average_now; } + + // If persistent gyro bias has never been set, do so now and request saving. + // This ensures a better starting point for the next boot. We do this only + // once to avoid unnecessary writes on every shutdown. It can be further + // refined with a calibration routine performed by the user. + if (persistent_settings && !(persistent_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET) && stationary_counter > 2) { + persistent_settings->angular_velocity_bias_start = gyro_bias; + persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET; + pbsys_storage_request_write(); + } } /**