From 030c59f48bee1e105a7e9e3ad886656c308b665e Mon Sep 17 00:00:00 2001
From: Josh Pieper <jjp@pobox.com>
Date: Fri, 19 Nov 2021 14:12:15 -0500
Subject: [PATCH] Implement servo.voltage_control_mode

In this mode, the current sense loop is not closed, and commanded
currents are translated into command voltages by assuming only the
phase resistance matters.

This can make high resistance motors easier to do useful things with
even if the default current sense resistors have not been changed.
---
 docs/reference.md | 17 +++++++++++++++++
 fw/bldc_servo.cc  | 46 ++++++++++++++++++++++++++--------------------
 fw/bldc_servo.h   |  8 ++++++++
 3 files changed, 51 insertions(+), 20 deletions(-)

diff --git a/docs/reference.md b/docs/reference.md
index 26605d8e..7fa6e0b7 100644
--- a/docs/reference.md
+++ b/docs/reference.md
@@ -1010,6 +1010,23 @@ thus *after* any scaling in position, velocity, and torque implied by
 These have the same semantics as the position mode PID controller, and
 affect the current control loop.
 
+## `servo.voltage_mode_control` ##
+
+When set to non-zero, the current control loop is not closed, and all
+current commands in amperes are instead treated as voltage mode
+commands in volts related by the calibrated phase resistance.  For
+high winding resistance motors, the default current sense resistors
+are too small for accurate current sensing, resulting in significant
+cogging torque and current sense noise.  If replacing the current
+sense resistors is not an option, this flag can be used to achieve
+smooth control.  The downside is that the actual torque will no longer
+follow the applied torque accurately at speed, or in the face of
+external disturbances.
+
+When set, the `servo.pid_dq` configuration values no longer affect
+anything.
+
+
 ## `servo.max_position_slip` ##
 
 When finite, this enforces a limit on the difference between the
diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc
index eb455b72..a43afcd8 100644
--- a/fw/bldc_servo.cc
+++ b/fw/bldc_servo.cc
@@ -1560,26 +1560,32 @@ class BldcServo::Impl {
     const float max_V = config_.max_power_W /
         (std::abs(status_.d_A) + std::abs(status_.q_A));
 
-    const float d_V =
-        Limit(
-            pid_d_.Apply(status_.d_A, i_d_A, kRateHz),
-            -max_V, max_V);
-
-    const float max_current_integral =
-        kMaxVoltageRatio * 0.5f * status_.filt_bus_V;
-    status_.pid_d.integral = Limit(
-        status_.pid_d.integral,
-        -max_current_integral, max_current_integral);
-
-    const float q_V =
-        Limit(
-            pid_q_.Apply(status_.q_A, i_q_A, kRateHz),
-            -max_V, max_V);
-    status_.pid_q.integral = Limit(
-        status_.pid_q.integral,
-        -max_current_integral, max_current_integral);
-
-    ISR_DoVoltageDQ(sin_cos, d_V, q_V);
+    if (!config_.voltage_mode_control) {
+      const float d_V =
+          Limit(
+              pid_d_.Apply(status_.d_A, i_d_A, kRateHz),
+              -max_V, max_V);
+
+      const float max_current_integral =
+          kMaxVoltageRatio * 0.5f * status_.filt_bus_V;
+      status_.pid_d.integral = Limit(
+          status_.pid_d.integral,
+          -max_current_integral, max_current_integral);
+
+      const float q_V =
+          Limit(
+              pid_q_.Apply(status_.q_A, i_q_A, kRateHz),
+              -max_V, max_V);
+      status_.pid_q.integral = Limit(
+          status_.pid_q.integral,
+          -max_current_integral, max_current_integral);
+
+      ISR_DoVoltageDQ(sin_cos, d_V, q_V);
+    } else {
+      ISR_DoVoltageDQ(sin_cos,
+                      i_d_A * motor_.resistance_ohm,
+                      i_q_A * motor_.resistance_ohm);
+    }
   }
 
   // The idiomatic thing to do in DoMeasureInductance would be to just
diff --git a/fw/bldc_servo.h b/fw/bldc_servo.h
index 299b661c..3c4ee279 100644
--- a/fw/bldc_servo.h
+++ b/fw/bldc_servo.h
@@ -174,6 +174,13 @@ class BldcServo {
     SimplePI::Config pid_dq;
     PID::Config pid_position;
 
+    // If true, then the currents in A that are calculated for the D
+    // and Q phase are instead directly commanded as voltages on the
+    // phase terminals.  This is primarily useful for high resistance
+    // motors like gimbal motors when the sense resistors are
+    // configured for a low resistance motor.
+    bool voltage_mode_control = false;
+
     float max_position_slip = std::numeric_limits<float>::quiet_NaN();
 
     float default_timeout_s = 0.1f;
@@ -248,6 +255,7 @@ class BldcServo {
       a->Visit(MJ_NVP(adc_aux_cycles));
       a->Visit(MJ_NVP(pid_dq));
       a->Visit(MJ_NVP(pid_position));
+      a->Visit(MJ_NVP(voltage_mode_control));
       a->Visit(MJ_NVP(max_position_slip));
       a->Visit(MJ_NVP(default_timeout_s));
       a->Visit(MJ_NVP(timeout_max_torque_Nm));