Skip to content

Commit 60759b4

Browse files
laurensvalklaurensvalk
committed
pbio/imu: Add setters for gravity offsets.
1 parent 9c75126 commit 60759b4

File tree

3 files changed

+94
-7
lines changed

3 files changed

+94
-7
lines changed

‎lib/pbio/include/pbio/imu.h‎

Lines changed: 21 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -25,17 +25,21 @@
2525
*/
2626
typedef enum {
2727
/**
28-
* The accelerometer stationary threshold has been updated.
28+
* The gyro stationary threshold has been updated.
2929
*/
30-
PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET = (1 << 0),
30+
PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET = (1 << 0),
3131
/**
32-
* The gyro stationary threshold has been updated.
32+
* The accelerometer stationary threshold has been updated.
3333
*/
34-
PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET = (1 << 1),
34+
PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET = (1 << 1),
3535
/**
3636
* The heading correction has been updated.
3737
*/
3838
PBIO_IMU_SETTINGS_FLAGS_GYRO_HEADING_CORRECTION_SET = (1 << 2),
39+
/**
40+
* The accelerometer offsets and scale has been calibrated.
41+
*/
42+
PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED = (1 << 3),
3943
} pbio_imu_persistent_settings_flags_t;
4044

4145
/**
@@ -56,8 +60,21 @@ typedef struct {
5660
* Number of degrees measured for one full turn along the user Z axis. This
5761
* is used to correct the heading value. Other rotation methods are not
5862
* affected.
63+
* FIXME: Deprecate and use 3D correction.
5964
*/
6065
float heading_correction;
66+
/** Positive acceleration value for gravity along the X axis. */
67+
float gravity_x_pos;
68+
/** Negative acceleration value for gravity along the X axis. */
69+
float gravity_x_neg;
70+
/** Positive acceleration value for gravity along the Y axis. */
71+
float gravity_y_pos;
72+
/** Negative acceleration value for gravity along the Y axis. */
73+
float gravity_y_neg;
74+
/** Positive acceleration value for gravity along the Z axis. */
75+
float gravity_z_pos;
76+
/** Negative acceleration value for gravity along the Z axis. */
77+
float gravity_z_neg;
6178
} pbio_imu_persistent_settings_t;
6279

6380
#if PBIO_CONFIG_IMU

‎lib/pbio/src/imu.c‎

Lines changed: 42 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,8 @@ static pbdrv_imu_config_t *imu_config;
2828
// Asynchronously loaded on boot. Cannot be used until loaded.
2929
static pbio_imu_persistent_settings_t *persistent_settings = NULL;
3030

31+
const float standard_gravity = 9806.65f; // mm/s^2
32+
3133
/**
3234
* Applies (newly set) settings to the driver.
3335
*/
@@ -52,10 +54,16 @@ static void pbio_imu_apply_pbdrv_settings(pbio_imu_persistent_settings_t *settin
5254
* @param [in] settings The loaded settings to apply.
5355
*/
5456
void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) {
57+
settings->flags = 0;
5558
settings->gyro_stationary_threshold = 3.0f;
5659
settings->accel_stationary_threshold = 2500.0f;
5760
settings->heading_correction = 360.0f;
58-
settings->flags = 0;
61+
settings->gravity_x_pos = standard_gravity;
62+
settings->gravity_x_neg = -standard_gravity;
63+
settings->gravity_y_pos = standard_gravity;
64+
settings->gravity_y_neg = -standard_gravity;
65+
settings->gravity_z_pos = standard_gravity;
66+
settings->gravity_z_neg = -standard_gravity;
5967
pbio_imu_apply_pbdrv_settings(settings);
6068
}
6169

@@ -188,6 +196,18 @@ bool pbio_imu_is_stationary(void) {
188196
return pbdrv_imu_is_stationary(imu_dev) && pbio_dcmotor_all_coasting();
189197
}
190198

