Fix offset translation for STS servos.

Author: kacper-uminski

src/servo/feetech/sts3215.rs

@@ -28,7 +28,7 @@ generate_servo!(
     reg: (ccw_dead_zone, rw, 27, u8, None),
     reg: (protection_current, rw, 28, u16, None),
     reg: (angular_resolution, rw, 30, u8, None),
-    reg: (offset, rw, 31, i16, AnglePosition),
+    reg: (offset, rw, 31, u16, Offset),
     reg: (mode, rw, 33, u8, None),
     reg: (protective_torque, rw, 34, u8, None),
     reg: (protection_time, rw, 35, u8, None),
@@ -76,3 +76,58 @@ impl Conversion for Velocity {
         value as u16
     }
 }
+
+pub struct Offset;
+const MAX_MAGNITUDE: u16 = 2047;
+
+impl Conversion for Offset {
+    type RegisterType = u16;
+    type UsiType = f64;
+
+    fn from_raw(raw: u16) -> f64 {
+        use std::f64::consts::PI;
+        let negative = (raw >> 11) == 1;
+        let magnitude = raw % (MAX_MAGNITUDE + 1);
+
+        let float_magnitude = PI * f64::from(magnitude) / f64::from(MAX_MAGNITUDE);
+
+        if negative {
+            -float_magnitude
+        } else {
+            float_magnitude
+        }
+    }
+
+    fn to_raw(value: f64) -> u16 {
+        use std::f64::consts::PI;
+
+        let magnitude = (value.abs() * f64::from(MAX_MAGNITUDE) / PI) as u16;
+
+        if value.is_sign_negative() {
+            magnitude | (1 << 11)
+        } else {
+            magnitude
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    #[test]
+    fn offset_conversions() {
+        use crate::servo::{conversion::Conversion, feetech::sts3215::Offset};
+        use std::f64::consts::{FRAC_PI_2, PI};
+
+        assert_eq!(Offset::to_raw(0.0), 0);
+        assert_eq!(Offset::to_raw(PI), 2047);
+        assert_eq!(Offset::to_raw(-PI), 4095);
+        assert_eq!(Offset::to_raw(FRAC_PI_2), 1023);
+        assert_eq!(Offset::to_raw(-FRAC_PI_2), 3071);
+
+        assert_eq!(Offset::from_raw(0), 0.0);
+        assert_eq!(Offset::from_raw(2047), PI);
+        assert_eq!(Offset::from_raw(4095), -PI);
+        assert_eq!(Offset::from_raw(1023), 1.5700289617109715); // About PI/2
+        assert_eq!(Offset::from_raw(3071), -1.5700289617109715); // About -PI/2
+    }
+}