diff --git a/CHANGELOG.md b/CHANGELOG.md index 47d64790e..6d96c6fdd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,8 +6,8 @@ ### Added -- Added optional `calibrated=True` parameter to `acceleration()` and `up()` - methods of the IMU ([support#943]). +- Added optional `calibrated=True` parameter to `acceleration()` and `up()` and + `angular_velocity()` methods of the IMU ([support#943]). ### Changed diff --git a/lib/pbio/include/pbio/imu.h b/lib/pbio/include/pbio/imu.h index bf436451b..5ebd21c3f 100644 --- a/lib/pbio/include/pbio/imu.h +++ b/lib/pbio/include/pbio/imu.h @@ -33,13 +33,17 @@ typedef enum { */ PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET = (1 << 1), /** - * The heading correction has been updated. + * The initial values for the gyro bias have been set. */ - PBIO_IMU_SETTINGS_FLAGS_GYRO_HEADING_CORRECTION_SET = (1 << 2), + PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET = (1 << 3), + /** + * The per-axis angular velocity scale has been set. + */ + PBIO_IMU_SETTINGS_FLAGS_GYRO_SCALE_SET = (1 << 4), /** * The accelerometer offsets and scale has been calibrated. */ - PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED = (1 << 3), + PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED = (1 << 5), } pbio_imu_persistent_settings_flags_t; /** @@ -56,17 +60,14 @@ typedef struct { float gyro_stationary_threshold; /** Acceleration threshold below which the IMU is considered stationary, in mm/s^2. */ float accel_stationary_threshold; - /** - * 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 values */ pbio_geometry_xyz_t gravity_pos; /** Negative acceleration values */ pbio_geometry_xyz_t gravity_neg; + /** Angular velocity stationary bias initially used on boot */ + pbio_geometry_xyz_t angular_velocity_bias_start; + /** Angular velocity scale (unadjusted measured degrees per whole rotation) */ + pbio_geometry_xyz_t angular_velocity_scale; } pbio_imu_persistent_settings_t; #if PBIO_CONFIG_IMU @@ -87,7 +88,7 @@ pbio_error_t pbio_imu_get_settings(pbio_imu_persistent_settings_t **settings); 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_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated); void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated); @@ -128,7 +129,7 @@ static inline pbio_error_t pbio_imu_set_settings(float angular_velocity, float a return PBIO_ERROR_NOT_SUPPORTED; } -static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { +static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated) { } static inline void pbio_imu_get_acceleration(pbio_geometry_xyz_t *values, bool calibrated) { diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index 2384df0f7..0f044700c 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -25,6 +25,14 @@ static pbdrv_imu_dev_t *imu_dev; static pbdrv_imu_config_t *imu_config; +// Cached sensor values that can be read at any time without polling again. +static pbio_geometry_xyz_t angular_velocity_uncalibrated; // deg/s, in hub frame +static pbio_geometry_xyz_t angular_velocity_calibrated; // deg/s, in hub frame, already adjusted for bias and scale. +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; // Starts at value from settings, then updated when stationary. +static pbio_geometry_xyz_t single_axis_rotation; // deg, in hub frame + // Asynchronously loaded on boot. Cannot be used until loaded. static pbio_imu_persistent_settings_t *persistent_settings = NULL; @@ -57,9 +65,10 @@ 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->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; + settings->angular_velocity_bias_start.x = settings->angular_velocity_bias_start.y = settings->angular_velocity_bias_start.z = 0.0f; + settings->angular_velocity_scale.x = settings->angular_velocity_scale.y = settings->angular_velocity_scale.z = 360.0f; pbio_imu_apply_pbdrv_settings(settings); } @@ -71,21 +80,21 @@ void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) { void pbio_imu_apply_loaded_settings(pbio_imu_persistent_settings_t *settings) { // This is called on load, so we can now access the settings directly. persistent_settings = settings; + + // The saved angular velocity bias only sets the initial value. We still + // update the bias continuously from stationary data. + gyro_bias.x = settings->angular_velocity_bias_start.x; + gyro_bias.y = settings->angular_velocity_bias_start.y; + gyro_bias.z = settings->angular_velocity_bias_start.z; + pbio_imu_apply_pbdrv_settings(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_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 - // Called by driver to process one frame of unfiltered gyro and accelerometer data. static void pbio_imu_handle_frame_data_func(int16_t *data) { - for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(angular_velocity.values); i++) { + for (uint8_t i = 0; i < PBIO_ARRAY_SIZE(angular_velocity_calibrated.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]; + angular_velocity_uncalibrated.values[i] = data[i] * imu_config->gyro_scale; acceleration_uncalibrated.values[i] = data[i + 3] * imu_config->accel_scale; // Once settings loaded, maintain calibrated cached values. @@ -93,8 +102,10 @@ static void pbio_imu_handle_frame_data_func(int16_t *data) { 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) * standard_gravity / acceleration_scale; + angular_velocity_calibrated.values[i] = (angular_velocity_uncalibrated.values[i] - gyro_bias.values[i]) * 360.0f / persistent_settings->angular_velocity_scale.values[i]; } else { acceleration_calibrated.values[i] = acceleration_uncalibrated.values[i]; + angular_velocity_calibrated.values[i] = angular_velocity_uncalibrated.values[i]; } // Update "heading" on all axes. This is not useful for 3D attitude @@ -102,7 +113,7 @@ static void pbio_imu_handle_frame_data_func(int16_t *data) { // the hub mounted at an arbitrary orientation. Such a 1D heading // is numerically more accurate, which is useful in drive base // applications so long as the vehicle drives on a flat surface. - single_axis_rotation.values[i] += angular_velocity.values[i] * imu_config->sample_time; + single_axis_rotation.values[i] += angular_velocity_calibrated.values[i] * imu_config->sample_time; } } @@ -235,20 +246,23 @@ 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) { - if (new_settings->heading_correction < 350 || new_settings->heading_correction > 370) { - return PBIO_ERROR_INVALID_ARG; + for (uint8_t i = 0; i < 3; i++) { + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET) { + persistent_settings->angular_velocity_bias_start.values[i] = new_settings->angular_velocity_bias_start.values[i]; + } + + if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_SCALE_SET) { + if (new_settings->angular_velocity_scale.values[i] < 350 || new_settings->angular_velocity_scale.values[i] > 370) { + return PBIO_ERROR_INVALID_ARG; + } + persistent_settings->angular_velocity_scale.values[i] = new_settings->angular_velocity_scale.values[i]; } - 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) { @@ -260,13 +274,13 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings) 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_pos = new_settings->gravity_pos; persistent_settings->gravity_neg = new_settings->gravity_neg; } // If any settings were changed, request saving. if (new_settings->flags) { + persistent_settings->flags |= new_settings->flags; pbsys_storage_request_write(); } @@ -296,9 +310,11 @@ pbio_error_t pbio_imu_get_settings(pbio_imu_persistent_settings_t **settings) { * Gets the cached IMU angular velocity in deg/s, compensated for gyro bias. * * @param [out] values The angular velocity vector. + * @param [in] calibrated Whether to get calibrated or uncalibrated data. */ -void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) { - pbio_geometry_vector_map(&pbio_orientation_base_orientation, &angular_velocity, values); +void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values, bool calibrated) { + pbio_geometry_xyz_t *angular_velocity = calibrated ? &angular_velocity_calibrated : &angular_velocity_uncalibrated; + pbio_geometry_vector_map(&pbio_orientation_base_orientation, angular_velocity, values); } /** @@ -359,13 +375,9 @@ static float heading_offset = 0; * @return Heading angle in the base frame. */ float pbio_imu_get_heading(void) { - pbio_geometry_xyz_t heading_mapped; pbio_geometry_vector_map(&pbio_orientation_base_orientation, &single_axis_rotation, &heading_mapped); - - float correction = persistent_settings ? (360.0f / persistent_settings->heading_correction) : 1.0f; - - return -heading_mapped.z * correction - heading_offset; + return -heading_mapped.z - heading_offset; } /** @@ -408,7 +420,7 @@ void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, i // The heading rate can be obtained by a simple scale because it always fits. pbio_geometry_xyz_t angular_rate; - pbio_imu_get_angular_velocity(&angular_rate); + pbio_imu_get_angular_velocity(&angular_rate, true); *heading_rate = (int32_t)(-angular_rate.z * ctl_steps_per_degree); } diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index 43d7d1269..1e6665389 100644 --- a/pybricks/common/pb_type_imu.c +++ b/pybricks/common/pb_type_imu.c @@ -120,11 +120,12 @@ static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_acceleration_obj, 1, pb_type_imu_a static mp_obj_t pb_type_imu_angular_velocity(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_TRUE(calibrated)); (void)self; pbio_geometry_xyz_t angular_velocity; - pbio_imu_get_angular_velocity(&angular_velocity); + pbio_imu_get_angular_velocity(&angular_velocity, mp_obj_is_true(calibrated_in)); // If no axis is specified, return a vector of values. if (axis_in == mp_const_none) { @@ -177,7 +178,8 @@ 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(angular_velocity_bias), + PB_ARG_DEFAULT_NONE(angular_velocity_scale), PB_ARG_DEFAULT_NONE(acceleration_correction)); (void)self; @@ -197,10 +199,23 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp mp_obj_new_float_from_f(get_settings->gravity_neg.z), }; + mp_obj_t angular_velocity_bias[] = { + mp_obj_new_float_from_f(get_settings->angular_velocity_bias_start.x), + mp_obj_new_float_from_f(get_settings->angular_velocity_bias_start.y), + mp_obj_new_float_from_f(get_settings->angular_velocity_bias_start.z), + }; + + mp_obj_t angular_velocity_scale[] = { + mp_obj_new_float_from_f(get_settings->angular_velocity_scale.x), + mp_obj_new_float_from_f(get_settings->angular_velocity_scale.y), + mp_obj_new_float_from_f(get_settings->angular_velocity_scale.z), + }; + 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(angular_velocity_bias), angular_velocity_bias), + mp_obj_new_tuple(MP_ARRAY_SIZE(angular_velocity_scale), angular_velocity_scale), mp_obj_new_tuple(MP_ARRAY_SIZE(acceleration_corrections), acceleration_corrections), }; return mp_obj_new_tuple(MP_ARRAY_SIZE(ret), ret); @@ -218,9 +233,30 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp set_settings.accel_stationary_threshold = mp_obj_get_float(acceleration_threshold_in); } - if (heading_correction_in != mp_const_none) { - set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_HEADING_CORRECTION_SET; - set_settings.heading_correction = mp_obj_get_float(heading_correction_in); + if (angular_velocity_bias_in != mp_const_none) { + mp_obj_t *bias; + size_t size; + mp_obj_get_array(angular_velocity_bias_in, &size, &bias); + if (size != 3) { + mp_raise_ValueError(MP_ERROR_TEXT("Angular velocity bias must be a 3-element tuple.")); + } + set_settings.angular_velocity_bias_start.x = mp_obj_get_float(bias[0]); + set_settings.angular_velocity_bias_start.y = mp_obj_get_float(bias[1]); + set_settings.angular_velocity_bias_start.z = mp_obj_get_float(bias[2]); + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_BIAS_INITIAL_SET; + } + + if (angular_velocity_scale_in != mp_const_none) { + mp_obj_t *scale; + size_t size; + mp_obj_get_array(angular_velocity_scale_in, &size, &scale); + if (size != 3) { + mp_raise_ValueError(MP_ERROR_TEXT("Angular velocity scale must be a 3-element tuple.")); + } + set_settings.angular_velocity_scale.x = mp_obj_get_float(scale[0]); + set_settings.angular_velocity_scale.y = mp_obj_get_float(scale[1]); + set_settings.angular_velocity_scale.z = mp_obj_get_float(scale[2]); + set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_SCALE_SET; } if (acceleration_correction_in != mp_const_none) {