implement correction, not yet confirmed working

Firmware/MotorControl/encoder.cpp

@@ -276,7 +276,7 @@ bool Encoder::run_hall_phase_calibration() {
     // at least optionally.
     // Perhaps the new loop_sync feature will give a loose timing guarantee that may be sufficient
     calibrate_hall_phase_ = true;
-    config_.hall_edge_phase.fill(0.0f);
+    config_.hall_edge_phcnt.fill(0.0f);
     hall_phase_calib_seen_count_.fill(0);
     bool success = axis_->run_lockin_spin(lockin_config, false, loop_cb);
     if (error_ & ERROR_ILLEGAL_HALL_STATE)
@@ -286,23 +286,25 @@ bool Encoder::run_hall_phase_calibration() {
         // Check deltas to dicern rotation direction
         float delta_phase = 0.0f;
         for (int i = 0; i < 6; i++) {
-            int next_i = (i == 6) ? 0 : i+1;
-            delta_phase += wrap_pm_pi(config_.hall_edge_phase[next_i] - config_.hall_edge_phase[i]);
+            int next_i = (i == 5) ? 0 : i+1;
+            delta_phase += wrap_pm_pi(config_.hall_edge_phcnt[next_i] - config_.hall_edge_phcnt[i]);
         }
         // Correct reverse rotation
         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]);
+                config_.hall_edge_phcnt[i] = wrap_pm_pi(-config_.hall_edge_phcnt[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++) 
-            config_.hall_edge_phase[i] = wrap_pm_pi(config_.hall_edge_phase[i] - offset);
+        // Normalize edge timing to 1st edge in sequence, and change units to counts
+        float offset = config_.hall_edge_phcnt[0];
+        for (int i = 0; i < 6; i++) {
+            float& phcnt = config_.hall_edge_phcnt[i];
+            phcnt = fmodf_pos((6.0f / (2.0f * M_PI)) * (phcnt - offset), 6.0f);
+        }
     } else {
-        config_.hall_edge_phase = hall_edge_phase_defaults;
+        config_.hall_edge_phcnt = hall_edge_defaults;
     }
 
     calibrate_hall_phase_ = false;
@@ -601,6 +603,28 @@ void Encoder::abs_spi_cs_pin_init(){
     abs_spi_cs_gpio_.write(true);
 }
 
+// Note that this may return counts +1 or -1 without any wrapping
+int32_t Encoder::hall_model(float internal_pos) {
+    int32_t base_cnt = (int32_t)std::floor(internal_pos);
+
+    float pos_in_range = fmodf_pos(internal_pos, 6.0f);
+    int pos_idx = (int)pos_in_range;
+    if (pos_idx == 6) pos_idx = 5; // in case of rounding error
+    int next_i = (pos_idx == 5) ? 0 : pos_idx+1;
+
+    float below_edge = config_.hall_edge_phcnt[pos_idx];
+    float above_edge = config_.hall_edge_phcnt[next_i];
+
+    // if we are blow the "below" edge, we are the count under
+    if (wrap_pm(pos_in_range - below_edge, 6.0f) < 0.0f)
+        return base_cnt - 1;
+    // if we are above the "above" edge, we are the count over
+    else if (wrap_pm(pos_in_range - above_edge, 6.0f) > 0.0f)
+        return base_cnt + 1;
+    // otherwise we are in the nominal count (or completely lost)
+    return base_cnt;
+}
+
 bool Encoder::update() {
     // update internal encoder state.
     int32_t delta_enc = 0;
@@ -641,7 +665,7 @@ bool Encoder::update() {
                                 float phase = maybe_phase.value();
                                 // Early increment to get the right divisor in recursive average
                                 hall_phase_calib_seen_count_[edge_idx]++;
-                                float& edge_phase = config_.hall_edge_phase[edge_idx];
+                                float& edge_phase = config_.hall_edge_phcnt[edge_idx];
                                 if (hall_phase_calib_seen_count_[edge_idx] == 1)
                                     edge_phase = phase;
                                 else {
@@ -726,10 +750,15 @@ bool Encoder::update() {
     pos_estimate_counts_ += current_meas_period * vel_estimate_counts_;
     pos_cpr_counts_      += current_meas_period * vel_estimate_counts_;
     // Encoder model
-
+    auto encoder_model = [this](float internal_pos)->int32_t {
+        if (config_.mode == MODE_HALL)
+            return hall_model(internal_pos);
+        else
+            return (int32_t)std::floor(internal_pos);
+    };
     // 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_));
+    float delta_pos_counts = (float)(shadow_count_ - encoder_model(pos_estimate_counts_));
+    float delta_pos_cpr_counts = (float)(count_in_cpr_ - encoder_model(pos_cpr_counts_));
     delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, (float)(config_.cpr));
     // pll feedback
     pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;

Firmware/MotorControl/encoder.hpp

@@ -11,8 +11,8 @@
 class Encoder : public ODriveIntf::EncoderIntf {
 public:
     static constexpr uint32_t MODE_FLAG_ABS = 0x100;
-    static constexpr std::array<float, 6> hall_edge_phase_defaults = 
-        {0*1.0471975512f, 1*1.0471975512f, 2*1.0471975512f, 3*1.0471975512f, 4*1.0471975512f, 5*1.0471975512f};
+    static constexpr std::array<float, 6> hall_edge_defaults = 
+        {0.0f, 1.0f, 2.0f, 3.0f, 4.0f, 5.0f};
 
     struct Config_t {
         Mode mode = MODE_INCREMENTAL;
@@ -36,7 +36,7 @@ class Encoder : public ODriveIntf::EncoderIntf {
         bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
         uint8_t hall_polarity = 0;
         bool hall_polarity_calibrated = false;
-        std::array<float, 6> hall_edge_phase = hall_edge_phase_defaults;
+        std::array<float, 6> hall_edge_phcnt = hall_edge_defaults;
         uint16_t abs_spi_cs_gpio_pin = 1;
         uint16_t sincos_gpio_pin_sin = 3;
         uint16_t sincos_gpio_pin_cos = 4;
@@ -76,6 +76,7 @@ class Encoder : public ODriveIntf::EncoderIntf {
     void sample_now();
     bool read_sampled_gpio(Stm32Gpio gpio);
     void decode_hall_samples();
+    int32_t hall_model(float internal_pos);
     bool update();
 
     TIM_HandleTypeDef* timer_;