From 47756c53a1fe298d02bca8f33454b9b72a46390c Mon Sep 17 00:00:00 2001
From: Josh Pieper <jjp@pobox.com>
Date: Mon, 9 May 2022 14:00:34 -0400
Subject: [PATCH] Revert "Remove PWM compensation when using current mode
 control"

This reverts commit 02d549ee2a3f888d2088c124d395da8095c1dfbd.

r4.8 boards (which have more exaggerated PWM compensation constants),
still benefit from this a lot.

Tweak the dyno cycle limit to pass.
---
 fw/bldc_servo.cc           | 25 ++++---------------------
 utils/dynamometer_drive.cc | 10 +++++-----
 2 files changed, 9 insertions(+), 26 deletions(-)

diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc
index 097dbe13..88fe6b74 100644
--- a/fw/bldc_servo.cc
+++ b/fw/bldc_servo.cc
@@ -1708,12 +1708,11 @@ class BldcServo::Impl {
           status_.pid_q.integral,
           -max_current_integral, max_current_integral);
 
-      ISR_DoVoltageDQUncompensated(sin_cos, d_V, q_V);
+      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);
+      ISR_DoVoltageDQ(sin_cos,
+                      i_d_A * motor_.resistance_ohm,
+                      i_q_A * motor_.resistance_ohm);
     }
   }
 
@@ -1748,22 +1747,6 @@ class BldcServo::Impl {
     return Vec3{idt.a, idt.b, idt.c};
   }
 
-  void ISR_DoVoltageDQUncompensated(const SinCos& sin_cos, float d_V, float q_V) MOTEUS_CCM_ATTRIBUTE {
-    ISR_DoVoltageUncompensated(ISR_CalculatePhaseVoltage(sin_cos, d_V, q_V));
-  }
-
-  void ISR_DoVoltageUncompensated(const Vec3& voltage) MOTEUS_CCM_ATTRIBUTE {
-    const auto scale =
-        [&](float phase_voltage) {
-          return 0.5f + phase_voltage / status_.filt_bus_V;
-        };
-
-    ISR_DoPwmControl(Vec3{
-        scale(voltage.a),
-        scale(voltage.b),
-        scale(voltage.c)});
-  }
-
   void ISR_DoVoltageDQ(const SinCos& sin_cos, float d_V, float q_V) MOTEUS_CCM_ATTRIBUTE {
     ISR_DoBalancedVoltageControl(ISR_CalculatePhaseVoltage(sin_cos, d_V, q_V));
   }
diff --git a/utils/dynamometer_drive.cc b/utils/dynamometer_drive.cc
index cdf8fbf1..f3655ddb 100644
--- a/utils/dynamometer_drive.cc
+++ b/utils/dynamometer_drive.cc
@@ -209,7 +209,7 @@ class ServoStatsReader {
     // Here we verify that the final and total timer are always valid.
     if (result.final_timer == 0 ||
         result.total_timer == 0 ||
-        result.final_timer > 3100 ||
+        result.final_timer > 3220 ||
         result.total_timer < 4250) {
       throw mjlib::base::system_error::einval(
           fmt::format("Invalid timer final={} total={}",
@@ -1341,16 +1341,16 @@ class Application {
         co_await dut_->Command(
             fmt::format("d pos {} 0 {}", position, options_.max_torque_Nm));
 
-        const double kDelayS = 2.0;
+        const double kDelayS = 1.0;
         co_await Sleep(kDelayS);
 
         const double expected_torque = kDelayS * pid.ki * position;
 
         verify_position_mode();
-        if (std::abs(current_torque_Nm_ - expected_torque) > 0.24) {
+        if (std::abs(current_torque_Nm_ - expected_torque) > 0.16) {
           throw mjlib::base::system_error::einval(
               fmt::format("ki torque not as expected {} != {} (within {})",
-                          current_torque_Nm_, expected_torque, 0.24));
+                          current_torque_Nm_, expected_torque, 0.16));
         }
 
         // Stop the DUT to clear out the I term.
@@ -1903,7 +1903,7 @@ class Application {
       { 100.0, 4.04 },
       { 20.0, 3.0 },
       { 10.0, 2.09 },
-      { 5.0, 1.3 },
+      { 5.0, 1.55 },
     };
 
     for (const auto test : tests) {