pbio/drivebase: Stop drivebase on gyro reset.

Author: laurensvalk

CHANGELOG.md

@@ -20,6 +20,8 @@
 - Added one byte program identifier to the hub status report to the host.
 - Added interface and implementation for storing and selecting multiple code
   slots on the Prime Hub and Inventor Hub.
+- Added ability to set distance and angle in `DriveBase.reset()`. If the
+  DriveBase is using the gyro, it will be set to the same angle. ([support#1617]).
 
 ### Changed
 
@@ -36,9 +38,11 @@
   the Bluetooth light. Only warning lights will be shown on the main button
   light. See ([support#1716]) and ([pybricks-micropython#261]).
 - Allow gyro calibration only while all motors are coasting ([support#1840]) to
-  prevent recalibration during very steady moves.
+  prevent recalibration during very steady moves ([support#1687])
 - Reduced default angular velocity stationary threshold from an undocumented
-  5 deg/s to 3 deg/s to reduce unwanted calibration while moving  ([support#1105]). 
+  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]).
 
 ### Fixed
 - Fixed not able to connect to new Technic Move hub with `LWP3Device()`.
@@ -58,9 +62,12 @@
 [support#1429]: https://github.com/pybricks/support/issues/1429
 [support#1460]: https://github.com/pybricks/support/issues/1460
 [support#1615]: https://github.com/pybricks/support/issues/1615
+[support#1617]: https://github.com/pybricks/support/issues/1617
 [support#1622]: https://github.com/pybricks/support/issues/1622
 [support#1678]: https://github.com/pybricks/support/issues/1678
+[support#1687]: https://github.com/pybricks/support/issues/1687
 [support#1716]: https://github.com/pybricks/support/issues/1716
+[support#1818]: https://github.com/pybricks/support/issues/1818
 [support#1840]: https://github.com/pybricks/support/issues/1840
 
 ## [3.5.0] - 2024-04-11

lib/pbio/include/pbio/drivebase.h

@@ -55,6 +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_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

@@ -725,8 +725,8 @@ pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distan
  * If the gyro is being used for control, it will be reset to the same angle.
  *
  * @param [in]  db          The drivebase instance.
- * @param [out] distance    Distance traveled in mm.
- * @param [out] angle       Angle turned in degrees.
+ * @param [in] distance     Distance traveled in mm.
+ * @param [in] angle        Angle turned in degrees.
  * @return                  Error code.
  */
 pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_t angle) {
@@ -756,13 +756,28 @@ pbio_error_t pbio_drivebase_reset(pbio_drivebase_t *db, int32_t distance, int32_
     pbio_angle_from_low_res(&reported_new, angle, db->control_heading.settings.ctl_steps_per_app_step);
     pbio_angle_diff(&measured_heading.position, &reported_new, &db->heading_offset);
 
-    // Whether or not the gyro is being used, synchronize heading and drivebase
-    // angle state.
-    pbio_imu_set_heading(angle);
+    // Synchronize heading and drivebase angle state if gyro in use.
+    if (db->use_gyro) {
+        pbio_imu_set_heading(angle);
+    }
 
     return PBIO_SUCCESS;
 }
 
+/**
+ * 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.
+ */
+void pbio_drivebase_stop_all_when_gyro_used(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);
+        }
+    }
+}
 
 /**
  * Gets the drivebase settings in user units.

lib/pbio/src/imu.c

@@ -11,6 +11,7 @@
 #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>
@@ -267,6 +268,13 @@ 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();
 }
 
 /**