pbio/drivebase: Use gyro for speed state.

While we have to use the motor speed estimate to get stable motor
motion, we can still use the measured gyro rate for things like stall
detection.

This improves accuracy of stall detection when the robot is lifted
in the air. This is not a typical operation that happens in the field,
but people often do this to test that the gyro is "working",
so we should make sure to produce intuitive behavior.

Partially fixes pybricks/support#1032

lib/pbio/include/pbio/imu.h

@@ -44,7 +44,7 @@ float pbio_imu_get_heading(void);
 
 void pbio_imu_set_heading(float desired_heading);
 
-void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_degree);
+void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree);
 
 #else // PBIO_CONFIG_IMU
 
@@ -79,7 +79,7 @@ static inline float pbio_imu_get_heading(void) {
 static inline void pbio_imu_set_heading(float desired_heading) {
 }
 
-static inline void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_degree) {
+static inline void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, int32_t ctl_steps_per_degree) {
 }
 
 #endif // PBIO_CONFIG_IMU

lib/pbio/src/drivebase.c

@@ -147,7 +147,7 @@ static pbio_error_t pbio_drivebase_get_state_control(pbio_drivebase_t *db, pbio_
 
     // Optionally use gyro to override the heading source for more accuracy.
     if (db->use_gyro) {
-        pbio_imu_get_heading_scaled(&state_heading->position, db->control_heading.settings.ctl_steps_per_app_step);
+        pbio_imu_get_heading_scaled(&state_heading->position, &state_heading->speed, db->control_heading.settings.ctl_steps_per_app_step);
     }
 
     return PBIO_SUCCESS;

lib/pbio/src/imu.c

@@ -212,6 +212,8 @@ static float heading_offset = 0;
 /**
  * Reads the estimated IMU heading in degrees, accounting for user offset.
  *
+ * Heading is defined as clockwise positive.
+ *
  * @return                  Heading angle in the base frame.
  */
 float pbio_imu_get_heading(void) {
@@ -242,10 +244,13 @@ void pbio_imu_set_heading(float desired_heading) {
  * drivebase, which measures heading as the half the difference of the two
  * motor positions in millidegrees.
  *
- * @param [out]  heading               The output angle object.
+ * Heading is defined as clockwise positive.
+ *
+ * @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 ctl_steps_per_degree) {
+void pbio_imu_get_heading_scaled(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();
@@ -256,6 +261,11 @@ void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t ctl_steps_per_de
     // The truncated part represents everything else.
     float truncated = heading_degrees - heading->rotations * (360000 / ctl_steps_per_degree);
     heading->millidegrees = truncated * ctl_steps_per_degree;
+
+    // The heading rate can be obtained by a simple scale because it always fits.
+    pbio_geometry_xyz_t angular_rate;
+    pbio_imu_get_angular_velocity(&angular_rate);
+    *heading_rate = (int32_t)(-angular_rate.z * ctl_steps_per_degree);
 }
 
 #endif // PBIO_CONFIG_IMU