Make rezeroing to non-zero positions work again

This was broken in 5e3d7f1
mjbots · Jun 9, 2021 · 78d4b05 · 78d4b05
1 parent b842366
commit 78d4b05
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 1 deletion.
diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc
@@ -871,7 +871,9 @@ class BldcServo::Impl {
           std::round(error / motor_.unwrapped_position_scale);
       status_.unwrapped_position_raw =
           static_cast<int64_t>(65536ll * 65536ll) *
-          zero_position + integral_offsets * 65536.0f;
+          zero_position +
+          static_cast<int32_t>(integral_offsets * 65536.0f) *
+          65536ll * 65536ll;
       status_.position_to_set = std::numeric_limits<float>::quiet_NaN();
       // In case we are in encoder PLL mode.
       status_.velocity = 0.0f;

diff --git a/utils/dynamometer_drive.cc b/utils/dynamometer_drive.cc
@@ -91,6 +91,7 @@ struct Options {
   bool validate_dq_ilimit = false;
   bool validate_power_limit = false;
   bool validate_max_velocity = false;
+  bool validate_rezero = false;
 
   template <typename Archive>
   void Serialize(Archive* a) {
@@ -121,6 +122,7 @@ struct Options {
     a->Visit(MJ_NVP(validate_dq_ilimit));
     a->Visit(MJ_NVP(validate_power_limit));
     a->Visit(MJ_NVP(validate_max_velocity));
+    a->Visit(MJ_NVP(validate_rezero));
   }
 };
 
@@ -609,6 +611,8 @@ class Application {
       co_await ValidatePowerLimit();
     } else if (options_.validate_max_velocity) {
       co_await ValidateMaxVelocity();
+    } else if (options_.validate_rezero) {
+      co_await ValidateRezero();
     } else {
       fmt::print("No cycle selected\n");
     }
@@ -1940,6 +1944,32 @@ class Application {
     co_return;
   }
 
+  boost::asio::awaitable<void> DoRezeroTest(double value) {
+    co_await dut_->Command(fmt::format("d rezero {}", value));
+    co_await Sleep(0.5);
+
+    {
+      // We should be within 0.5 of the desired.
+      const auto pos = dut_->servo_stats().unwrapped_position;
+      if (std::abs(pos - value) > 0.5) {
+        throw mjlib::base::system_error::einval(
+            fmt::format(
+                "DUT rezero != {} ({})",
+                value, pos));
+      }
+    }
+  }
+  boost::asio::awaitable<void> ValidateRezero() {
+    co_await fixture_->Command("d stop");
+    co_await dut_->Command("d stop");
+
+    co_await DoRezeroTest(0.0);
+    co_await DoRezeroTest(4.2);
+    co_await DoRezeroTest(-7.9);
+
+    co_return;
+  }
+
   boost::asio::awaitable<void> Sleep(double seconds) {
     boost::asio::deadline_timer timer(executor_);
     timer.expires_from_now(mjlib::base::ConvertSecondsToDuration(seconds));

diff --git a/utils/firmware_validate.py b/utils/firmware_validate.py
@@ -94,6 +94,9 @@ def test_validate_power_limit(self):
     def test_validate_max_velocity(self):
         dyno('--validate_max_velocity', '1')
 
+    def test_rezero(self):
+        dyno('--validate_rezero', '1')
+
 
 class TestDynoSlow(unittest.TestCase):
     def test_torque_ripple(self):