From 43c4294d8d34e3f8e47b431017755960d6e37118 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 21 Oct 2024 13:31:06 +0200 Subject: [PATCH] pbio/imu: Calibrate acceleration values. --- CHANGELOG.md | 8 ++++- lib/pbio/include/pbio/imu.h | 24 +++++---------- lib/pbio/src/imu.c | 56 ++++++++++++++++++++--------------- pybricks/common/pb_type_imu.c | 39 +++++++++++++----------- 4 files changed, 68 insertions(+), 59 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c2d8e3154..4dd34cf8b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,11 @@ ## [Unreleased] +### Added + +- Added optional `calibrated=True` parameter to `acceleration()` and `up()` + methods of the IMU ([support#943]). + ### Changed - The method `DriveBase.angle()` now returns a float ([support#1844]). This @@ -13,8 +18,9 @@ - Fixed `DriveBase.angle()` getting an incorrectly rounded gyro value, which could cause `turn(360)` to be off by a degree ([support#1844]). -[support#1844]: https://github.com/pybricks/support/issues/1844 +[support#943]: https://github.com/pybricks/support/issues/943 [support#1886]: https://github.com/pybricks/support/issues/1886 +[support#1844]: https://github.com/pybricks/support/issues/1844 ## [3.6.0b2] - 2024-10-15 diff --git a/lib/pbio/include/pbio/imu.h b/lib/pbio/include/pbio/imu.h index 503bf077a..bf436451b 100644 --- a/lib/pbio/include/pbio/imu.h +++ b/lib/pbio/include/pbio/imu.h @@ -63,18 +63,10 @@ typedef struct { * 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; + /** Positive acceleration values */ + pbio_geometry_xyz_t gravity_pos; + /** Negative acceleration values */ + pbio_geometry_xyz_t gravity_neg; } pbio_imu_persistent_settings_t; #if PBIO_CONFIG_IMU @@ -97,11 +89,11 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings) void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values); -void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values); +void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated); pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float *angle); -pbio_geometry_side_t pbio_imu_get_up_side(void); +pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated); float pbio_imu_get_heading(void); @@ -139,10 +131,10 @@ static inline pbio_error_t pbio_imu_set_settings(float angular_velocity, float a static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { } -static inline void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values) { +static inline void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated) { } -static inline pbio_geometry_side_t pbio_imu_get_up_side(void) { +static inline pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated) { return PBIO_GEOMETRY_SIDE_TOP; } diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index 525619be6..f32b83ca3 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -58,12 +58,8 @@ void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) { settings->gyro_stationary_threshold = 3.0f; settings->accel_stationary_threshold = 2500.0f; settings->heading_correction = 360.0f; - 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; + settings->gravity_pos.x = settings->gravity_pos.y = settings->gravity_pos.z = standard_gravity; + settings->gravity_neg.x = settings->gravity_neg.y = settings->gravity_neg.z = -standard_gravity; pbio_imu_apply_pbdrv_settings(settings); } @@ -80,7 +76,8 @@ void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings) { // Cached sensor values that can be read at any time without polling again. static pbio_geometry_xyz_t angular_velocity; // deg/s, in hub frame, already adjusted for bias. -static pbio_geometry_xyz_t acceleration; // mm/s^2, in hub frame +static pbio_geometry_xyz_t acceleration_uncalibrated; // mm/s^2, in hub frame +static pbio_geometry_xyz_t acceleration_calibrated; // mm/s^2, in hub frame static pbio_geometry_xyz_t gyro_bias; static pbio_geometry_xyz_t single_axis_rotation; // deg, in hub frame @@ -89,7 +86,16 @@ static void pbio_imu_handle_frame_data_func(int16_t *data) { for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(angular_velocity.values); i++) { // Update angular velocity and acceleration cache so user can read them. angular_velocity.values[i] = data[i] * imu_config->gyro_scale - gyro_bias.values[i]; - acceleration.values[i] = data[i + 3] * imu_config->accel_scale; + acceleration_uncalibrated.values[i] = data[i + 3] * imu_config->accel_scale; + + // Once settings loaded, maintain calibrated cached values. + if (persistent_settings) { + float acceleration_offset = (persistent_settings->gravity_pos.values[i] + persistent_settings->gravity_neg.values[i]) / 2; + float acceleration_scale = (persistent_settings->gravity_pos.values[i] - persistent_settings->gravity_neg.values[i]) / 2; + acceleration_calibrated.values[i] = (acceleration_uncalibrated.values[i] - acceleration_offset) / acceleration_scale; + } else { + acceleration_calibrated.values[i] = acceleration_uncalibrated.values[i]; + } // Update "heading" on all axes. This is not useful for 3D attitude // estimation, but it allows the user to get a 1D heading even with @@ -246,21 +252,17 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings) } 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)) { + if (pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_pos.x, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_neg.x, false) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_pos.y, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_neg.y, false) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_pos.z, true) || + pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_neg.z, 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; + persistent_settings->gravity_pos = new_settings->gravity_pos; + persistent_settings->gravity_neg = new_settings->gravity_neg; } // If any settings were changed, request saving. @@ -310,10 +312,13 @@ void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { /** * Gets the cached IMU acceleration in mm/s^2. * + * @param [in] calibrated Whether to use calibrated or uncalibrated data. + * * @param [out] values The acceleration vector. */ -void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values) { - pbio_geometry_vector_map(&pbio_orientation_base_orientation, &acceleration, values); +void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated) { + pbio_geometry_xyz_t *acceleration = calibrated ? &acceleration_calibrated : &acceleration_uncalibrated; + pbio_geometry_vector_map(&pbio_orientation_base_orientation, acceleration, values); } /** @@ -338,14 +343,17 @@ pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float /** * Gets which side of a hub points upwards. * + * @param [in] calibrated Whether to use calibrated or uncalibrated data. + * * @return Which side is up. */ -pbio_geometry_side_t pbio_imu_get_up_side(void) { +pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated) { // Up is which side of a unit box intersects the +Z vector first. // So read +Z vector of the inertial frame, in the body frame. // For now, this is the gravity vector. In the future, we can make this // slightly more accurate by using the full IMU orientation. - return pbio_geometry_side_from_vector(&acceleration); + pbio_geometry_xyz_t *acceleration = calibrated ? &acceleration_calibrated : &acceleration_uncalibrated; + return pbio_geometry_side_from_vector(acceleration); } static float heading_offset = 0; diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index ae6d03fd7..f0d878707 100644 --- a/pybricks/common/pb_type_imu.c +++ b/pybricks/common/pb_type_imu.c @@ -31,8 +31,8 @@ typedef struct _pb_type_imu_obj_t { } pb_type_imu_obj_t; // pybricks._common.IMU.up -static mp_obj_t pb_type_imu_up(mp_obj_t self_in) { - switch (pbio_imu_get_up_side()) { +static mp_obj_t pb_type_imu_up(mp_obj_t self_in, mp_obj_t calibrated_in) { + switch (pbio_imu_get_up_side(mp_obj_is_true(calibrated_in))) { default: case PBIO_GEOMETRY_SIDE_FRONT: return MP_OBJ_FROM_PTR(&pb_Side_FRONT_obj); @@ -48,14 +48,16 @@ static mp_obj_t pb_type_imu_up(mp_obj_t self_in) { return MP_OBJ_FROM_PTR(&pb_Side_BOTTOM_obj); } } -MP_DEFINE_CONST_FUN_OBJ_1(pb_type_imu_up_obj, pb_type_imu_up); +MP_DEFINE_CONST_FUN_OBJ_2(pb_type_imu_up_obj, pb_type_imu_up); // pybricks._common.IMU.tilt static mp_obj_t pb_type_imu_tilt(mp_obj_t self_in) { + // Revisit: optionally use not just calibrated but also IMU. + // Read acceleration in the user frame. pbio_geometry_xyz_t accl; - pbio_imu_get_acceleration(&accl); + pbio_imu_get_acceleration(&accl, false); mp_obj_t tilt[2]; // Pitch @@ -86,11 +88,12 @@ static void pb_type_imu_extract_axis(mp_obj_t obj_in, pbio_geometry_xyz_t *vecto static mp_obj_t pb_type_imu_acceleration(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) { PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args, pb_type_imu_obj_t, self, - PB_ARG_DEFAULT_NONE(axis)); + PB_ARG_DEFAULT_NONE(axis), + PB_ARG_DEFAULT_NONE(calibrated)); (void)self; pbio_geometry_xyz_t acceleration; - pbio_imu_get_acceleration(&acceleration); + pbio_imu_get_acceleration(&acceleration, mp_obj_is_true(calibrated_in)); // If no axis is specified, return a vector of values. if (axis_in == mp_const_none) { @@ -184,12 +187,12 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp 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_new_float_from_f(get_settings->gravity_pos.x), + mp_obj_new_float_from_f(get_settings->gravity_neg.x), + mp_obj_new_float_from_f(get_settings->gravity_pos.y), + mp_obj_new_float_from_f(get_settings->gravity_neg.y), + mp_obj_new_float_from_f(get_settings->gravity_pos.z), + mp_obj_new_float_from_f(get_settings->gravity_neg.z), }; mp_obj_t ret[] = { @@ -226,12 +229,12 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp 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]); + set_settings.gravity_pos.x = mp_obj_get_float(gravity[0]); + set_settings.gravity_neg.x = mp_obj_get_float(gravity[1]); + set_settings.gravity_pos.y = mp_obj_get_float(gravity[2]); + set_settings.gravity_neg.y = mp_obj_get_float(gravity[3]); + set_settings.gravity_pos.z = mp_obj_get_float(gravity[4]); + set_settings.gravity_neg.z = mp_obj_get_float(gravity[5]); } pb_assert(pbio_imu_set_settings(&set_settings));