Merge pull request odriverobotics#531 from madcowswe/hall-calib

Hall calib

CHANGELOG.md

@@ -1,11 +1,13 @@
 # Unreleased Features
 Please add a note of your changes below this heading if you make a Pull Request.
 ### Added
+* Added polarity and phase offset calibration for hall effect encoders
 * [Mechanical brake support](docs/mechanical-brakes.md)
 * Added periodic sending of encoder position on CAN
 * Support for UART1 on GPIO3 and GPIO4. UART0 (on GPIO1/2) and UART1 can currently not be enabled at the same time.
 
 ### Changed
+* Full calibration sequence now includes hall polarity calibration if a hall effect encoder is used
 * Modified encoder offset calibration to work correctly when calib_scan_distance is not a multiple of 4pi
 * Moved thermistors from being a top level object to belonging to Motor objects. Also changed errors: thermistor errors rolled into motor errors
 * Use DMA for DRV8301 setup

Firmware/MotorControl/axis.cpp

@@ -183,7 +183,8 @@ bool Axis::watchdog_check() {
     }
 }
 
-bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed) {
+bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed,
+        std::function<bool(bool)> loop_cb) {
     CRITICAL_SECTION() {
         // Reset state variables
         open_loop_controller_.Idq_setpoint_ = {0.0f, 0.0f};
@@ -214,7 +215,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_arme
 
     motor_.arm(&motor_.current_control_);
 
-    bool subscribed_to_idx = false;
+    bool subscribed_to_idx_once = false;
     bool success = false;
     float dir = lockin_config.vel >= 0.0f ? 1.0f : -1.0f;
 
@@ -233,11 +234,17 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_arme
 
         // Activate index pin as soon as target velocity was reached. This is
         // to avoid hitting the index from the wrong direction.
-        if (reached_target_vel && !encoder_.index_found_ && !subscribed_to_idx) {
+        if (reached_target_vel && !encoder_.index_found_ && !subscribed_to_idx_once) {
             encoder_.set_idx_subscribe(true);
-            subscribed_to_idx = true;
+            subscribed_to_idx_once = true;
         }
 
+        if (loop_cb)
+            if (!loop_cb(reached_target_vel))
+                break;
+
+        // TODO: use new sync function instead
+        asm volatile ("" ::: "memory");
         osDelay(1);
     }
 
@@ -446,6 +453,8 @@ void Axis::run_state_machine_loop() {
                 task_chain_[pos++] = AXIS_STATE_IDLE;
             } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
                 task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
+                if (encoder_.config_.mode == ODriveIntf::EncoderIntf::MODE_HALL)
+                    task_chain_[pos++] = AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION;
                 if (encoder_.config_.use_index)
                     task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
                 task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
@@ -494,6 +503,25 @@ void Axis::run_state_machine_loop() {
                 status = encoder_.run_direction_find();
             } break;
 
+            case AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
+
+                status = encoder_.run_hall_polarity_calibration();
+            } break;
+
+            case AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
+
+                if (!encoder_.config_.hall_polarity_calibrated) {
+                    encoder_.set_error(ODriveIntf::EncoderIntf::ERROR_HALL_NOT_CALIBRATED_YET);
+                    goto invalid_state_label;
+                }
+
+                status = encoder_.run_hall_phase_calibration();
+            } break;
+
             case AXIS_STATE_HOMING: {
                 if (odrv.any_error())
                     goto invalid_state_label;

Firmware/MotorControl/axis.hpp

@@ -142,7 +142,8 @@ class Axis : public ODriveIntf::AxisIntf {
 
     bool start_closed_loop_control();
     bool stop_closed_loop_control();
-    bool run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed);
+    bool run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed,
+                std::function<bool(bool)> loop_cb = {} );
     bool run_closed_loop_control_loop();
     bool run_homing();
     bool run_idle_loop();

Firmware/MotorControl/encoder.cpp

@@ -1,7 +1,7 @@
 
 #include "odrive_main.h"
 #include <Drivers/STM32/stm32_system.h>
