Limit the DQ integrator to an achievable voltage

Previously, the DQ integrator would be allowed to wind up well beyond
the maximum voltage that could be output.  If operating at the maximum
possible speed, this could result in large delays when slowing down.

We also tweak the test fixture's rigid PID parameters to work with
modern firmwares.

fw/bldc_servo.cc

@@ -150,6 +150,7 @@ constexpr float kCurrentSampleTime = 1.03e-6f;
 
 constexpr float kMinPwm = kCurrentSampleTime / (0.5f / static_cast<float>(kPwmRateHz));
 constexpr float kMaxPwm = 1.0f - kMinPwm;
+constexpr float kMaxVoltageRatio = (kMaxPwm - 0.5f) * 2.0f;
 
 constexpr float kRateHz = kIntRateHz;
 constexpr float kPeriodS = 1.0f / kRateHz;
@@ -1420,12 +1421,21 @@ class BldcServo::Impl {
         (config_.feedforward_scale * i_d_A * motor_.resistance_ohm) +
         pid_d_.Apply(status_.d_A, i_d_A, 1.0f, 0.0f, kRateHz);
 
+    const float max_current_integral =
+        kMaxVoltageRatio * 0.25f * status_.filt_bus_V;
+    status_.pid_d.integral = Limit(
+        status_.pid_d.integral,
+        -max_current_integral, max_current_integral);
+
     const float q_V =
         (config_.feedforward_scale * (
             i_q_A * motor_.resistance_ohm +
             status_.velocity * motor_.v_per_hz /
             motor_.unwrapped_position_scale)) +
         pid_q_.Apply(status_.q_A, i_q_A, 0.0f, 0.0f, kRateHz);
+    status_.pid_q.integral = Limit(
+        status_.pid_q.integral,
+        -max_current_integral, max_current_integral);
 
     ISR_DoVoltageDQ(sin_cos, d_V, q_V);
   }

utils/dynamometer_drive.cc

@@ -88,6 +88,7 @@ struct Options {
   bool validate_max_slip = false;
   bool validate_slip_stop_position = false;
   bool validate_slip_bounds = false;
+  bool validate_dq_ilimit = false;
 
   template <typename Archive>
   void Serialize(Archive* a) {
@@ -115,6 +116,7 @@ struct Options {
     a->Visit(MJ_NVP(validate_max_slip));
     a->Visit(MJ_NVP(validate_slip_stop_position));
     a->Visit(MJ_NVP(validate_slip_bounds));
+    a->Visit(MJ_NVP(validate_dq_ilimit));
   }
 };
 
@@ -584,6 +586,8 @@ class Application {
       co_await ValidateSlipStopPosition();
     } else if (options_.validate_slip_bounds) {
       co_await ValidateSlipBounds();
+    } else if (options_.validate_dq_ilimit) {
+      co_await ValidateDqIlimit();
     } else {
       fmt::print("No cycle selected\n");
     }
@@ -609,8 +613,8 @@ class Application {
   boost::asio::awaitable<void> CommandFixtureRigid() {
     Controller::PidConstants pid;
     pid.ki = 200.0;
-    pid.kd = 0.2;
-    pid.kp = 10.0;
+    pid.kd = 0.15;
+    pid.kp = 5.0;
     pid.ilimit = 0.3;
 
     co_await fixture_->ConfigurePid(pid);
@@ -906,7 +910,7 @@ class Application {
                     const std::string& message) -> boost::asio::awaitable<void> {
       fmt::print("Testing d={} q={}\n", d_A, q_A);
       co_await dut_->Command(fmt::format("d dq {} {}", d_A, q_A));
-      co_await Sleep(0.5);
+      co_await Sleep(1.0);
 
       try {
         if (dut_->servo_stats().mode != ServoStats::kCurrent) {
@@ -1780,6 +1784,32 @@ class Application {
     co_return;
   }
 
+  boost::asio::awaitable<void> ValidateDqIlimit() {
+    Controller::PidConstants pid;
+    pid.position_min = kNaN;
+    pid.position_max = kNaN;
+
+    pid.kp = 1.0;
+    pid.ki = 0.0;
+    pid.kd = 0.05;
+
+    co_await dut_->Command("d stop");
+    co_await fixture_->Command("d stop");
+
+    // The dq ilimit shouldn't be able to go larger than the input
+    // voltage permits.  Otherwise, it can wind-up, resulting in a
+    // long time before torque stops being applied after time spent at
+    // maximum speed.
+
+    // It would be nice to validate this in the normal run, but to do
+    // so we'd need to spin at maximum speed for some time, which
+    // isn't probably super advisable at the normal 24V run of these
+    // tests.  When we have control over the voltage input, then we
+    // could consider making an actual automated test for this.
+
+    co_return;
+  }
+
   boost::asio::awaitable<void> Sleep(double seconds) {
     boost::asio::deadline_timer timer(executor_);
     timer.expires_from_now(mjlib::base::ConvertSecondsToDuration(seconds));

utils/firmware_validate.py

@@ -85,6 +85,9 @@ def test_validate_slip_stop_position(self):
     def test_validate_slip_bounds(self):
         dyno('--validate_slip_bounds', '1')
 
+    def test_validate_dq_ilimit(self):
+        dyno('--validate_dq_ilimit', '1')
+
 
 class TestDynoSlow(unittest.TestCase):
     def test_torque_ripple(self):