From 5836cfb9d0f2a32e0e1c5706650cb90203a983c2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 7 Nov 2020 20:59:56 -0800 Subject: [PATCH] hall phase calib first pass --- Firmware/MotorControl/axis.cpp | 4 +- Firmware/MotorControl/encoder.cpp | 71 ++++++++++++++++++++++++++++--- Firmware/MotorControl/encoder.hpp | 12 +++++- Firmware/odrive-interface.yaml | 3 +- tools/odrive/enums.py | 1 + 5 files changed, 81 insertions(+), 10 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 2c45d11c..27fc1cf9 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -507,7 +507,9 @@ void Axis::run_state_machine_loop() { if (!motor_.is_calibrated_) goto invalid_state_label; - status = encoder_.run_hall_calibration(); + status = encoder_.run_hall_polarity_calibration(); + if (status) + status = encoder_.run_hall_phase_calibration(); } break; case AXIS_STATE_HOMING: { diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 7bdddb0d..80c60115 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -22,7 +22,7 @@ bool Encoder::apply_config(ODriveIntf::MotorIntf::MotorType motor_type) { update_pll_gains(); if (config_.pre_calibrated) { - if (config_.mode == Encoder::MODE_HALL && config_.hall_calibrated) + if (config_.mode == Encoder::MODE_HALL && config_.hall_polarity_calibrated) is_ready_ = true; if (config_.mode == Encoder::MODE_SINCOS) is_ready_ = true; @@ -200,7 +200,7 @@ bool Encoder::run_direction_find() { } -bool Encoder::run_hall_calibration() { +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; @@ -214,8 +214,8 @@ bool Encoder::run_hall_calibration() { return true; }; + config_.hall_polarity_calibrated = false; states_seen_count_.fill(0); - config_.hall_calibrated = false; bool success = axis_->run_lockin_spin(lockin_config, false, loop_cb); sample_hall_states_ = false; @@ -236,7 +236,7 @@ bool Encoder::run_hall_calibration() { uint8_t states = state_seen.to_ulong(); uint8_t hall_polarity = 0; auto flip_detect = [](uint8_t states, unsigned int idx)->bool { - return (0xFF & ~states) == (1<<(0+idx) | 1<<(7-idx)); + return (~states & 0xFF) == (1<<(0+idx) | 1<<(7-idx)); }; if (flip_detect(states, 0)) { hall_polarity = 0b000; @@ -256,6 +256,39 @@ bool Encoder::run_hall_calibration() { return success; } +bool Encoder::run_hall_phase_calibration() { + Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin; + lockin_config.finish_distance = lockin_config.vel * 10.0f; // run for 10 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_phase.fill(0); + bool success = axis_->run_lockin_spin(lockin_config, false, loop_cb); + + if (success) { + for (int i = 0; i < 6; i++) + config_.hall_edge_phase[i] /= (float)hall_phase_calib_seen_count_[i]; + } else { + config_.hall_edge_phase = hall_edge_phase_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. @@ -268,7 +301,7 @@ bool Encoder::run_offset_calibration() { return false; } - if (config_.mode == MODE_HALL && !config_.hall_calibrated) { + if (config_.mode == MODE_HALL && !config_.hall_polarity_calibrated) { set_error(ERROR_HALL_NOT_CALIBRATED_YET); return false; } @@ -566,9 +599,35 @@ bool Encoder::update() { if (sample_hall_states_) { states_seen_count_[hall_state_]++; } - if (config_.hall_calibrated) { + 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 = (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_.get_any(); + if (maybe_phase) { + config_.hall_edge_phase[edge_idx] += maybe_phase.value(); + hall_phase_calib_seen_count_[edge_idx]++; + } + } + 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) diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 5d3d8348..cea130d2 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -11,6 +11,8 @@ class Encoder : public ODriveIntf::EncoderIntf { public: static constexpr uint32_t MODE_FLAG_ABS = 0x100; + static constexpr std::array hall_edge_phase_defaults = + {0*1.0471975512f, 1*1.0471975512f, 2*1.0471975512f, 3*1.0471975512f, 4*1.0471975512f, 5*1.0471975512f}; struct Config_t { Mode mode = MODE_INCREMENTAL; @@ -33,7 +35,8 @@ public: 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_calibrated = false; + bool hall_polarity_calibrated = false; + std::array hall_edge_phase = hall_edge_phase_defaults; uint16_t abs_spi_cs_gpio_pin = 1; uint16_t sincos_gpio_pin_sin = 3; uint16_t sincos_gpio_pin_cos = 4; @@ -67,7 +70,8 @@ public: bool run_index_search(); bool run_direction_find(); - bool run_hall_calibration(); + bool run_hall_polarity_calibration(); + bool run_hall_phase_calibration(); bool run_offset_calibration(); void sample_now(); bool read_sampled_gpio(Stm32Gpio gpio); @@ -113,8 +117,12 @@ 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 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 states_seen_count_; // for hall polarity calibration + std::array hall_phase_calib_seen_count_; float sincos_sample_s_ = 0.0f; float sincos_sample_c_ = 0.0f; diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 0f8d3d14..68ed0def 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -964,6 +964,7 @@ interfaces: AbsSpiTimeout: AbsSpiComFail: AbsSpiNotReady: + HallNotCalibratedYet: is_ready: readonly bool index_found: readonly bool shadow_count: readonly int32 @@ -1000,7 +1001,7 @@ interfaces: calib_scan_omega: float32 ignore_illegal_hall_state: bool hall_polarity: uint8 - hall_calibrated: bool + 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`. diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 2674241d..9442723a 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -140,6 +140,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