From 31562aca12b26d293be518bd9699cd774a2439f3 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Tue, 15 Jan 2019 19:44:07 -0800
Subject: [PATCH] add voltage magnitude clamping for gimbal motors in closed
 loop

---
 CHANGELOG.md                         | 1 +
 Firmware/MotorControl/controller.cpp | 7 ++++++-
 Firmware/MotorControl/motor.cpp      | 1 -
 3 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 76f535d2..57a6de10 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -4,6 +4,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
 ### Added
 * `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors.
 * `q` command to ascii protocol. It is like the old `p` command, but velocity and current mean limits, not feed-forward.
+* Voltage limit soft clamping instead of ERROR_MODULATION_MAGNITUDE in gimbal motor closed loop.
 
 # Releases
 ## [0.4.7] - 2018-11-28
diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 16aef47a..debca10d 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -183,8 +183,13 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
     Iq += vel_integrator_current_;
 
     // Current limiting
-    float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
     bool limited = false;
+    float Ilim = 0.0f;
+    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
+        Ilim = std::min(axis_->motor_.config_.current_lim, 0.98f*one_by_sqrt3*vbus_voltage);
+    } else {
+        Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
+    }
     if (Iq > Ilim) {
         limited = true;
         Iq = Ilim;
diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
index 7cee19b0..74ea20bc 100644
--- a/Firmware/MotorControl/motor.cpp
+++ b/Firmware/MotorControl/motor.cpp
@@ -282,7 +282,6 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
     return true;
 }
 
-// TODO: This doesn't update brake current
 // We should probably make FOC Current call FOC Voltage to avoid duplication.
 bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
     float c = our_arm_cos_f32(phase);