pybricks.common.IMU.reset_heading: Raise on drive base busy.

This partially reverts commit fee5925, then implements the alternate behavior.

See pybricks/support#1818

Author: laurensvalk

CHANGELOG.md

@@ -42,7 +42,7 @@
 - Reduced default angular velocity stationary threshold from an undocumented
   5 deg/s to 3 deg/s to reduce unwanted calibration while moving ([support#1105]).
 - If `imu.reset_heading()` is called while a drive base is actively using the
-  gyro, the drive base will stop to avoid confusion  ([support#1818]).
+  gyro, an exception will be raised ([support#1818]).
 
 ### Fixed
 - Fixed not able to connect to new Technic Move hub with `LWP3Device()`.

lib/pbio/include/pbio/drivebase.h

@@ -55,7 +55,7 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
 
 void pbio_drivebase_update_all(void);
 bool pbio_drivebase_update_loop_is_running(pbio_drivebase_t *db);
-void pbio_drivebase_stop_all_when_gyro_used(void);
+bool pbio_drivebase_any_uses_gyro(void);
 bool pbio_drivebase_is_done(const pbio_drivebase_t *db);
 pbio_error_t pbio_drivebase_is_stalled(pbio_drivebase_t *db, bool *stalled, uint32_t *stall_duration);
 

lib/pbio/src/drivebase.c

@@ -765,18 +765,25 @@ pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_
 }
 
 /**
- * Stops all drivebases that use the gyro. Called by the imu module when the
- * imu heading is reset. Resetting it would throw off ongoing drivebase
- * controls, so we should stop them.
+ * Tests if any drive base is currently actively using the gyro.
+ *
+ * @return @c true if the gyro is being used, else @c false
  */
-void pbio_drivebase_stop_all_when_gyro_used(void) {
+bool pbio_drivebase_any_uses_gyro(void) {
     for (uint8_t i = 0; i < PBIO_CONFIG_NUM_DRIVEBASES; i++) {
         pbio_drivebase_t *db = &drivebases[i];
-        if (pbio_drivebase_update_loop_is_running(db) && db->use_gyro) {
-            // Let errors pass.
-            pbio_drivebase_stop(db, PBIO_CONTROL_ON_COMPLETION_COAST);
+
+        // Only consider activated drive bases that use the gyro.
+        if (!pbio_drivebase_update_loop_is_running(db) || !db->use_gyro) {
+            continue;
+        }
+
+        // Active controller means driving or holding, so gyro is in use.
+        if (pbio_control_is_active(&db->control_distance) || pbio_control_is_active(&db->control_heading)) {
+            return true;
         }
     }
+    return false;
 }
 
 /**

lib/pbio/src/imu.c

@@ -11,7 +11,6 @@
 #include <pbio/angle.h>
 #include <pbio/config.h>
 #include <pbio/dcmotor.h>
-#include <pbio/drivebase.h>
 #include <pbio/error.h>
 #include <pbio/geometry.h>
 #include <pbio/imu.h>
@@ -268,13 +267,6 @@ float pbio_imu_get_heading(void) {
  */
 void pbio_imu_set_heading(float desired_heading) {
     heading_offset = pbio_imu_get_heading() + heading_offset - desired_heading;
-
-    // Callbacks to other resources to inform that the gyro has been (re)set.
-
-    // REVISIT: At the moment, only drivebases use the gyro. If more resources
-    // need it, we can enable subscribing to the imu and have a callback
-    // called here on resets.
-    pbio_drivebase_stop_all_when_gyro_used();
 }
 
 /**

pybricks/common/pb_type_imu.c

@@ -9,6 +9,7 @@
 #include <stdbool.h>
 #include <string.h>
 
+#include <pbio/drivebase.h>
 #include <pbio/error.h>
 #include <pbio/geometry.h>
 #include <pbio/imu.h>
@@ -213,6 +214,10 @@ static mp_obj_t pb_type_imu_reset_heading(size_t n_args, const mp_obj_t *pos_arg
         pb_type_imu_obj_t, self,
         PB_ARG_REQUIRED(angle));
 
+    if (pbio_drivebase_any_uses_gyro()) {
+        mp_raise_msg(&mp_type_OSError, MP_ERROR_TEXT("Can't reset heading while gyro in use. Stop driving first."));
+    }
+
     // Set the new angle
     (void)self;
     pbio_imu_set_heading(mp_obj_get_float(angle_in));