From 78d4b057136704870e1979d74da08d8170e98c77 Mon Sep 17 00:00:00 2001
From: Josh Pieper <jjp@pobox.com>
Date: Tue, 8 Jun 2021 21:40:01 -0400
Subject: [PATCH] Make rezeroing to non-zero positions work again

This was broken in 5e3d7f1997dc33627ff8876908c39a6f24705926
---
 fw/bldc_servo.cc           |  4 +++-
 utils/dynamometer_drive.cc | 30 ++++++++++++++++++++++++++++++
 utils/firmware_validate.py |  3 +++
 3 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc
index 6b88114d..537f5abb 100644
--- 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
index 40ea40a9..0c7c310f 100644
--- 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
index e7c0c577..f50fcb5e 100644
--- 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):