From 900ae1735804ac4bb006d131dba493fbd5714655 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 17 Oct 2024 13:32:33 +0200 Subject: [PATCH] pbio/imu: Fix internal rounding error from imu to motor control units. The drivebase controller operates in terms of millidegrees of the wheels. When the gyro is used for heading, it is scaled to these wheel units. Depending on the robot dimensions, this scaling could be off by one or two degrees per rotation due to a rounding error. Fixes https://github.com/pybricks/support/issues/1886 --- lib/pbio/src/imu.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/lib/pbio/src/imu.c b/lib/pbio/src/imu.c index 62d7c1f09..ad773f24b 100644 --- a/lib/pbio/src/imu.c +++ b/lib/pbio/src/imu.c @@ -288,11 +288,12 @@ void pbio_imu_get_heading_scaled(pbio_angle_t *heading, int32_t *heading_rate, i float heading_degrees = pbio_imu_get_heading(); // Number of whole rotations in control units (in terms of wheels, not robot). - heading->rotations = heading_degrees / (360000 / ctl_steps_per_degree); + heading->rotations = (int32_t)(heading_degrees / (360000.0f / ctl_steps_per_degree)); - // 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 truncated part represents everything else. NB: The scaling factor + // is a float here to ensure we don't lose precision while scaling. + float truncated = heading_degrees - heading->rotations * (360000.0f / ctl_steps_per_degree); + heading->millidegrees = (int32_t)(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;