From 7d062e90180aafb7207f02240a2ffd7855181056 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 6 Nov 2020 22:59:56 -0800 Subject: [PATCH 01/15] initial hall polarity detection algo --- Firmware/MotorControl/axis.cpp | 19 ++++++++--- Firmware/MotorControl/axis.hpp | 3 +- Firmware/MotorControl/encoder.cpp | 56 ++++++++++++++++++++++++++++++- Firmware/MotorControl/encoder.hpp | 3 ++ Firmware/odrive-interface.yaml | 4 +++ 5 files changed, 79 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 8971e415..2942c076 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -181,7 +181,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 const_vel_cb) { CRITICAL_SECTION() { // Reset state variables open_loop_controller_.Idq_setpoint_ = {0.0f, 0.0f}; @@ -212,7 +213,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; @@ -231,11 +232,14 @@ 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 (reached_target_vel && const_vel_cb) + const_vel_cb(); + osDelay(1); } @@ -496,6 +500,13 @@ void Axis::run_state_machine_loop() { status = encoder_.run_direction_find(); } break; + case AXIS_STATE_ENCODER_HALL_CALIBRATION: { + if (!motor_.is_calibrated_) + goto invalid_state_label; + + status = encoder_.run_hall_calibration(); + } break; + case AXIS_STATE_HOMING: { status = run_homing(); } break; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index e9477e0b..0894fd4f 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 const_vel_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 7c40a185..b15e56ef 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, @@ -197,6 +197,60 @@ bool Encoder::run_direction_find() { return success; } + +bool Encoder::run_hall_calibration() { + + // This will run every cycle when the lockin has reached the constant speed part + int states_seen_count[8] = {0}; + auto constant_speed_cb = [this, &states_seen_count]() { + states_seen_count[hall_state_]++; + }; + + 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; + + bool success = axis_->run_lockin_spin(lockin_config, false, constant_speed_cb); + + 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; + } + + uint8_t states = state_seen.to_ulong(); + uint8_t hall_polarity = 0; + auto flip_detect = [](uint8_t states, unsigned int idx)->bool { + return ~states == (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; + } + hall_polarity_ = hall_polarity; + } + + 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. diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 841f7a4f..2390b01d 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -65,6 +65,7 @@ public: bool run_index_search(); bool run_direction_find(); + bool run_hall_calibration(); bool run_offset_calibration(); void sample_now(); bool read_sampled_gpio(Stm32Gpio gpio); @@ -110,6 +111,8 @@ 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 + bool hall_calibration_running_ = false; + 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 c8d04b05..a623f1e1 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -1159,6 +1159,10 @@ valuetypes: brief: Run axis homing function. doc: Endstops must be enabled to use this feature. + EncoderHallCalibration: + brief: Rotate the motor in lockin and calibrate hall states + doc: + The phase offset is not calibrated at this time, so the map is only relative ODrive.Encoder.Mode: values: From b0a1ef7caa37c71759fad58576700d41870a00fa Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 7 Nov 2020 15:04:17 -0800 Subject: [PATCH 02/15] also mask the states --- Firmware/MotorControl/encoder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index b15e56ef..7ca0720a 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -231,7 +231,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 ~states == (1<<(0+idx) | 1<<(7-idx)); + return (0xFF & ~states) == (1<<(0+idx) | 1<<(7-idx)); }; if (flip_detect(states, 0)) { hall_polarity = 0b000; From eab5801a656300338460460ad04f7e469b2f21e4 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Sat, 7 Nov 2020 15:11:33 -0500 Subject: [PATCH 03/15] Add enum generation to makefile --- Firmware/Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/Makefile b/Firmware/Makefile index a40ae89f..d12efb32 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -12,6 +12,7 @@ OPENOCD := openocd -f interface/stlink-v2.cfg \ all: @tup --quiet --no-environ-check + @python interface_generator_stub.py --definitions odrive-interface.yaml --template ../tools/enums_template.j2 --output ../tools/odrive/enums.py flash: all $(OPENOCD) -c init \ From 336a8b504c0f129abfb8b670c13f9b2434721581 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 7 Nov 2020 16:41:51 -0800 Subject: [PATCH 04/15] hall polarity calibration --- Firmware/MotorControl/axis.cpp | 9 ++++-- Firmware/MotorControl/axis.hpp | 2 +- Firmware/MotorControl/encoder.cpp | 50 ++++++++++++++++++------------- Firmware/MotorControl/encoder.hpp | 5 +++- Firmware/odrive-interface.yaml | 1 + tools/odrive/enums.py | 1 + 6 files changed, 42 insertions(+), 26 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 2942c076..2c45d11c 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -182,7 +182,7 @@ bool Axis::watchdog_check() { } bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed, - std::function const_vel_cb) { + std::function loop_cb) { CRITICAL_SECTION() { // Reset state variables open_loop_controller_.Idq_setpoint_ = {0.0f, 0.0f}; @@ -237,9 +237,12 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_arme subscribed_to_idx_once = true; } - if (reached_target_vel && const_vel_cb) - const_vel_cb(); + if (loop_cb) + if (!loop_cb(reached_target_vel)) + break; + // TODO: use new sync function instead + asm volatile ("" ::: "memory"); osDelay(1); } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 0894fd4f..da93bedf 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -143,7 +143,7 @@ public: bool start_closed_loop_control(); bool stop_closed_loop_control(); bool run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed, - std::function const_vel_cb = {}); + 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 7ca0720a..b861b28f 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -199,28 +199,31 @@ bool Encoder::run_direction_find() { bool Encoder::run_hall_calibration() { - - // This will run every cycle when the lockin has reached the constant speed part - int states_seen_count[8] = {0}; - auto constant_speed_cb = [this, &states_seen_count]() { - states_seen_count[hall_state_]++; - }; - 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; - bool success = axis_->run_lockin_spin(lockin_config, false, constant_speed_cb); + auto loop_cb = [this](bool const_vel) { + if (const_vel) + sample_hall_states_ = true; + // No need to cancel early + return true; + }; + + states_seen_count_.fill(0); + hall_calibrated_ = false; + 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) + if (states_seen_count_[i] > 0) state_seen[i] = true; - if (states_seen_count[i] > 50) + if (states_seen_count_[i] > 50) state_confirmed[i] = true; } if (!(state_seen == state_confirmed)) { @@ -245,7 +248,7 @@ bool Encoder::run_hall_calibration() { set_error(ERROR_ILLEGAL_HALL_STATE); return false; } - hall_polarity_ = hall_polarity; + config_.hall_polarity = hall_polarity; } return success; @@ -553,16 +556,21 @@ 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 (hall_calibrated_) { + int32_t hall_cnt; + if (decode_hall((hall_state_ ^ config_.hall_polarity), &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; + } } } } break; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 2390b01d..71542649 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -32,6 +32,7 @@ public: float bandwidth = 1000.0f; 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; uint16_t abs_spi_cs_gpio_pin = 1; uint16_t sincos_gpio_pin_sin = 3; uint16_t sincos_gpio_pin_cos = 4; @@ -111,7 +112,9 @@ 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 - bool hall_calibration_running_ = false; + bool sample_hall_states_ = false; + bool hall_calibrated_ = false; + std::array states_seen_count_; // for hall polarity calibration 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 a623f1e1..6de402ef 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -980,6 +980,7 @@ interfaces: calib_scan_response: readonly float32 pos_abs: int32 spi_error_rate: readonly float32 + hall_calibrated: readonly bool config: c_is_class: False attributes: diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 801d2e3c..2674241d 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -35,6 +35,7 @@ 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_CALIBRATION = 12 # ODrive.Encoder.Mode ENCODER_MODE_INCREMENTAL = 0 From f923d63d167f058202c0b18b8d603dbde5f4b0ff Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 7 Nov 2020 17:03:24 -0800 Subject: [PATCH 05/15] hopefully the correct is_ready --- Firmware/MotorControl/encoder.cpp | 13 ++++++++++--- Firmware/MotorControl/encoder.hpp | 4 ++-- Firmware/odrive-interface.yaml | 3 ++- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index b861b28f..7bdddb0d 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -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_calibrated) + is_ready_ = true; + if (config_.mode == Encoder::MODE_SINCOS) is_ready_ = true; if (motor_type == Motor::MOTOR_TYPE_ACIM) is_ready_ = true; @@ -213,7 +215,7 @@ bool Encoder::run_hall_calibration() { }; states_seen_count_.fill(0); - hall_calibrated_ = false; + config_.hall_calibrated = false; bool success = axis_->run_lockin_spin(lockin_config, false, loop_cb); sample_hall_states_ = false; @@ -266,6 +268,11 @@ bool Encoder::run_offset_calibration() { return false; } + if (config_.mode == MODE_HALL && !config_.hall_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_; @@ -559,7 +566,7 @@ bool Encoder::update() { if (sample_hall_states_) { states_seen_count_[hall_state_]++; } - if (hall_calibrated_) { + if (config_.hall_calibrated) { int32_t hall_cnt; if (decode_hall((hall_state_ ^ config_.hall_polarity), &hall_cnt)) { delta_enc = hall_cnt - count_in_cpr_; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 71542649..5d3d8348 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -32,7 +32,8 @@ public: float bandwidth = 1000.0f; 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; + uint8_t hall_polarity = 0; + bool hall_calibrated = false; uint16_t abs_spi_cs_gpio_pin = 1; uint16_t sincos_gpio_pin_sin = 3; uint16_t sincos_gpio_pin_cos = 4; @@ -113,7 +114,6 @@ public: // Updated by low_level pwm_adc_cb uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC bool sample_hall_states_ = false; - bool hall_calibrated_ = false; std::array states_seen_count_; // for hall polarity calibration float sincos_sample_s_ = 0.0f; diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 6de402ef..0f8d3d14 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -980,7 +980,6 @@ interfaces: calib_scan_response: readonly float32 pos_abs: int32 spi_error_rate: readonly float32 - hall_calibrated: readonly bool config: c_is_class: False attributes: @@ -1000,6 +999,8 @@ interfaces: calib_scan_distance: float32 calib_scan_omega: float32 ignore_illegal_hall_state: bool + hall_polarity: uint8 + hall_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`. From 5836cfb9d0f2a32e0e1c5706650cb90203a983c2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 7 Nov 2020 20:59:56 -0800 Subject: [PATCH 06/15] 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 From 7ad508990d276ee9ab02e66617b4e739b620ffa9 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 8 Nov 2020 21:03:46 -0800 Subject: [PATCH 07/15] implement normalized phase corrected hall calibration --- Firmware/MotorControl/encoder.cpp | 35 ++++++++++++++++++++++++++----- 1 file changed, 30 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 80c60115..5254e1bb 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -251,6 +251,7 @@ bool Encoder::run_hall_polarity_calibration() { return false; } config_.hall_polarity = hall_polarity; + config_.hall_polarity_calibrated = true; } return success; @@ -275,12 +276,27 @@ 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); + config_.hall_edge_phase.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) { - for (int i = 0; i < 6; i++) - config_.hall_edge_phase[i] /= (float)hall_phase_calib_seen_count_[i]; + // 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]); + } + // Correct reverse rotation + if (delta_phase < 0.0f) + for (int i = 0; i < 6; i++) + config_.hall_edge_phase[i] = wrap_pm_pi(-config_.hall_edge_phase[i]); + // 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); } else { config_.hall_edge_phase = hall_edge_phase_defaults; } @@ -604,7 +620,7 @@ bool Encoder::update() { 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; + 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 @@ -618,8 +634,17 @@ bool Encoder::update() { auto maybe_phase = axis_->open_loop_controller_.phase_.get_any(); if (maybe_phase) { - config_.hall_edge_phase[edge_idx] += maybe_phase.value(); + 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]; + 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: From 34f55f02c1b5dcc9546f4079fdb4b2d061481acf Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 8 Nov 2020 22:00:38 -0800 Subject: [PATCH 08/15] also find direction during hall calib --- Firmware/MotorControl/encoder.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 5254e1bb..b56632af 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -290,9 +290,13 @@ bool Encoder::run_hall_phase_calibration() { delta_phase += wrap_pm_pi(config_.hall_edge_phase[next_i] - config_.hall_edge_phase[i]); } // Correct reverse rotation - if (delta_phase < 0.0f) + 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]); + } 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++) @@ -721,6 +725,8 @@ 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 + // 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_)); From 04b4e9f84e9151a8ab8a85a056d8daf201e1edc5 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 8 Nov 2020 23:19:19 -0800 Subject: [PATCH 09/15] implement correction, not yet confirmed working --- Firmware/MotorControl/encoder.cpp | 55 +++++++++++++++++++++++-------- Firmware/MotorControl/encoder.hpp | 7 ++-- 2 files changed, 46 insertions(+), 16 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index b56632af..56e4c842 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/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; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index cea130d2..2f384d7b 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/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 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 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 @@ public: 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_phase = hall_edge_phase_defaults; + 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; @@ -76,6 +76,7 @@ public: 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_; From 7afa99447c852bc188cb3b644b95f77dfdba4086 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 12 Nov 2020 15:21:25 -0800 Subject: [PATCH 10/15] make hall calib 30s by default --- Firmware/MotorControl/encoder.cpp | 3 ++- Firmware/MotorControl/encoder.hpp | 1 + Firmware/odrive-interface.yaml | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 56e4c842..d8be9295 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -259,7 +259,7 @@ bool Encoder::run_hall_polarity_calibration() { 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_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; @@ -760,6 +760,7 @@ bool Encoder::update() { 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 2f384d7b..eef286fe 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -99,6 +99,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] diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 68ed0def..35376cc1 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -974,6 +974,7 @@ interfaces: pos_estimate: {type: readonly float32, c_getter: pos_estimate_.get_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_.get_any().value_or(0.0f)} hall_state: readonly uint8 vel_estimate: {type: readonly float32, c_getter: vel_estimate_.get_any().value_or(0.0f)} From ee06628e4cddf25eceba14e89c05b4d4c5e0277f Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 1 Dec 2020 19:48:09 -0500 Subject: [PATCH 11/15] Added comment to hall polarity detection function --- Firmware/MotorControl/encoder.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index d8be9295..812377f2 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -233,6 +233,10 @@ bool Encoder::run_hall_polarity_calibration() { 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 { From 39570ecbf4605105cf30e68f78965f282ff2ffd7 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 1 Dec 2020 21:44:07 -0500 Subject: [PATCH 12/15] Split hall calibration into polarity and offset calibration, full calibration sequence only includes hall polarity cal --- Firmware/MotorControl/axis.cpp | 18 +++++++++++++++--- Firmware/odrive-interface.yaml | 9 +++++++-- tools/.vscode/launch.json | 2 +- tools/odrive/enums.py | 3 ++- 4 files changed, 25 insertions(+), 7 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 27fc1cf9..de54d1af 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -465,6 +465,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; @@ -503,13 +505,23 @@ void Axis::run_state_machine_loop() { status = encoder_.run_direction_find(); } break; - case AXIS_STATE_ENCODER_HALL_CALIBRATION: { + case AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION: { if (!motor_.is_calibrated_) goto invalid_state_label; status = encoder_.run_hall_polarity_calibration(); - if (status) - status = encoder_.run_hall_phase_calibration(); + } break; + + case AXIS_STATE_ENCODER_HALL_OFFSET_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: { diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 35376cc1..2482184b 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -1163,8 +1163,13 @@ valuetypes: brief: Run axis homing function. doc: Endstops must be enabled to use this feature. - EncoderHallCalibration: - brief: Rotate the motor in lockin and calibrate hall states + 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 + EncoderHallOffsetCalibration: + 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 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 9442723a..ab3054ee 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -35,7 +35,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_CALIBRATION = 12 +AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION = 12 +AXIS_STATE_ENCODER_HALL_OFFSET_CALIBRATION = 13 # ODrive.Encoder.Mode ENCODER_MODE_INCREMENTAL = 0 From 3f206be2ec39697e081f88567a68d9be0b82e7df Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 1 Dec 2020 22:18:03 -0500 Subject: [PATCH 13/15] Better enum name for the hall phase calibration --- Firmware/MotorControl/axis.cpp | 2 +- Firmware/odrive-interface.yaml | 2 +- tools/odrive/enums.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index de54d1af..5d778eb0 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -512,7 +512,7 @@ void Axis::run_state_machine_loop() { status = encoder_.run_hall_polarity_calibration(); } break; - case AXIS_STATE_ENCODER_HALL_OFFSET_CALIBRATION: { + case AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION: { if (!motor_.is_calibrated_) goto invalid_state_label; diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 2482184b..e393615d 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -1168,7 +1168,7 @@ valuetypes: 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 - EncoderHallOffsetCalibration: + 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 diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index ab3054ee..e68500ae 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -36,7 +36,7 @@ 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_OFFSET_CALIBRATION = 13 +AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION = 13 # ODrive.Encoder.Mode ENCODER_MODE_INCREMENTAL = 0 From dc1721a1dd85dc6e1201d14a050a6b85489c4da3 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 1 Dec 2020 22:48:47 -0500 Subject: [PATCH 14/15] merge fixes --- Firmware/MotorControl/encoder.cpp | 2 +- Firmware/MotorControl/encoder.hpp | 3 --- Firmware/odrive-interface.yaml | 2 +- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 488a2a3a..17ac87a1 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -664,7 +664,7 @@ bool Encoder::update() { return false; } - auto maybe_phase = axis_->open_loop_controller_.phase_.get_any(); + 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 diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 4ce863a5..f20436ab 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -24,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 diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index d7d2e008..7a414ea6 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -974,7 +974,7 @@ interfaces: 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_.get_any().value_or(0.0f)} + 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)} vel_estimate_counts: readonly float32 From 17e0318048c001e3c4825cbf91e84ab4c765b220 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Tue, 1 Dec 2020 23:10:15 -0500 Subject: [PATCH 15/15] Updated changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) 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