pid/src/lib.rs

@@ -147,10 +147,13 @@ impl<T: SaturatingAdd + SaturatingSub + Scaled + Copy> Pid<T> {
         let deriv_error = error.saturating_sub(&self.prev_error);
         self.prev_error = error;
         self.integrated_error = self.integrated_error.saturating_add(&error);
+
+        // The resulting p, i, and d are kept unscaled until they are added.
+        // The addition will then be rescaled.
         let p = error.saturating_mul_unscaled(self.k_p);
         let i = self.integrated_error.saturating_mul_unscaled(self.k_i);
         let d = deriv_error.saturating_mul_unscaled(self.k_d);
-        (p + i + d).rescale()
+        p.saturating_add(&i).saturating_add(&d).rescale()
     }
 
     pub fn command(&mut self, target: T, measured: T) -> T {

pid/tests/fixed.rs

-use pid::Fixed;
+use pid::{Fixed, Scaled};
 
 type F16 = Fixed<16>;
 
@@ -21,3 +21,70 @@ fn round() {
     let v = F16::from_raw(-0x7fff);
     assert_eq!(v.round(), 0);
 }
+
+type F8 = Fixed<8>;
+
+#[test]
+fn rescale() {
+    let a = F8::from_raw(0x12345678);
+    let b = a.rescale();
+    assert_eq!(b, F8::from_raw(0x123456));
+
+    let a = F8::from_raw(0x12345687);
+    let b = a.rescale();
+    assert_eq!(b, F8::from_raw(0x123457));
+
+    let a = F8::from_raw(-1);
+    let b = a.rescale();
+    assert_eq!(b, F8::from_raw(0));
+
+    let a = F8::from_raw(-1);
+    let b = a.rescale();
+    assert_eq!(b, F8::from_raw(0));
+
+    let a = F8::from_raw(-127);
+    let b = a.rescale();
+    assert_eq!(b, F8::from_raw(0));
+
+    let a = F8::from_raw(-128);
+    let b = a.rescale();
+    assert_eq!(b, F8::from_raw(-1));
+}
+
+#[test]
+fn mul_unscaled() {
+    let a = F8::from_int(1);
+    let b = F8::from_int(2);
+    let c = a.saturating_mul_unscaled(b);
+    assert_eq!(c, F8::from_int(512));
+
+    let a = F8::from_int(-1);
+    let b = F8::from_int(-2);
+    let c = a.saturating_mul_unscaled(b);
+    assert_eq!(c, F8::from_int(512));
+
+    let a = F8::from_int(-1);
+    let b = F8::from_int(2);
+    let c = a.saturating_mul_unscaled(b);
+    assert_eq!(c, F8::from_int(-512));
+
+    let a = F8::from_int(1);
+    let b = F8::from_int(-2);
+    let c = a.saturating_mul_unscaled(b);
+    assert_eq!(c, F8::from_int(-512));
+
+    let a = F8::from_raw(0x7fff_ffff);
+    let b = F8::from_raw(2);
+    let c = a.saturating_mul_unscaled(b);
+    assert_eq!(c, F8::from_raw(0x7fff_ffff));
+
+    let a = F8::from_raw(-0x8000_0000);
+    let b = F8::from_raw(2);
+    let c = a.saturating_mul_unscaled(b);
+    assert_eq!(c, F8::from_raw(-0x8000_0000));
+
+    let a = F8::from_raw(-0x8000_0000);
+    let b = F8::from_raw(0x7fff_ffff);
+    let c = a.saturating_mul_unscaled(b);
+    assert_eq!(c, F8::from_raw(-0x8000_0000));
+}