199+
/**
200+
* Tests if the acceleration value is within a reasonable range for a stationary hub.
201+
*
202+
* @param [in] value The acceleration value to test.
203+
* @return True if the value is within 10% off from standard gravity.
204+
*/
205+
static bool pbio_imu_stationary_acceleration_out_of_range(float value, bool expect_positive) {
206+
const float expected_value = expect_positive ? standard_gravity : -standard_gravity;
207+
const float absolute_error = value > expected_value ? value - expected_value : expected_value - value;
208+
return absolute_error > standard_gravity / 15;
209+
}
210+
191211
/**
192212
* Sets the IMU settings. This includes the thresholds that define when the hub
193213
* is stationary. When the measurements are steadily below these levels, the
@@ -209,17 +229,38 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings)
209229

210230
if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET) {
211231
persistent_settings->accel_stationary_threshold = new_settings->accel_stationary_threshold;
232+
persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_ACCEL_STATIONARY_THRESHOLD_SET;
212233
}
213234

214235
if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET) {
215236
persistent_settings->gyro_stationary_threshold = new_settings->gyro_stationary_threshold;
237+
persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_STATIONARY_THRESHOLD_SET;
216238
}
217239

218240
if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_GYRO_HEADING_CORRECTION_SET) {
219241
if (new_settings->heading_correction < 350 || new_settings->heading_correction > 370) {
220242
return PBIO_ERROR_INVALID_ARG;
221243
}
222244
persistent_settings->heading_correction = new_settings->heading_correction;
245+
persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_GYRO_HEADING_CORRECTION_SET;
246+
}
247+
248+
if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED) {
249+
if (pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_x_pos, true) ||
250+
pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_x_neg, false) ||
251+
pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_y_pos, true) ||
252+
pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_y_neg, false) ||
253+
pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_z_pos, true) ||
254+
pbio_imu_stationary_acceleration_out_of_range(new_settings->gravity_z_neg, false)) {
255+
return PBIO_ERROR_INVALID_ARG;
256+
}
257+
persistent_settings->flags |= PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED;
258+
persistent_settings->gravity_x_pos = new_settings->gravity_x_pos;
259+
persistent_settings->gravity_x_neg = new_settings->gravity_x_neg;
260+
persistent_settings->gravity_y_pos = new_settings->gravity_y_pos;
261+
persistent_settings->gravity_y_neg = new_settings->gravity_y_neg;
262+
persistent_settings->gravity_z_pos = new_settings->gravity_z_pos;
263+
persistent_settings->gravity_z_neg = new_settings->gravity_z_neg;
223264
}
224265

225266
// If any settings were changed, request saving.

‎pybricks/common/pb_type_imu.c‎

Lines changed: 31 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -168,22 +168,35 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
168168
pb_type_imu_obj_t, self,
169169
PB_ARG_DEFAULT_NONE(angular_velocity_threshold),
170170
PB_ARG_DEFAULT_NONE(acceleration_threshold),
171-
PB_ARG_DEFAULT_NONE(heading_correction));
171+
PB_ARG_DEFAULT_NONE(heading_correction),
172+
PB_ARG_DEFAULT_NONE(acceleration_correction));
172173

173174
(void)self;
174175

175176
// Return current values if no arguments are given.
176177
if (angular_velocity_threshold_in == mp_const_none &&
177178
acceleration_threshold_in == mp_const_none &&
178-
heading_correction_in == mp_const_none) {
179+
heading_correction_in == mp_const_none &&
180+
acceleration_correction_in == mp_const_none) {
179181

180182
// Raises if not set, so can safely dereference.
181183
pbio_imu_persistent_settings_t *get_settings;
182184
pb_assert(pbio_imu_get_settings(&get_settings));
185+
186+
mp_obj_t acceleration_corrections[] = {
187+
mp_obj_new_float_from_f(get_settings->gravity_x_pos),
188+
mp_obj_new_float_from_f(get_settings->gravity_x_neg),
189+
mp_obj_new_float_from_f(get_settings->gravity_y_pos),
190+
mp_obj_new_float_from_f(get_settings->gravity_y_neg),
191+
mp_obj_new_float_from_f(get_settings->gravity_z_pos),
192+
mp_obj_new_float_from_f(get_settings->gravity_z_neg),
193+
};
194+
183195
mp_obj_t ret[] = {
184196
mp_obj_new_float_from_f(get_settings->gyro_stationary_threshold),
185197
mp_obj_new_float_from_f(get_settings->accel_stationary_threshold),
186198
mp_obj_new_float_from_f(get_settings->heading_correction),
199+
mp_obj_new_tuple(MP_ARRAY_SIZE(acceleration_corrections), acceleration_corrections),
187200
};
188201
return mp_obj_new_tuple(MP_ARRAY_SIZE(ret), ret);
189202
}
@@ -205,6 +218,22 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
205218
set_settings.heading_correction = mp_obj_get_float(heading_correction_in);
206219
}
207220

221+
if (acceleration_correction_in != mp_const_none) {
222+
mp_obj_t *gravity;
223+
size_t size;
224+
mp_obj_get_array(acceleration_correction_in, &size, &gravity);
225+
if (size != 6) {
226+
mp_raise_ValueError(MP_ERROR_TEXT("Acceleration correction must be a 6-element tuple."));
227+
}
228+
set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED;
229+
set_settings.gravity_x_pos = mp_obj_get_float(gravity[0]);
230+
set_settings.gravity_x_neg = mp_obj_get_float(gravity[1]);
231+
set_settings.gravity_y_pos = mp_obj_get_float(gravity[2]);
232+
set_settings.gravity_y_neg = mp_obj_get_float(gravity[3]);
233+
set_settings.gravity_z_pos = mp_obj_get_float(gravity[4]);
234+
set_settings.gravity_z_neg = mp_obj_get_float(gravity[5]);
235+
}
236+
208237
pb_assert(pbio_imu_set_settings(&set_settings));
209238

210239
return mp_const_none;

0 commit comments

Comments
 (0)