diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index b56632afb..56e4c8422 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 cea130d28..2f384d7b3 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 @@ class Encoder : public ODriveIntf::EncoderIntf { 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 @@ class Encoder : public ODriveIntf::EncoderIntf { 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_;