pbio/drivebase: Add getter for float angle.

Fixes pybricks/support#1844

Author: laurensvalk

CHANGELOG.md

@@ -4,6 +4,18 @@
 
 ## [Unreleased]
 
+### Changed
+
+- The method `DriveBase.angle()` now returns a float ([support#1844]). This
+  makes it properly equivalent to `hub.imu.heading`.
+
+### Fixed
+- Fixed `DriveBase.angle()` getting an incorrectly rounded gyro value, which
+  could cause `turn(360)` to be off by a degree ([support#1844]).
+
+[support#1844]: https://github.com/pybricks/support/issues/1844
+[support#1886]: https://github.com/pybricks/support/issues/1886
+
 ## [3.6.0b2] - 2024-10-15
 
 ### Added

lib/pbio/include/pbio/angle.h

@@ -34,6 +34,7 @@ typedef struct _pbio_angle_t {
 // Conversion to and from basic types:
 
 int32_t pbio_angle_to_low_res(const pbio_angle_t *a, int32_t scale);
+float pbio_angle_to_low_res_float(const pbio_angle_t *a, float scale);
 void pbio_angle_from_low_res(pbio_angle_t *a, int32_t input, int32_t scale);
 
 // Inplace operations on an angle:

lib/pbio/include/pbio/control_settings.h

@@ -130,6 +130,7 @@ uint32_t pbio_control_time_ms_to_ticks(uint32_t ms);
 uint32_t pbio_control_time_ticks_to_ms(uint32_t ticks);
 int32_t pbio_control_settings_ctl_to_app(const pbio_control_settings_t *s, int32_t input);
 int32_t pbio_control_settings_ctl_to_app_long(const pbio_control_settings_t *s, const pbio_angle_t *input);
+float pbio_control_settings_ctl_to_app_long_float(const pbio_control_settings_t *s, const pbio_angle_t *input);
 int32_t pbio_control_settings_app_to_ctl(const pbio_control_settings_t *s, int32_t input);
 void pbio_control_settings_app_to_ctl_long(const pbio_control_settings_t *s, int32_t input, pbio_angle_t *output);
 int32_t pbio_control_settings_actuation_ctl_to_app(int32_t input);

lib/pbio/include/pbio/drivebase.h

@@ -75,6 +75,7 @@ pbio_error_t pbio_drivebase_stop(pbio_drivebase_t *db, pbio_control_on_completio
 // Measuring and settings:
 
 pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distance, int32_t *drive_speed, int32_t *angle, int32_t *turn_rate);
+pbio_error_t pbio_drivebase_get_state_user_angle(pbio_drivebase_t *db, float *angle);
 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);

lib/pbio/src/angle.c

@@ -141,6 +141,22 @@ int32_t pbio_angle_to_low_res(const pbio_angle_t *a, int32_t scale) {
     return rotations_component + millidegree_component;
 }
 
+/**
+ * Scales down high resolution angle to floating point value.
+ *
+ * @param [out]  a       Angle a.
+ * @param [in]   scale   Ratio between high resolution angle and input.
+ */
+float pbio_angle_to_low_res_float(const pbio_angle_t *a, float scale) {
+
+    // Fail safely on zero division.
+    if (scale < 1) {
+        return 0;
+    }
+
+    return a->rotations * (MDEG_PER_ROT / scale) + a->millidegrees / scale;
+}
+
 /**
  * Populates object from scaled-up integer value.
  *

lib/pbio/src/control_settings.c

@@ -48,7 +48,7 @@ int32_t pbio_control_settings_ctl_to_app(const pbio_control_settings_t *s, int32
 }
 
 /**
- * Converts position-like control units to application-specific units.
+ * Converts position-like control units to application-specific units (integer).
  *
  * This can be used with large inputs but there is more overhead.
  *
@@ -60,6 +60,17 @@ int32_t pbio_control_settings_ctl_to_app_long(const pbio_control_settings_t *s,
     return pbio_angle_to_low_res(input, s->ctl_steps_per_app_step);
 }
 
+/**
+ * Converts position-like control units to application-specific units (float).
+ *
+ * @param [in] s              Control settings containing the scale.
+ * @param [in] input          Signal in control units.
+ * @return                    Signal in application units.
+ */
+float pbio_control_settings_ctl_to_app_long_float(const pbio_control_settings_t *s, const pbio_angle_t *input) {
+    return pbio_angle_to_low_res_float(input, s->ctl_steps_per_app_step);
+}
+
 /**
  * Converts application-specific units to position-like control units.
  *

lib/pbio/src/drivebase.c

@@ -800,6 +800,28 @@ pbio_error_t pbio_drivebase_get_state_user(pbio_drivebase_t *db, int32_t *distan
     return PBIO_SUCCESS;
 }
 
+/**
+ * Gets the drivebase angle in user units. This is the same as the angle
+ * returned by ::pbio_drivebase_get_state_user, but as a floating point value.
+ *
+ * @param [in]  db          The drivebase instance.
+ * @param [out] angle       Angle turned in degrees, floating point.
+ * @return                  Error code.
+ */
+pbio_error_t pbio_drivebase_get_state_user_angle(pbio_drivebase_t *db, float *angle) {
+
+    // Get drive base state
+    pbio_control_state_t state_distance;
+    pbio_control_state_t state_heading;
+    pbio_error_t err = pbio_drivebase_get_state_control(db, &state_distance, &state_heading);
+    if (err != PBIO_SUCCESS) {
+        return err;
+    }
+
+    *angle = pbio_control_settings_ctl_to_app_long_float(&db->control_heading.settings, &state_heading.position);
+    return PBIO_SUCCESS;
+}
+
 /**
  * Stops the drivebase and resets the accumulated drivebase state in user units.
  *

pybricks/robotics/pb_type_drivebase.c

@@ -279,10 +279,15 @@ MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_distance_obj, pb_type_DriveBase_dist
 static mp_obj_t pb_type_DriveBase_angle(mp_obj_t self_in) {
     pb_type_DriveBase_obj_t *self = MP_OBJ_TO_PTR(self_in);
 
+    #if MICROPY_PY_BUILTINS_FLOAT
+    float angle;
+    pb_assert(pbio_drivebase_get_state_user_angle(self->db, &angle));
+    return mp_obj_new_float_from_f(angle);
+    #else
     int32_t heading, _;
     pb_assert(pbio_drivebase_get_state_user(self->db, &_, &_, &heading, &_));
-
     return mp_obj_new_int(heading);
+    #endif
 }
 MP_DEFINE_CONST_FUN_OBJ_1(pb_type_DriveBase_angle_obj, pb_type_DriveBase_angle);