Fix wrapping on pico IMU pwm
Using modulo to keep the heading between 0 and 360 was allowing it to go
negative then getting cast to an unsigned int. It *almost* had the right
behavior, but there was a 16 degree range below zero where it was
getting rounded to 0.
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I4bc3ef01f83d6b9c1925c4faaa09718e9aeacef0
diff --git a/frc971/imu/ADIS16505.cc b/frc971/imu/ADIS16505.cc
index 4c31cab..bd65e4d 100644
--- a/frc971/imu/ADIS16505.cc
+++ b/frc971/imu/ADIS16505.cc
@@ -377,7 +377,12 @@
pwm_set_gpio_level(RATE_PWM, rate_level);
// 0 to 360
- uint16_t heading_level = (int16_t)(fmod(yaw, 360) / 360.0 * PWM_TOP);
+ double wrapped_heading = fmod(yaw, 360);
+ if (wrapped_heading < 0) {
+ wrapped_heading = wrapped_heading + 360;
+ }
+
+ uint16_t heading_level = (int16_t)(wrapped_heading / 360.0 * PWM_TOP);
pwm_set_gpio_level(HEADING_PWM, heading_level);
}