From 4f1041a28fe9c9cd485676a893d72cd2c0ebbb5f Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 24 Sep 2024 11:06:17 +0200 Subject: [PATCH] pybricks.common.IMU.reset_heading: Raise on drive base busy. This partially reverts commit fee592555d9c56e5cb61705349a5ff36458b4772, then implements the alternate behavior. See https://github.com/pybricks/support/issues/1818 --- CHANGELOG.md | 2 +- lib/pbio/include/pbio/drivebase.h | 2 +- lib/pbio/src/drivebase.c | 21 ++++++++++++++------- lib/pbio/src/imu.c | 8 -------- pybricks/common/pb_type_imu.c | 5 +++++ 5 files changed, 21 insertions(+), 17 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6f580f78a..015c06c69 100644 --- a/CHANGELOG.md +++ b/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()`. diff --git a/lib/pbio/include/pbio/drivebase.h b/lib/pbio/include/pbio/drivebase.h index b3784c987..696645f77 100644 --- a/lib/pbio/include/pbio/drivebase.h +++ b/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); diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index 032459ebb..68ca18373 100644 --- a/lib/pbio/src/drivebase.c +++ b/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; } /** diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index 9c2a18734..62d7c1f09 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -11,7 +11,6 @@ #include #include #include -#include #include #include #include @@ -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(); } /** diff --git a/pybricks/common/pb_type_imu.c b/pybricks/common/pb_type_imu.c index f26a54389..1e8f74994 100644 --- a/pybricks/common/pb_type_imu.c +++ b/pybricks/common/pb_type_imu.c @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -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));