Skip to content

Commit

Permalink
pbio/imu: Add setters for gravity offsets.
Browse files Browse the repository at this point in the history
  • Loading branch information
@laurensvalk
laurensvalk committed Oct 21, 2024
1 parent 9c75126 commit 60759b4
Show file tree
Hide file tree
Showing 3 changed files with 94 additions and 7 deletions.
25 changes: 21 additions & 4 deletions lib/pbio/include/pbio/imu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

/**
Expand All @@ -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
Expand Down
43 changes: 42 additions & 1 deletion lib/pbio/src/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*/
Expand All @@ -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);
}

Expand Down Expand Up @@ -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
Expand All @@ -209,17 +229,38 @@ 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;
}
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.
Expand Down
33 changes: 31 additions & 2 deletions pybricks/common/pb_type_imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand All @@ -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;
Expand Down

0 comments on commit 60759b4

Please sign in to comment.