From 5c84b7438bf80fc252b91487d5d2fbda30afacfa Mon Sep 17 00:00:00 2001
From: topquark12 <aw@m-labs.hk>
Date: Fri, 8 Jan 2021 11:25:01 +0800
Subject: [PATCH] Integral rescaling

---
 src/main.rs |  4 ++--
 src/pid.rs  | 10 ++++++++++
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index 571d04f..c745a8f 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -332,8 +332,8 @@ fn main() -> ! {
                                             pid.target = value,
                                         KP =>
                                             pid.parameters.kp = value as f32,
-                                        KI =>
-                                            pid.parameters.ki = value as f32,
+                                        KI => 
+                                            pid.update_ki(value as f32),
                                         KD =>
                                             pid.parameters.kd = value as f32,
                                         OutputMin =>
diff --git a/src/pid.rs b/src/pid.rs
index 6001c54..9da0246 100644
--- a/src/pid.rs
+++ b/src/pid.rs
@@ -109,6 +109,16 @@ impl Controller {
             integral: self.integral,
         }
     }
+
+    pub fn update_ki(&mut self, new_ki: f32) {
+        if new_ki == 0.0 {
+            self.integral = 0.0;
+        } else {
+            // Rescale integral with changes to kI, aka "Bumpless operation"
+            self.integral = f64::from(self.parameters.ki) * self.integral / f64::from(new_ki);
+        }
+        self.parameters.ki = new_ki;
+    }
 }
 
 #[derive(Clone, Serialize, Deserialize)]