From ef03591379add1a8bfcd6502e017f64c0c5d98f8 Mon Sep 17 00:00:00 2001
From: pbrier <git@pbrier.nl>
Date: Thu, 16 Sep 2021 10:06:07 +0200
Subject: [PATCH] Added integrator_limit to allow prevention of integrator
 windup

---
 Firmware/MotorControl/controller.cpp | 2 ++
 Firmware/MotorControl/controller.hpp | 1 +
 Firmware/odrive-interface.yaml       | 5 +++++
 3 files changed, 8 insertions(+)

diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 59dd1d84..4149b30d 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -364,6 +364,8 @@ bool Controller::update() {
         } else {
             vel_integrator_torque_ += ((vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err;
         }
+        // integrator limiting to prevent windup 
+        vel_integrator_torque_ = std::clamp(vel_integrator_torque_, -config_.vel_integrator_limit, config_.vel_integrator_limit);
     }
 
     float ideal_electrical_power = 0.0f;
diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index 91a85ec5..da19149e 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -30,6 +30,7 @@ public:
         float vel_integrator_gain = 2.0f / 6.0f; // [Nm/(turn/s * s)]
         float vel_limit = 2.0f;                  // [turn/s] Infinity to disable.
         float vel_limit_tolerance = 1.2f;        // ratio to vel_lim. Infinity to disable.
+        float vel_integrator_limit = INFINITY;   // Vel. integrator clamping value. Infinity to disable.
         float vel_ramp_rate = 1.0f;              // [(turn/s) / s]
         float torque_ramp_rate = 0.01f;          // Nm / sec
         bool circular_setpoints = false;
diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml
index 53c5aedb..eb0104aa 100644
--- a/Firmware/odrive-interface.yaml
+++ b/Firmware/odrive-interface.yaml
@@ -931,6 +931,11 @@ interfaces:
             type: float32
             unit: Nm/(turn/s * s)
             doc: units = Nm/(turn/s * s), default = 0.32
+            doc: units = 'Nm/(turn/s), default = 0.16
+          vel_integrator_limit:
+            type: float32
+            unit: Nm
+            doc: Limit the integrator output (independent of proportional gain output) to prevent 'integrator windup'. Set to infinity to disable. Units = Nm, default = inf
           vel_limit:
             type: float32
             unit: turn/s