Skip to content

Commit

Permalink
pbio/imu: Calibrate acceleration values.
Browse files Browse the repository at this point in the history
  • Loading branch information
@laurensvalk
laurensvalk committed Oct 21, 2024
1 parent 60759b4 commit 43c4294
Show file tree
Hide file tree
Showing 4 changed files with 68 additions and 59 deletions.
8 changes: 7 additions & 1 deletion CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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

Expand Down
24 changes: 8 additions & 16 deletions lib/pbio/include/pbio/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);

Expand Down Expand Up @@ -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;
}

Expand Down
56 changes: 32 additions & 24 deletions lib/pbio/src/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand All @@ -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

Expand All @@ -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
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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);
}

/**
Expand All @@ -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;
Expand Down
39 changes: 21 additions & 18 deletions pybricks/common/pb_type_imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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[] = {
Expand Down Expand Up @@ -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));
Expand Down

0 comments on commit 43c4294

Please sign in to comment.