From 34f55f02c1b5dcc9546f4079fdb4b2d061481acf Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Sun, 8 Nov 2020 22:00:38 -0800
Subject: [PATCH] also find direction during hall calib

---
 Firmware/MotorControl/encoder.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index 5254e1bb..b56632af 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -290,9 +290,13 @@ bool Encoder::run_hall_phase_calibration() {
             delta_phase += wrap_pm_pi(config_.hall_edge_phase[next_i] - config_.hall_edge_phase[i]);
         }
         // Correct reverse rotation
-        if (delta_phase < 0.0f)
+        if (delta_phase < 0.0f) {
+            config_.direction = -1;
             for (int i = 0; i < 6; i++)
                 config_.hall_edge_phase[i] = wrap_pm_pi(-config_.hall_edge_phase[i]);
+        } else {
+            config_.direction = 1;
+        }
         // Normalize edge timing to 1st edge in sequence
         float offset = config_.hall_edge_phase[0];
         for (int i = 0; i < 6; i++) 
@@ -721,6 +725,8 @@ bool Encoder::update() {
     // Predict current pos
     pos_estimate_counts_ += current_meas_period * vel_estimate_counts_;
     pos_cpr_counts_      += current_meas_period * vel_estimate_counts_;
+    // Encoder model
+    
     // discrete phase detector
     float delta_pos_counts = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_counts_));
     float delta_pos_cpr_counts = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_counts_));