pbio/imu: Make 3D heading optional.

This way we can introduce the feature gradually, defaulting to the current 1D behavior.

Author: laurensvalk

CHANGELOG.md

@@ -19,9 +19,9 @@
   makes it properly equivalent to `hub.imu.heading`.
 - Re-implemented tilt using the gyro data by default. Pure accelerometer tilt
   can still be obtained with `hub.imu.tilt(use_gyro=False)`.
-- Re-implemented `hub.imu.heading()` to use projection of 3D orientation to
-  improve performance when the hub is lifted off the ground. If necessary,
-  pure 1D rotation can still be obtained from `hub.imu.rotation()`.
+- Re-implemented `hub.imu.heading()` to use optionally use the projection of 3D
+  orientation to improve performance when the hub is lifted off the ground.
+  The 1D-based heading remains the default for now.
 
 ### Fixed
 - Fixed `DriveBase.angle()` getting an incorrectly rounded gyro value, which

lib/pbio/include/pbio/drivebase.h

@@ -16,13 +16,15 @@
 
 #include <pbio/servo.h>
 
+#include <pbio/imu.h>
+
 #if PBIO_CONFIG_NUM_DRIVEBASES > 0
 
 typedef struct _pbio_drivebase_t {
     /**
-     * True if a gyro or compass is used for heading control, else false.
+     * Whether to use the gyro for heading control, and if so which type.
      */
-    bool use_gyro;
+    pbio_imu_heading_type_t gyro_heading_type;
     /**
      * Synchronization state to indicate that one or more controllers are paused.
      */
@@ -79,7 +81,7 @@ pbio_error_t pbio_drivebase_get_state_user_angle(pbio_drivebase_t *db, float *an
 pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_t angle);
 pbio_error_t pbio_drivebase_get_drive_settings(const pbio_drivebase_t *db, int32_t *drive_speed, int32_t *drive_acceleration, int32_t *drive_deceleration, int32_t *turn_rate, int32_t *turn_acceleration, int32_t *turn_deceleration);
 pbio_error_t pbio_drivebase_set_drive_settings(pbio_drivebase_t *db, int32_t drive_speed, int32_t drive_acceleration, int32_t drive_deceleration, int32_t turn_rate, int32_t turn_acceleration, int32_t turn_deceleration);
-pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro);
+pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, pbio_imu_heading_type_t heading_type);
 
 #if PBIO_CONFIG_DRIVEBASE_SPIKE
 

lib/pbio/include/pbio/imu.h

@@ -18,6 +18,25 @@
 #include <pbio/error.h>
 #include <pbio/geometry.h>
 
+/**
+ * Heading type to use, set, or get.
+ */
+typedef enum {
+    /**
+     * Heading should not be used.
+     */
+    PBIO_IMU_HEADING_TYPE_NONE,
+    /**
+     * The heading is the integrated gyro rate along one fixed axis.
+     */
+    PBIO_IMU_HEADING_TYPE_1D,
+    /**
+     * The heading is angle between the projection of the line coming out of
+     * the front of the hub onto the horizontal plane and the x-axis.
+     */
+    PBIO_IMU_HEADING_TYPE_3D,
+} pbio_imu_heading_type_t;
+
 /**
  * IMU settings flags.
  *
@@ -44,6 +63,10 @@ typedef enum {
      * The accelerometer offsets and scale has been calibrated.
      */
     PBIO_IMU_SETTINGS_FLAGS_ACCEL_CALIBRATED = (1 << 5),
+    /**
+     * The heading correction for 1D rotation has been set.
+     */
+    PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET = (1 << 6),
 } pbio_imu_persistent_settings_flags_t;
 
 /**
@@ -68,6 +91,13 @@ typedef struct {
     pbio_geometry_xyz_t angular_velocity_bias_start;
     /** Angular velocity scale (unadjusted measured degrees per whole rotation) */
     pbio_geometry_xyz_t angular_velocity_scale;
+    /**
+     * Additional correction for 1D rotation in the user frame. Works on top
+     * of other calibration settings and values. Only used for 1D heading.
+     *
+     * This setting may be removed in the future when removing 1D support.
+     */
+    float heading_correction_1d;
 } pbio_imu_persistent_settings_t;
 
 #if PBIO_CONFIG_IMU
@@ -98,11 +128,11 @@ pbio_error_t pbio_imu_get_single_axis_rotation(pbio_geometry_xyz_t *axis, float
 
 pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated);
 
