pbio/imu: Calibrate acceleration values.

Author: laurensvalk

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
 

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;
 }
 

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) * standard_gravity / 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.
@@ -275,14 +277,6 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings)
     return PBIO_SUCCESS;
 }
 
-/**
- * Gets the thresholds that define when the hub is stationary.
- *
- * @param [out]  angular_velocity    Angular velocity threshold in deg/s.
- * @param [out]  acceleration        Acceleration threshold in mm/s^2
- * @param [out]  heading_correction  Measured degrees per full rotation of the hub.
- */
-
 /**
  * Gets the IMU settings
  *
@@ -310,10 +304,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 +335,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;

pybricks/common/pb_type_imu.c

@@ -31,8 +31,14 @@ 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(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_TRUE(calibrated));
+
+    (void)self;
+
+    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 +54,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);
+static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_up_obj, 1, 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 +94,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_TRUE(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 +193,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 +235,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));