From 482459d7d2e1a0dcca0af420e35fba9896ba9ccd Mon Sep 17 00:00:00 2001
From: Josh Pieper <jjp@pobox.com>
Date: Sat, 12 Mar 2022 12:38:38 -0500
Subject: [PATCH] When in zero velocity mode, ensure we don't apply any ki

---
 fw/bldc_servo.cc | 5 +++--
 fw/pid.h         | 4 +++-
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc
index f28a8b75..ddf2b513 100644
--- a/fw/bldc_servo.cc
+++ b/fw/bldc_servo.cc
@@ -1710,8 +1710,9 @@ class BldcServo::Impl {
 
   void ISR_DoZeroVelocity(const SinCos& sin_cos, CommandData* data) MOTEUS_CCM_ATTRIBUTE {
     PID::ApplyOptions apply_options;
-    apply_options.kp_scale = 0.0;
-    apply_options.kd_scale = 1.0;
+    apply_options.kp_scale = 0.0f;
+    apply_options.kd_scale = 1.0f;
+    apply_options.ki_scale = 0.0f;
 
     ISR_DoPositionCommon(sin_cos, data,
                          apply_options, config_.timeout_max_torque_Nm,
diff --git a/fw/pid.h b/fw/pid.h
index 20845d60..fc2cc954 100644
--- a/fw/pid.h
+++ b/fw/pid.h
@@ -100,6 +100,7 @@ class PID {
   struct ApplyOptions {
     float kp_scale = 1.0f;
     float kd_scale = 1.0f;
+    float ki_scale = 1.0f;
 
     ApplyOptions() {}
   };
@@ -153,7 +154,8 @@ class PID {
     state_->d = apply_options.kd_scale * config_->kd * state_->error_rate;
     state_->pd = state_->p + state_->d;
 
-    state_->command = config_->sign * (state_->pd + state_->integral);
+    state_->command = config_->sign *
+        (state_->pd + apply_options.ki_scale * state_->integral);
 
     return state_->command;
   }