diff --git a/CHANGELOG.md b/CHANGELOG.md index 22034ba4..2f919929 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 07fd263f..aefc661a 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/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 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; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index c79c91dd..4b82cc85 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -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 loop_cb = {} ); bool run_closed_loop_control_loop(); bool run_homing(); bool run_idle_loop(); diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 05318b49..17ac87a1 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -1,7 +1,7 @@ #include "odrive_main.h" #include - +#include 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; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 9676b396..f20436ab 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_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 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 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 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 c186b678..7a414ea6 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -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: diff --git a/tools/.vscode/launch.json b/tools/.vscode/launch.json index 9a36a076..5247bf4a 100644 --- a/tools/.vscode/launch.json +++ b/tools/.vscode/launch.json @@ -9,7 +9,7 @@ "type": "python", "request": "launch", "stopOnEntry": true, - "pythonPath": "${command:python.pythonPath}", + "python": "${command:python.pythonPath}", "program": "${file}", "cwd": "${workspaceRoot}", "env": {}, diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 79649de8..d1c1e084 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -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