-float pbio_imu_get_heading(void);
+float pbio_imu_get_heading(pbio_imu_heading_type_t type);
 
 void pbio_imu_set_heading(float desired_heading);
 
-void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree);
+void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree);
 
 void pbio_orientation_imu_get_rotation(pbio_geometry_matrix_3x3_t *rotation);
 
@@ -143,14 +173,14 @@ static inline pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated) {
     return PBIO_GEOMETRY_SIDE_TOP;
 }
 
-static inline float pbio_imu_get_heading(void) {
+static inline float pbio_imu_get_heading(pbio_imu_heading_type_t type) {
     return 0.0f;
 }
 
 static inline void pbio_imu_set_heading(float desired_heading) {
 }
 
-static inline void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) {
+static inline void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) {
 }
 
 static inline void pbio_orientation_imu_get_rotation(pbio_geometry_matrix_3x3_t *rotation) {

lib/pbio/src/drivebase.c

@@ -182,8 +182,8 @@ static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_
     // Note that the heading speed estimate (used for derivative control still
     // uses the motor estimate rather than the gyro speed, to guarantee the
     // same stability properties to stabilize the motors.)
-    if (db->use_gyro) {
-        pbio_imu_get_heading_scaled(&state_heading->position, &state_heading->speed, db->control_heading.settings.ctl_steps_per_app_step);
+    if (db->gyro_heading_type != PBIO_IMU_HEADING_TYPE_NONE) {
+        pbio_imu_get_heading_scaled(db->gyro_heading_type, &state_heading->position, &state_heading->speed, db->control_heading.settings.ctl_steps_per_app_step);
     }
 
     return PBIO_SUCCESS;
@@ -365,7 +365,7 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
     pbio_drivebase_reset(db, 0, 0);
 
     // By default, don't use gyro for steering control.
-    db->use_gyro = false;
+    db->gyro_heading_type = PBIO_IMU_HEADING_TYPE_NONE;
 
     return PBIO_SUCCESS;
 }
@@ -376,13 +376,13 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
  * This function will stop the drivebase if it is running.
  *
  * @param [in]  db               Drivebase instance.
- * @param [in]  use_gyro         Whether to use the gyro for heading control.
+ * @param [in]  heading_type     Whether to use the gyro for heading control, and if so which type.
  * @return                       Error code.
  */
-pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro) {
+pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, pbio_imu_heading_type_t heading_type) {
 
     // No need to reset controls if already in correct mode.
-    if (db->use_gyro == use_gyro) {
+    if (db->gyro_heading_type == heading_type) {
         return PBIO_SUCCESS;
     }
 
@@ -393,7 +393,7 @@ pbio_error_t pbio_drivebase_set_use_gyro(pbio_drivebase_t *db, bool use_gyro) {
         return err;
     }
 
-    db->use_gyro = use_gyro;
+    db->gyro_heading_type = heading_type;
     return PBIO_SUCCESS;
 }
 
@@ -860,7 +860,7 @@ pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_
     pbio_angle_diff(&measured_heading.position, &reported_new, &db->heading_offset);
 
     // Synchronize heading and drivebase angle state if gyro in use.
-    if (db->use_gyro) {
+    if (db->gyro_heading_type != PBIO_IMU_HEADING_TYPE_NONE) {
         pbio_imu_set_heading(angle);
     }
 
@@ -877,7 +877,7 @@ bool pbio_drivebase_any_uses_gyro(void) {
         pbio_drivebase_t *db = &drivebases[i];
 
         // Only consider activated drive bases that use the gyro.
-        if (!pbio_drivebase_update_loop_is_running(db) || !db->use_gyro) {
+        if (!pbio_drivebase_update_loop_is_running(db) || db->gyro_heading_type == PBIO_IMU_HEADING_TYPE_NONE) {
             continue;
         }
 

lib/pbio/src/imu.c

@@ -82,7 +82,7 @@ static pbio_geometry_xyz_t acceleration_calibrated; // mm/s^2, in hub frame
  * 1D integrated angular velocity for each body axis.
  *
  * This is based on integrating the calibrated angular velocity over time, so
- * including its bias and adjustments to achhieve 360 degrees per rotation.
+ * including its bias and adjustments to achieve 360 degrees per rotation.
  *
  * This is not used for 3D attitude estimation, but serves as a useful way to
  * estimate 1D rotations without being effected by accelerometer fusion which
@@ -180,9 +180,8 @@ const float standard_gravity = 9806.65f;
  */
 static void pbio_imu_apply_pbdrv_settings(pbio_imu_persistent_settings_t *settings) {
 
-    // IMU config is loaded set pbio, while this first occurence of applying
-    // settings is only called by pbsys (after pbio init), so this should
-    // never happen.
+    // First occurence of this being called is when pbsys loads or resets
+    // the settings, so this should never happen.
     if (!imu_config) {
         return;
     }
@@ -206,6 +205,7 @@ void pbio_imu_set_default_settings(pbio_imu_persistent_settings_t *settings) {
     settings->gravity_neg.x = settings->gravity_neg.y = settings->gravity_neg.z = -standard_gravity;
     settings->angular_velocity_bias_start.x = settings->angular_velocity_bias_start.y = settings->angular_velocity_bias_start.z = 0.0f;
     settings->angular_velocity_scale.x = settings->angular_velocity_scale.y = settings->angular_velocity_scale.z = 360.0f;
+    settings->heading_correction_1d = 360.0f;
     pbio_imu_apply_pbdrv_settings(settings);
 }
 
@@ -521,6 +521,10 @@ pbio_error_t pbio_imu_set_settings(pbio_imu_persistent_settings_t *new_settings)
         persistent_settings->gravity_neg = new_settings->gravity_neg;
     }
 
+    if (new_settings->flags & PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET) {
+        persistent_settings->heading_correction_1d = new_settings->heading_correction_1d;
+    }
+
     // If any settings were changed, request saving.
     if (new_settings->flags) {
         persistent_settings->flags |= new_settings->flags;
@@ -621,18 +625,38 @@ pbio_geometry_side_t pbio_imu_get_up_side(bool calibrated) {
     return pbio_geometry_side_from_vector(acceleration);
 }
 
-static float heading_offset = 0;
+static float heading_offset_1d = 0;
+static float heading_offset_3d = 0;
 
 /**
  * Reads the estimated IMU heading in degrees, accounting for user offset and
  * user-specified heading correction scaling constant.
  *
  * Heading is defined as clockwise positive.
  *
+ * @param [in]  type        The type of heading to get.
+ *
  * @return                  Heading angle in the base frame.
  */
-float pbio_imu_get_heading(void) {
-    return heading_rotations * 360.0f + heading_projection - heading_offset;
+float pbio_imu_get_heading(pbio_imu_heading_type_t type) {
+
+    // 3D. Mapping into user frame is already accounted for in the projection.
+    if (type == PBIO_IMU_HEADING_TYPE_3D) {
+        return heading_rotations * 360.0f + heading_projection - heading_offset_3d;
+    }
+
+    // 1D. Map the per-axis integrated rotation to the user frame, then take
+    // the negative z component as the heading for positive-clockwise convention.
+    pbio_geometry_xyz_t heading_mapped;
+    pbio_geometry_vector_map(&pbio_imu_base_orientation, &single_axis_rotation, &heading_mapped);
+
+    float correction = (persistent_settings && (persistent_settings->flags & PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET)) ?
+        // If set, adjust by the user-specified scaling constant.
+        (360.0f / persistent_settings->heading_correction_1d):
+        // No (additional) correction.
+        1.0f;
+
+    return -heading_mapped.z * correction - heading_offset_1d;
 }
 
 /**
@@ -645,7 +669,8 @@ float pbio_imu_get_heading(void) {
  */
 void pbio_imu_set_heading(float desired_heading) {
     heading_rotations = 0;
-    heading_offset = pbio_imu_get_heading() + heading_offset - desired_heading;
+    heading_offset_3d = pbio_imu_get_heading(PBIO_IMU_HEADING_TYPE_3D) + heading_offset_3d - desired_heading;
+    heading_offset_1d = pbio_imu_get_heading(PBIO_IMU_HEADING_TYPE_1D) + heading_offset_1d - desired_heading;
 }
 
 /**
@@ -657,14 +682,15 @@ void pbio_imu_set_heading(float desired_heading) {
  *
  * Heading is defined as clockwise positive.
  *
+ * @param [in]   type                  Heading type to get.
  * @param [out]  heading               The heading angle in control units.
  * @param [out]  heading_rate          The heading rate in control units.
  * @param [in]   ctl_steps_per_degree  The number of control steps per heading degree.
  */
-void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) {
+void pbio_imu_get_heading_scaled(pbio_imu_heading_type_t type, pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) {
 
     // Heading in degrees of the robot.
-    float heading_degrees = pbio_imu_get_heading();
+    float heading_degrees = pbio_imu_get_heading(type);
 
     // Number of whole rotations in control units (in terms of wheels, not robot).
     heading->rotations = (int32_t)(heading_degrees / (360000.0f / ctl_steps_per_degree));

pybricks/common/pb_type_imu.c

@@ -188,7 +188,8 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
         PB_ARG_DEFAULT_NONE(acceleration_threshold),
         PB_ARG_DEFAULT_NONE(angular_velocity_bias),
         PB_ARG_DEFAULT_NONE(angular_velocity_scale),
-        PB_ARG_DEFAULT_NONE(acceleration_correction));
+        PB_ARG_DEFAULT_NONE(acceleration_correction),
+        PB_ARG_DEFAULT_NONE(heading_correction));
 
     (void)self;
 
@@ -225,6 +226,7 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
             mp_obj_new_tuple(MP_ARRAY_SIZE(angular_velocity_bias), angular_velocity_bias),
             mp_obj_new_tuple(MP_ARRAY_SIZE(angular_velocity_scale), angular_velocity_scale),
             mp_obj_new_tuple(MP_ARRAY_SIZE(acceleration_corrections), acceleration_corrections),
+            mp_obj_new_float_from_f(get_settings->heading_correction_1d),
         };
         return mp_obj_new_tuple(MP_ARRAY_SIZE(ret), ret);
     }
@@ -283,18 +285,31 @@ static mp_obj_t pb_type_imu_settings(size_t n_args, const mp_obj_t *pos_args, mp
         set_settings.gravity_neg.z = mp_obj_get_float(gravity[5]);
     }
 
+    if (heading_correction_in != mp_const_none) {
+        set_settings.flags |= PBIO_IMU_SETTINGS_FLAGS_HEADING_CORRECTION_1D_SET;
+        set_settings.heading_correction_1d = mp_obj_get_float(heading_correction_in);
+    }
+
     pb_assert(pbio_imu_set_settings(&set_settings));
 
     return mp_const_none;
 }
 static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_settings_obj, 1, pb_type_imu_settings);
 
 // pybricks._common.IMU.heading
-static mp_obj_t pb_type_imu_heading(mp_obj_t self_in) {
-    (void)self_in;
-    return mp_obj_new_float(pbio_imu_get_heading());
+static mp_obj_t pb_type_imu_heading(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(heading_type));
+
+    (void)self;
+    pbio_imu_heading_type_t type = heading_type_in == MP_OBJ_NEW_QSTR(MP_QSTR_3D) ?
+        PBIO_IMU_HEADING_TYPE_3D : PBIO_IMU_HEADING_TYPE_1D;
+
+    return mp_obj_new_float(pbio_imu_get_heading(type));
+    ;
 }
-MP_DEFINE_CONST_FUN_OBJ_1(pb_type_imu_heading_obj, pb_type_imu_heading);
+static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_imu_heading_obj, 1, pb_type_imu_heading);
 
 // pybricks._common.IMU.orientation
 STATIC mp_obj_t common_IMU_orientation(mp_obj_t self_in) {

pybricks/robotics/pb_type_drivebase.c

@@ -377,7 +377,19 @@ static mp_obj_t pb_type_DriveBase_use_gyro(size_t n_args, const mp_obj_t *pos_ar
     PB_PARSE_ARGS_METHOD(n_args, pos_args, kw_args,
         pb_type_DriveBase_obj_t, self,
         PB_ARG_REQUIRED(use_gyro));
-    pb_assert(pbio_drivebase_set_use_gyro(self->db, mp_obj_is_true(use_gyro_in)));
+
+    pbio_imu_heading_type_t type = PBIO_IMU_HEADING_TYPE_NONE;
+    if (mp_obj_is_true(use_gyro_in)) {
+        type = PBIO_IMU_HEADING_TYPE_1D;
+    }
+
+    // Allows testing of 3D heading calculation before it becomes the default
+    // in a future release.
+    if (use_gyro_in == MP_OBJ_NEW_QSTR(MP_QSTR_3D)) {
+        type = PBIO_IMU_HEADING_TYPE_3D;
+    }
+
+    pb_assert(pbio_drivebase_set_use_gyro(self->db, type));
     return mp_const_none;
 }
 static MP_DEFINE_CONST_FUN_OBJ_KW(pb_type_DriveBase_use_gyro_obj, 1, pb_type_DriveBase_use_gyro);