From 26e636ddb36b702a361291b0ab842acd881f281b Mon Sep 17 00:00:00 2001
From: Josh Pieper <jjp@pobox.com>
Date: Mon, 1 Aug 2022 07:02:12 -0400
Subject: [PATCH] Properly handle offsets in hall effect sources

---
 fw/motor_position.h                           | 14 ++++-
 fw/test/motor_position_test.cc                | 57 +++++++++++++++++++
 .../moteus/test/calibrate_encoder_test.py     | 34 +++++++++++
 3 files changed, 102 insertions(+), 3 deletions(-)

diff --git a/fw/motor_position.h b/fw/motor_position.h
index c777ffa9..76cd4d66 100644
--- a/fw/motor_position.h
+++ b/fw/motor_position.h
@@ -738,10 +738,18 @@ class MotorPosition {
           status.raw = hall_data->bits;
 
           const uint32_t cpr = kHallCounts / 2 * motor_.poles;
-          const int32_t delta = static_cast<int32_t>(
-              (config.sign * hall_data->count + 2 * cpr + (kHallCounts / 2) -
-               status.offset_value) % kHallCounts) -
+          const int32_t current_with_offset =
+              ((hall_data->count +
+                static_cast<int32_t>(config.offset)) * config.sign +
+               kHallCounts) % kHallCounts;
+          const int32_t old_with_offset =
+              status.offset_value % kHallCounts;
+          const int32_t delta =
+              (current_with_offset - old_with_offset +
+               kHallCounts + kHallCounts / 2) %
+              kHallCounts -
               (kHallCounts / 2);
+
           const uint32_t new_value = (status.offset_value + cpr + delta) % cpr;
 
           updated = ISR_UpdateAbsoluteSource(
diff --git a/fw/test/motor_position_test.cc b/fw/test/motor_position_test.cc
index 0732aae8..52052a19 100644
--- a/fw/test/motor_position_test.cc
+++ b/fw/test/motor_position_test.cc
@@ -715,6 +715,63 @@ BOOST_AUTO_TEST_CASE(MotorPositionHallSource) {
 
     run(ctx, test_values);
   }
+
+  {
+    Context ctx;
+    ctx.dut.config()->sources[0].type = MotorPosition::SourceConfig::kHall;
+    ctx.dut.config()->sources[0].sign = -1;
+    ctx.dut.config()->sources[0].offset = 1;
+    ctx.aux1_status.hall.active = true;
+
+    TestValues test_values[] = {
+      { 0, 11 },
+      { 1, 10 },
+      { 2, 9 },
+      { 3, 8 },
+      { 4, 7 },
+      { 5, 6 },
+      { 0, 5 },
+      { 1, 4 },
+      { 2, 3 },
+      { 3, 2 },
+      { 4, 1 },
+      { 5, 0 },
+      { 0, 11 },
+      { 1, 10 },
+      { 0, 11 },
+      { 5, 0 },
+    };
+
+    run(ctx, test_values);
+  }
+  {
+    Context ctx;
+    ctx.dut.config()->sources[0].type = MotorPosition::SourceConfig::kHall;
+    ctx.dut.config()->sources[0].sign = -1;
+    ctx.dut.config()->sources[0].offset = -2;
+    ctx.aux1_status.hall.active = true;
+
+    TestValues test_values[] = {
+      { 0, 2 },
+      { 1, 1 },
+      { 2, 0 },
+      { 3, 11 },
+      { 4, 10 },
+      { 5, 9 },
+      { 0, 8 },
+      { 1, 7 },
+      { 2, 6 },
+      { 3, 5 },
+      { 4, 4 },
+      { 5, 3 },
+      { 0, 2 },
+      { 1, 1 },
+      { 0, 2 },
+      { 5, 3 },
+    };
+
+    run(ctx, test_values);
+  }
 }
 
 BOOST_AUTO_TEST_CASE(MotorPositionQuadratureTest) {
diff --git a/lib/python/moteus/test/calibrate_encoder_test.py b/lib/python/moteus/test/calibrate_encoder_test.py
index 0d614641..0ecc49be 100644
--- a/lib/python/moteus/test/calibrate_encoder_test.py
+++ b/lib/python/moteus/test/calibrate_encoder_test.py
@@ -2260,6 +2260,40 @@ def test(b1, b2, b3, desired_dir,
         test(0, 1, 2, -1,    0,  0,  1, 1)
         test(1, 0, 2, -1,    0, -2, -1, 1)
 
+    def test_calibrate_hall2(self):
+        data1 = [
+            (0.0, 4),
+            (0.2617993877991494, 4),
+            (0.5235987755982988, 6),
+            (0.7853981633974483, 6),
+            (1.0471975511965976, 6),
+            (1.3089969389957472, 6),
+            (1.5707963267948966, 6),
+            (1.8325957145940461, 2),
+            (2.0943951023931953, 2),
+            (2.356194490192345, 2),
+            (2.6179938779914944, 3),
+            (2.8797932657906435, 3),
+            (3.141592653589793, 3),
+            (3.4033920413889422, 3),
+            (3.6651914291880923, 3),
+            (3.9269908169872414, 1),
+            (4.1887902047863905, 1),
+            (4.4505895925855405, 1),
+            (4.71238898038469, 5),
+            (4.974188368183839, 5),
+            (5.235987755982989, 5),
+            (5.497787143782138, 5),
+            (5.759586531581287, 5),
+            (6.021385919380437, 4),
+        ]
+
+        result = ce.calibrate_hall(data1, desired_direction=0)
+        self.assertEqual(result.polarity, 0)
+        self.assertEqual(result.offset, -4)
+        self.assertEqual(result.sign, -1)
+        self.assertEqual(result.phase_invert, 0)
+
 
 if __name__ == '__main__':
     unittest.main()