-
+#include <bitset>
 
 Encoder::Encoder(TIM_HandleTypeDef* timer, Stm32Gpio index_gpio,
                  Stm32Gpio hallA_gpio, Stm32Gpio hallB_gpio, Stm32Gpio hallC_gpio,
@@ -22,7 +22,9 @@ bool Encoder::apply_config(ODriveIntf::MotorIntf::MotorType motor_type) {
     update_pll_gains();
 
     if (config_.pre_calibrated) {
-        if (config_.mode == Encoder::MODE_HALL || config_.mode == Encoder::MODE_SINCOS)
+        if (config_.mode == Encoder::MODE_HALL && config_.hall_polarity_calibrated)
+            is_ready_ = true;
+        if (config_.mode == Encoder::MODE_SINCOS)
             is_ready_ = true;
         if (motor_type == Motor::MOTOR_TYPE_ACIM)
             is_ready_ = true;
@@ -197,6 +199,122 @@ bool Encoder::run_direction_find() {
     return success;
 }
 
+
+bool Encoder::run_hall_polarity_calibration() {
+    Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
+    lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
+    lockin_config.finish_on_distance = true;
+    lockin_config.finish_on_enc_idx = false;
+    lockin_config.finish_on_vel = false;
+
+    auto loop_cb = [this](bool const_vel) {
+        if (const_vel)
+            sample_hall_states_ = true;
+        // No need to cancel early
+        return true;
+    };
+
+    config_.hall_polarity_calibrated = false;
+    states_seen_count_.fill(0);
+    bool success = axis_->run_lockin_spin(lockin_config, false, loop_cb);
+    sample_hall_states_ = false;
+
+    if (success) {
+        std::bitset<8> state_seen;
+        std::bitset<8> state_confirmed;
+        for (int i = 0; i < 8; i++) {
+            if (states_seen_count_[i] > 0)
+                state_seen[i] = true;
+            if (states_seen_count_[i] > 50)
+                state_confirmed[i] = true;
+        }
+        if (!(state_seen == state_confirmed)) {
+            set_error(ERROR_ILLEGAL_HALL_STATE);
+            return false;
+        }
+
+        // Hall effect sensors can be arranged at 60 or 120 electrical degrees.
+        // Out of 8 possible states, 120 and 60 deg arrangements each miss 2 states.
+        // ODrive assumes 120 deg separation - if a 60 deg setup is used, it can
+        // be converted to 120 deg states by flipping the polarity of one sensor.
+        uint8_t states = state_seen.to_ulong();
+        uint8_t hall_polarity = 0;
+        auto flip_detect = [](uint8_t states, unsigned int idx)->bool {
+            return (~states & 0xFF) == (1<<(0+idx) | 1<<(7-idx));
+        };
+        if (flip_detect(states, 0)) {
+            hall_polarity = 0b000;
+        } else if (flip_detect(states, 1)) {
+            hall_polarity = 0b001;
+        } else if (flip_detect(states, 2)) {
+            hall_polarity = 0b010;
+        } else if (flip_detect(states, 3)) {
+            hall_polarity = 0b100;
+        } else {
+            set_error(ERROR_ILLEGAL_HALL_STATE);
+            return false;
+        }
+        config_.hall_polarity = hall_polarity;
+        config_.hall_polarity_calibrated = true;
+    }
+
+    return success;
+}
+
+bool Encoder::run_hall_phase_calibration() {
+    Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
+    lockin_config.finish_distance = lockin_config.vel * 30.0f; // run for 30 seconds
+    lockin_config.finish_on_distance = true;
+    lockin_config.finish_on_enc_idx = false;
+    lockin_config.finish_on_vel = false;
+
+    auto loop_cb = [this](bool const_vel) {
+        if (const_vel)
+            sample_hall_phase_ = true;
+        // No need to cancel early
+        return true;
+    };
+
+    // TODO: There is a race condition here with the execution in Encoder::update.
+    // We should evaluate making thread execution synchronous with the control loops
+    // 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_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)
+        success = false;
+
+    if (success) {
+        // Check deltas to dicern rotation direction
+        float delta_phase = 0.0f;
+        for (int i = 0; i < 6; 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_phcnt[i] = wrap_pm_pi(-config_.hall_edge_phcnt[i]);
+        } else {
+            config_.direction = 1;
+        }
+        // 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_phcnt = hall_edge_defaults;
+    }
+
+    calibrate_hall_phase_ = false;
+    return success;
+}
+
 // @brief Turns the motor in one direction for a bit and then in the other
 // direction in order to find the offset between the electrical phase 0
 // and the encoder state 0.
@@ -209,6 +327,11 @@ bool Encoder::run_offset_calibration() {
         return false;
     }
 
+    if (config_.mode == MODE_HALL && !config_.hall_polarity_calibrated) {
+        set_error(ERROR_HALL_NOT_CALIBRATED_YET);
+        return false;
+    }
+
     // We use shadow_count_ to do the calibration, but the offset is used by count_in_cpr_
     // Therefore we have to sync them for calibration
     shadow_count_ = count_in_cpr_;
@@ -484,6 +607,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;
@@ -499,16 +644,56 @@ bool Encoder::update() {
 
         case MODE_HALL: {
             decode_hall_samples();
-            int32_t hall_cnt;
-            if (decode_hall(hall_state_, &hall_cnt)) {
-                delta_enc = hall_cnt - count_in_cpr_;
-                delta_enc = mod(delta_enc, 6);
-                if (delta_enc > 3)
-                    delta_enc -= 6;
-            } else {
-                if (!config_.ignore_illegal_hall_state) {
-                    set_error(ERROR_ILLEGAL_HALL_STATE);
-                    return false;
+            if (sample_hall_states_) {
+                states_seen_count_[hall_state_]++;
+            }
+            if (config_.hall_polarity_calibrated) {
+                int32_t hall_cnt;
+                if (decode_hall((hall_state_ ^ config_.hall_polarity), &hall_cnt)) {
+                    if (calibrate_hall_phase_) {
+                        if (sample_hall_phase_ && last_hall_cnt_.has_value()) {
+                            int mod_hall_cnt = mod(hall_cnt - last_hall_cnt_.value(), 6);
+                            size_t edge_idx;
+                            if (mod_hall_cnt == 0) { goto skip; } // no count - do nothing
+                            else if (mod_hall_cnt == 1) { // counted up
+                                edge_idx = hall_cnt;
+                            } else if (mod_hall_cnt == 5) { // counted down
+                                edge_idx = last_hall_cnt_.value();
+                            } else {
+                                set_error(ERROR_ILLEGAL_HALL_STATE);
+                                return false;
+                            }
+
+                            auto maybe_phase = axis_->open_loop_controller_.phase_.any();
+                            if (maybe_phase) {
+                                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_phcnt[edge_idx];
+                                if (hall_phase_calib_seen_count_[edge_idx] == 1)
+                                    edge_phase = phase;
+                                else {
+                                    // circularly wrapped recursive average
+                                    edge_phase += (phase - edge_phase) / hall_phase_calib_seen_count_[edge_idx];
+                                    edge_phase = wrap_pm_pi(edge_phase);
+                                }
+                            }
+                        }
+                    skip:
+                        last_hall_cnt_ = hall_cnt;
+
+                        return true; // Skip all velocity and phase estimation
+                    }
+
+                    delta_enc = hall_cnt - count_in_cpr_;
+                    delta_enc = mod(delta_enc, 6);
+                    if (delta_enc > 3)
+                        delta_enc -= 6;
+                } else {
+                    if (!config_.ignore_illegal_hall_state) {
+                        set_error(ERROR_ILLEGAL_HALL_STATE);
+                        return false;
+                    }
                 }
             }
         } break;
@@ -568,10 +753,18 @@ 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
+    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));
+    delta_pos_cpr_counts_ += 0.1f * (delta_pos_cpr_counts - delta_pos_cpr_counts_); // for debug
     // pll feedback
     pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;
     pos_cpr_counts_ += current_meas_period * pll_kp_ * delta_pos_cpr_counts;