pbio/imu: Add user adjustable heading correction.

Author: laurensvalk

CHANGELOG.md

@@ -9,14 +9,19 @@
 - Allow Bluetooth to be toggled off and on with the Bluetooth button on the
   Prime Hub and the Inventor Hub ([support#1615]), and have this state persist
   between reboots.
+- Added `heading_correction` to `hub.imu.settings` to allow for automatic
+  correction of the `hub.imu.heading()` value ([support#1678]).
 
 ### Changed
 
 - When upgrading the firmware to a new version, the user program will now
   be erased. This avoids issues with incompatible program files ([support#1622]).
+- The `angular_velocity_threshold`, and `acceleration_threshold` settings
+  in `hub.imu.settings` are now persistent between reboots.
 
 [support#1615]: https://github.com/pybricks/support/issues/1615
 [support#1622]: https://github.com/pybricks/support/issues/1622
+[support#1678]: https://github.com/pybricks/support/issues/1678
 
 ## [3.5.0] - 2024-04-11
 

lib/pbio/include/pbio/imu.h

@@ -28,9 +28,9 @@ bool pbio_imu_is_stationary(void);
 
 bool pbio_imu_is_ready(void);
 
-void pbio_imu_get_stationary_thresholds(float *angular_velocity, float *acceleration);
+void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction);
 
-void pbio_imu_set_stationary_thresholds(float angular_velocity, float acceleration);
+pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction);
 
 void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values);
 
@@ -59,7 +59,11 @@ static inline bool pbio_imu_is_stationary(void) {
     return false;
 }
 
-static inline void pbio_imu_set_stationary_thresholds(float angular_velocity, float acceleration) {
+static inline void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction) {
+}
+
+static inline pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction) {
+    return PBIO_ERROR_NOT_SUPPORTED;
 }
 
 static inline void pbio_imu_get_angular_velocity(pbio_geometry_xyz_t *values) {

lib/pbio/include/pbsys/storage_settings.h

@@ -41,6 +41,12 @@ typedef struct _pbsys_storage_settings_t {
     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.
+     */
+    float heading_correction;
     #endif
 } pbsys_storage_settings_t;
 

lib/pbio/src/imu.c

@@ -3,6 +3,7 @@
 
 #include <stdbool.h>
 #include <string.h>
+#include <math.h>
 
 #include <pbdrv/clock.h>
 #include <pbdrv/imu.h>
@@ -133,34 +134,52 @@ bool pbio_imu_is_stationary(void) {
     return pbdrv_imu_is_stationary(imu_dev);
 }
 
+// Measured rotation of the Z axis in the user frame for exactly one rotation
+// of the hub. This will be used to adjust the heading value, which is slightly
+// different for each hub.
+static float heading_degrees_per_rotation = 360.0f;
+
 /**
- * Sets the thresholds that define when the hub is stationary. When the
- * measurements are steadily below these levels, the orientation module
- * automatically recalibrates.
- * 
- * If a value is negative, it is ignored.
+ * Sets the IMU settings. This includes the thresholds that define when the hub
+ * is stationary. When the measurements are steadily below these levels, the
+ * orientation module automatically recalibrates. Also includes the hub-specific
+ * correction value to get a more accurate heading value.
+ *
+ * If a value is nan, it is ignored.
  *
- * @param [in]  angular_velocity Angular velocity threshold in deg/s.
- * @param [in]  acceleration     Acceleration threshold in mm/s^2
+ * @param [in]  angular_velocity    Angular velocity threshold in deg/s.
+ * @param [in]  acceleration        Acceleration threshold in mm/s^2
+ * @param [in]  heading_correction  Measured degrees per full rotation of the hub.
+ * @returns ::PBIO_ERROR_INVALID_ARG if the heading correction is out of range,
+ *          otherwise ::PBIO_SUCCESS.
  */
-void pbio_imu_set_stationary_thresholds(float angular_velocity, float acceleration) {
-    if (angular_velocity >= 0) {
+pbio_error_t pbio_imu_set_settings(float angular_velocity, float acceleration, float heading_correction) {
+    if (!isnan(angular_velocity)) {
         imu_config->gyro_stationary_threshold = pbio_int_math_bind(angular_velocity / imu_config->gyro_scale, 1, INT16_MAX);
     }
-    if (acceleration >= 0) {
+    if (!isnan(acceleration)) {
         imu_config->accel_stationary_threshold = pbio_int_math_bind(acceleration / imu_config->accel_scale, 1, INT16_MAX);
     }
+    if (!isnan(heading_correction)) {
+        if (heading_correction < 350 || heading_correction > 370) {
+            return PBIO_ERROR_INVALID_ARG;
+        }
+        heading_degrees_per_rotation = heading_correction;
+    }
+    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]  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.
  */
-void pbio_imu_get_stationary_thresholds(float *angular_velocity, float *acceleration) {
+void pbio_imu_get_settings(float *angular_velocity, float *acceleration, float *heading_correction) {
     *angular_velocity = imu_config->gyro_stationary_threshold * imu_config->gyro_scale;
     *acceleration = imu_config->accel_stationary_threshold * imu_config->accel_scale;
+    *heading_correction = heading_degrees_per_rotation;
 }
 
 /**
@@ -216,7 +235,8 @@ pbio_geometry_side_t pbio_imu_get_up_side(void) {
 static float heading_offset = 0;
 
 /**
- * Reads the estimated IMU heading in degrees, accounting for user offset.
+ * Reads the estimated IMU heading in degrees, accounting for user offset and
+ * user-specified heading correction scaling constant.
  *
  * Heading is defined as clockwise positive.
  *
@@ -228,7 +248,7 @@ float pbio_imu_get_heading(void) {
 
     pbio_geometry_vector_map(&pbio_orientation_base_orientation, &single_axis_rotation, &heading_mapped);
 
-    return -heading_mapped.z - heading_offset;
+    return -heading_mapped.z * 360.0f / heading_degrees_per_rotation - heading_offset;
 }
 
 /**

lib/pbio/sys/storage_settings.c

@@ -31,6 +31,7 @@ void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) {
     #if PBIO_CONFIG_IMU
     settings->gyro_stationary_threshold = 5;
     settings->accel_stationary_threshold = 2500;
+    settings->heading_correction = 360.0f;
     #endif // PBIO_CONFIG_IMU
 }
 
@@ -41,7 +42,12 @@ void pbsys_storage_settings_set_defaults(pbsys_storage_settings_t *settings) {
  */
 void pbsys_storage_settings_apply_loaded_settings(pbsys_storage_settings_t *settings) {
     #if PBIO_CONFIG_IMU
-    pbio_imu_set_stationary_thresholds(settings->gyro_stationary_threshold, settings->accel_stationary_threshold);
+    // return value not checked, assumed to be set valid previously
+    pbio_imu_set_settings(
+        settings->gyro_stationary_threshold,
+        settings->accel_stationary_threshold,
+        settings->heading_correction
+        );
     #endif // PBIO_CONFIG_IMU
 }
 
@@ -56,7 +62,11 @@ void pbsys_storage_settings_save_imu_settings(void) {
         return;
     }
     #if PBIO_CONFIG_IMU
-    pbio_imu_get_stationary_thresholds(&settings->gyro_stationary_threshold, &settings->accel_stationary_threshold);
+    pbio_imu_get_settings(
+        &settings->gyro_stationary_threshold,
+        &settings->accel_stationary_threshold,
+        &settings->heading_correction
+        );
     pbsys_storage_request_write();
     #endif // PBIO_CONFIG_IMU
 }

pybricks/common/pb_type_imu.c

@@ -164,27 +164,33 @@ STATIC mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
     PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args,
         pb_type_imu_obj_t, self,
         PB_ARG_DEFAULT_NONE(angular_velocity_threshold),
-        PB_ARG_DEFAULT_NONE(acceleration_threshold));
+        PB_ARG_DEFAULT_NONE(acceleration_threshold),
+        PB_ARG_DEFAULT_NONE(heading_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) {
+    if (angular_velocity_threshold_in == mp_const_none &&
+        acceleration_threshold_in == mp_const_none &&
+        heading_correction_in == mp_const_none) {
         float angular_velocity;
         float acceleration;
-        pbio_imu_get_stationary_thresholds(&angular_velocity, &acceleration);
+        float heading_correction;
+        pbio_imu_get_settings(&angular_velocity, &acceleration, &heading_correction);
         mp_obj_t ret[] = {
             mp_obj_new_float_from_f(angular_velocity),
             mp_obj_new_float_from_f(acceleration),
+            mp_obj_new_float_from_f(heading_correction),
         };
         return mp_obj_new_tuple(MP_ARRAY_SIZE(ret), ret);
     }
 
     // Otherwise set new values, only if given.
-    pbio_imu_set_stationary_thresholds(
-        angular_velocity_threshold_in == mp_const_none ? -1.0f : mp_obj_get_float(angular_velocity_threshold_in),
-        acceleration_threshold_in == mp_const_none ? -1.0f : mp_obj_get_float(acceleration_threshold_in)
-    );
+    pb_assert(pbio_imu_set_settings(
+        angular_velocity_threshold_in == mp_const_none ? NAN : mp_obj_get_float(angular_velocity_threshold_in),
+        acceleration_threshold_in == mp_const_none ? NAN : mp_obj_get_float(acceleration_threshold_in),
+        heading_correction_in == mp_const_none ? NAN : mp_obj_get_float(heading_correction_in)
+        ));
 
     // Request that changed settings are saved on shutdown.
     pbsys_storage_settings_save_imu_settings();