Merge pull request #531 from madcowswe/hall-calib

Hall calib
2026-08-18 18:20:15 +08:00 · 2020-12-01 20:16:14 -08:00
parent 3b23e23386 17e0318048
commit 65c88eb6c8
8 changed files with 279 additions and 23 deletions
@@ -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
@@ -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;
@@ -142,7 +142,8 @@ public:

    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();
@@ -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;
@@ -11,6 +11,8 @@
 class Encoder : public ODriveIntf::EncoderIntf {
 public:
    static constexpr uint32_t MODE_FLAG_ABS = 0x100;
+    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;
@@ -22,9 +24,6 @@ public:
        float phase_offset_float = 0.0f; // Sub-count phase alignment offset
        int32_t cpr = (2048 * 4);   // Default resolution of CUI-AMT102 encoder,
        float index_offset = 0.0f;
-        uint16_t abs_spi_cs_gpio_pin = 1;
-        uint16_t sincos_gpio_pin_sin = 3;
-        uint16_t sincos_gpio_pin_cos = 4;
        bool use_index = false;
        bool pre_calibrated = false; // If true, this means the offset stored in
                                    // configuration is valid and does not need
@@ -36,6 +35,12 @@ public:
        bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
        bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
        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_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;


        // custom setters
@@ -67,10 +72,13 @@ public:

    bool run_index_search();
    bool run_direction_find();
+    bool run_hall_polarity_calibration();
+    bool run_hall_phase_calibration();
    bool run_offset_calibration();
    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_;
@@ -93,6 +101,7 @@ public:
    OutputPort<float> phase_vel_ = 0.0f; // [rad/s]
    float pos_estimate_counts_ = 0.0f;  // [count]
    float pos_cpr_counts_ = 0.0f;  // [count]
+    float delta_pos_cpr_counts_ = 0.0f;  // [count] phase detector result for debug
    float vel_estimate_counts_ = 0.0f;  // [count/s]
    float pll_kp_ = 0.0f;   // [count/s / count]
    float pll_ki_ = 0.0f;   // [(count/s^2) / count]
@@ -112,6 +121,13 @@ public:
    uint16_t port_samples_[sizeof(ports_to_sample) / sizeof(ports_to_sample[0])];
    // Updated by low_level pwm_adc_cb
    uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
+    std::optional<uint8_t> last_hall_cnt_ = std::nullopt; // Used to find hall edges for calibration
+    bool calibrate_hall_phase_ = false;
+    bool sample_hall_states_ = false;
+    bool sample_hall_phase_ = false;
+    std::array<int, 8> states_seen_count_; // for hall polarity calibration
+    std::array<int, 6> hall_phase_calib_seen_count_;
+
    float sincos_sample_s_ = 0.0f;
    float sincos_sample_c_ = 0.0f;

@@ -963,6 +963,7 @@ interfaces:
          AbsSpiTimeout:
          AbsSpiComFail:
          AbsSpiNotReady:
+          HallNotCalibratedYet:
      is_ready: readonly bool
      index_found: readonly bool
      shadow_count: readonly int32
@@ -972,6 +973,7 @@ interfaces:
      pos_estimate: {type: readonly float32, c_getter: pos_estimate_.any().value_or(0.0f)}
      pos_estimate_counts: readonly float32
      pos_cpr_counts: readonly float32
+      delta_pos_cpr_counts: readonly float32
      pos_circular: {type: readonly float32, c_getter: pos_circular_.any().value_or(0.0f)}
      hall_state: readonly uint8
      vel_estimate: {type: readonly float32, c_getter: vel_estimate_.any().value_or(0.0f)}
@@ -999,6 +1001,8 @@ interfaces:
          calib_scan_distance: float32
          calib_scan_omega: float32
          ignore_illegal_hall_state: bool
+          hall_polarity: uint8
+          hall_polarity_calibrated: bool
          sincos_gpio_pin_sin:
            type: uint16
            doc: Analog sine signal of a sin/cos encoder. The corresponding GPIO must be in `GPIO_MODE_ANALOG_IN`.
@@ -1193,6 +1197,15 @@ valuetypes:
        brief: Run axis homing function.
        doc:
          Endstops must be enabled to use this feature.
+      EncoderHallPolarityCalibration:
+        brief: Rotate the motor in lockin and calibrate hall polarity
+        doc:
+          ODrive assumes 120 degree electrical hall spacing. This routine determines if that
+          is the case and sets the polarity if the halls are on 60 degree electrical spacing
+      EncoderHallPhaseCalibration:
+        brief: Rotate the motor for 30s to calibrate hall sensor edge offsets
+        doc:
+          The phase offset is not calibrated at this time, so the map is only relative

  ODrive.Encoder.Mode:
    values:
@@ -9,7 +9,7 @@
            "type": "python",
            "request": "launch",
            "stopOnEntry": true,
-            "pythonPath": "${command:python.pythonPath}",
+            "python": "${command:python.pythonPath}",
            "program": "${file}",
            "cwd": "${workspaceRoot}",
            "env": {},
@@ -36,6 +36,8 @@ AXIS_STATE_CLOSED_LOOP_CONTROL           = 8
 AXIS_STATE_LOCKIN_SPIN                   = 9
 AXIS_STATE_ENCODER_DIR_FIND              = 10
 AXIS_STATE_HOMING                        = 11
+AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION = 12
+AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION = 13

 # ODrive.Encoder.Mode
 ENCODER_MODE_INCREMENTAL                 = 0
@@ -141,6 +143,7 @@ ENCODER_ERROR_INDEX_NOT_FOUND_YET        = 0x00000020
 ENCODER_ERROR_ABS_SPI_TIMEOUT            = 0x00000040
 ENCODER_ERROR_ABS_SPI_COM_FAIL           = 0x00000080
 ENCODER_ERROR_ABS_SPI_NOT_READY          = 0x00000100
+ENCODER_ERROR_HALL_NOT_CALIBRATED_YET    = 0x00000200

 # ODrive.SensorlessEstimator.Error
 SENSORLESS_ESTIMATOR_ERROR_NONE          = 0x00000000