From 60759b4ce191311ae1f310db3ee68b7a6c3fb36c Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 21 Oct 2024 13:03:16 +0200 Subject: [PATCH] pbio/imu: Add setters for gravity offsets. --- lib/pbio/include/pbio/imu.h | 25 ++++++++++++++++---- lib/pbio/src/imu.c | 43 ++++++++++++++++++++++++++++++++++- pybricks/common/pb_type_imu.c | 33 +++++++++++++++++++++++++-- 3 files changed, 94 insertions(+), 7 deletions(-) diff --git a/lib/pbio/include/pbio/imu.h b/lib/pbio/include/pbio/imu.h index 4572188a1..503bf077a 100644 --- a/lib/pbio/include/pbio/imu.h +++ b/lib/pbio/include/pbio/imu.h @@ -25,17 +25,21 @@ */ typedef enum { /** - * The accelerometer stationary threshold has been updated. + * The gyro stationary threshold has been updated. */ - PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET = (1 << 0), + PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET = (1 << 0), /** - * The gyro stationary threshold has been updated. + * The accelerometer stationary threshold has been updated. */ - PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET = (1 << 1), + PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET = (1 << 1), /** * The heading correction has been updated. */ PBIO_IMU_SETTINGS_FLAGS_GYRO_HEADING_CORRECTION_SET = (1 << 2), + /** + * The accelerometer offsets and scale has been calibrated. + */ + PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED = (1 << 3), } pbio_imu_persistent_settings_flags_t; /** @@ -56,8 +60,21 @@ typedef struct { * Number of degrees measured for one full turn along the user Z axis. This * is used to correct the heading value. Other rotation methods are not * affected. + * FIXME: Deprecate and use 3D correction. */ float heading_correction; + /** Positive acceleration value for gravity along the X axis. */ + float gravity_x_pos; + /** Negative acceleration value for gravity along the X axis. */ + float gravity_x_neg; + /** Positive acceleration value for gravity along the Y axis. */ + float gravity_y_pos; + /** Negative acceleration value for gravity along the Y axis. */ + float gravity_y_neg; + /** Positive acceleration value for gravity along the Z axis. */ + float gravity_z_pos; + /** Negative acceleration value for gravity along the Z axis. */ + float gravity_z_neg; } pbio_imu_persistent_settings_t; #if PBIO_CONFIG_IMU diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index a0ef01ba6..525619be6 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -28,6 +28,8 @@ static pbdrv_imu_config_t *imu_config; // Asynchronously loaded on boot. Cannot be used until loaded. static pbio_imu_persistent_settings_t *persistent_settings = NULL; +const float standard_gravity = 9806.65f; // mm/s^2 + /** * Applies (newly set) settings to the driver. */ @@ -52,10 +54,16 @@ static void pbio_imu_apply_pbdrv_settings(pbio_imu_persistent_settings_t *settin * @param [in] settings The loaded settings to apply. */ void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) { + settings->flags = 0; settings->gyro_stationary_threshold = 3.0f; settings->accel_stationary_threshold = 2500.0f; settings->heading_correction = 360.0f; - settings->flags = 0; + settings->gravity_x_pos = standard_gravity; + settings->gravity_x_neg = -standard_gravity; + settings->gravity_y_pos = standard_gravity; + settings->gravity_y_neg = -standard_gravity; + settings->gravity_z_pos = standard_gravity; + settings->gravity_z_neg = -standard_gravity; pbio_imu_apply_pbdrv_settings(settings); } @@ -188,6 +196,18 @@ bool pbio_imu_is_stationary(void) { return pbdrv_imu_is_stationary(imu_dev) && pbio_dcmotor_all_coasting(); } +/** + * Tests if the acceleration value is within a reasonable range for a stationary hub. + * + * @param [in] value The acceleration value to test. + * @return True if the value is within 10% off from standard gravity. + */ +static bool pbio_imu_stationary_acceleration_out_of_range(float value, bool expect_positive) { + const float expected_value = expect_positive ? standard_gravity : -standard_gravity; + const float absolute_error = value > expected_value ? value - expected_value : expected_value - value; + return absolute_error > standard_gravity / 15; +} + /** * Sets the IMU settings. This includes the thresholds that define when the hub * is stationary. When the measurements are steadily below these levels, the @@ -209,10 +229,12 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings) if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET) { persistent_settings->accel_stationary_threshold = new_settings->accel_stationary_threshold; + persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET; } if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET) { persistent_settings->gyro_stationary_threshold = new_settings->gyro_stationary_threshold; + persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET; } if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_HEADING_CORRECTION_SET) { @@ -220,6 +242,25 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings) return PBIO_ERROR_INVALID_ARG; } persistent_settings->heading_correction = new_settings->heading_correction; + persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_HEADING_CORRECTION_SET; + } + + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED) { + if (pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_x_pos, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_x_neg, false) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_y_pos, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_y_neg, false) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_z_pos, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_z_neg, false)) { + return PBIO_ERROR_INVALID_ARG; + } + persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED; + persistent_settings->gravity_x_pos = new_settings->gravity_x_pos; + persistent_settings->gravity_x_neg = new_settings->gravity_x_neg; + persistent_settings->gravity_y_pos = new_settings->gravity_y_pos; + persistent_settings->gravity_y_neg = new_settings->gravity_y_neg; + persistent_settings->gravity_z_pos = new_settings->gravity_z_pos; + persistent_settings->gravity_z_neg = new_settings->gravity_z_neg; } // If any settings were changed, request saving. diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index 120d15571..ae6d03fd7 100644 --- a/pybricks/common/pb_type_imu.c +++ b/pybricks/common/pb_type_imu.c @@ -168,22 +168,35 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp pb_type_imu_obj_t, self, PB_ARG_DEFAULT_NONE(angular_velocity_threshold), PB_ARG_DEFAULT_NONE(acceleration_threshold), - PB_ARG_DEFAULT_NONE(heading_correction)); + PB_ARG_DEFAULT_NONE(heading_correction), + PB_ARG_DEFAULT_NONE(acceleration_correction)); (void)self; // Return current values if no arguments are given. if (angular_velocity_threshold_in == mp_const_none && acceleration_threshold_in == mp_const_none && - heading_correction_in == mp_const_none) { + heading_correction_in == mp_const_none && + acceleration_correction_in == mp_const_none) { // Raises if not set, so can safely dereference. pbio_imu_persistent_settings_t *get_settings; pb_assert(pbio_imu_get_settings(&get_settings)); + + mp_obj_t acceleration_corrections[] = { + mp_obj_new_float_from_f(get_settings->gravity_x_pos), + mp_obj_new_float_from_f(get_settings->gravity_x_neg), + mp_obj_new_float_from_f(get_settings->gravity_y_pos), + mp_obj_new_float_from_f(get_settings->gravity_y_neg), + mp_obj_new_float_from_f(get_settings->gravity_z_pos), + mp_obj_new_float_from_f(get_settings->gravity_z_neg), + }; + mp_obj_t ret[] = { mp_obj_new_float_from_f(get_settings->gyro_stationary_threshold), mp_obj_new_float_from_f(get_settings->accel_stationary_threshold), mp_obj_new_float_from_f(get_settings->heading_correction), + mp_obj_new_tuple(MP_ARRAY_SIZE(acceleration_corrections), acceleration_corrections), }; return mp_obj_new_tuple(MP_ARRAY_SIZE(ret), ret); } @@ -205,6 +218,22 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp set_settings.heading_correction = mp_obj_get_float(heading_correction_in); } + if (acceleration_correction_in != mp_const_none) { + mp_obj_t *gravity; + size_t size; + mp_obj_get_array(acceleration_correction_in, &size, &gravity); + if (size != 6) { + mp_raise_ValueError(MP_ERROR_TEXT("Acceleration correction must be a 6-element tuple.")); + } + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED; + set_settings.gravity_x_pos = mp_obj_get_float(gravity[0]); + set_settings.gravity_x_neg = mp_obj_get_float(gravity[1]); + set_settings.gravity_y_pos = mp_obj_get_float(gravity[2]); + set_settings.gravity_y_neg = mp_obj_get_float(gravity[3]); + set_settings.gravity_z_pos = mp_obj_get_float(gravity[4]); + set_settings.gravity_z_neg = mp_obj_get_float(gravity[5]); + } + pb_assert(pbio_imu_set_settings(&set_settings)); return mp_const_none;