mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-18 09:29:03 +08:00
642 lines
22 KiB
C++
642 lines
22 KiB
C++
|
|
#include "odrive_main.h"
|
|
#include <Drivers/STM32/stm32_system.h>
|
|
|
|
|
|
Encoder::Encoder(TIM_HandleTypeDef* timer, Stm32Gpio index_gpio,
|
|
Stm32Gpio hallA_gpio, Stm32Gpio hallB_gpio, Stm32Gpio hallC_gpio,
|
|
Stm32SpiArbiter* spi_arbiter) :
|
|
timer_(timer), index_gpio_(index_gpio),
|
|
hallA_gpio_(hallA_gpio), hallB_gpio_(hallB_gpio), hallC_gpio_(hallC_gpio),
|
|
spi_arbiter_(spi_arbiter)
|
|
{
|
|
}
|
|
|
|
static void enc_index_cb_wrapper(void* ctx) {
|
|
reinterpret_cast<Encoder*>(ctx)->enc_index_cb();
|
|
}
|
|
|
|
bool Encoder::apply_config(ODriveIntf::MotorIntf::MotorType motor_type) {
|
|
config_.parent = this;
|
|
|
|
update_pll_gains();
|
|
|
|
if (config_.pre_calibrated) {
|
|
if (config_.mode == Encoder::MODE_HALL || config_.mode == Encoder::MODE_SINCOS)
|
|
is_ready_ = true;
|
|
if (motor_type == Motor::MOTOR_TYPE_ACIM)
|
|
is_ready_ = true;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void Encoder::setup() {
|
|
HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL);
|
|
set_idx_subscribe();
|
|
|
|
mode_ = config_.mode;
|
|
|
|
spi_task_.config = {
|
|
.Mode = SPI_MODE_MASTER,
|
|
.Direction = SPI_DIRECTION_2LINES,
|
|
.DataSize = SPI_DATASIZE_16BIT,
|
|
.CLKPolarity = mode_ == MODE_SPI_ABS_AEAT ? SPI_POLARITY_HIGH : SPI_POLARITY_LOW,
|
|
.CLKPhase = SPI_PHASE_2EDGE,
|
|
.NSS = SPI_NSS_SOFT,
|
|
.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32,
|
|
.FirstBit = SPI_FIRSTBIT_MSB,
|
|
.TIMode = SPI_TIMODE_DISABLE,
|
|
.CRCCalculation = SPI_CRCCALCULATION_DISABLE,
|
|
.CRCPolynomial = 10,
|
|
};
|
|
|
|
if(mode_ & MODE_FLAG_ABS){
|
|
abs_spi_cs_pin_init();
|
|
|
|
if (axis_->controller_.config_.anticogging.pre_calibrated) {
|
|
axis_->controller_.anticogging_valid_ = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Encoder::set_error(Error error) {
|
|
vel_estimate_valid_ = false;
|
|
pos_estimate_valid_ = false;
|
|
error_ |= error;
|
|
}
|
|
|
|
bool Encoder::do_checks(){
|
|
return error_ == ERROR_NONE;
|
|
}
|
|
|
|
//--------------------
|
|
// Hardware Dependent
|
|
//--------------------
|
|
|
|
// Triggered when an encoder passes over the "Index" pin
|
|
// TODO: only arm index edge interrupt when we know encoder has powered up
|
|
// (maybe by attaching the interrupt on start search, synergistic with following)
|
|
void Encoder::enc_index_cb() {
|
|
if (config_.use_index) {
|
|
set_circular_count(0, false);
|
|
if (config_.zero_count_on_find_idx)
|
|
set_linear_count(0); // Avoid position control transient after search
|
|
if (config_.pre_calibrated) {
|
|
is_ready_ = true;
|
|
if(axis_->controller_.config_.anticogging.pre_calibrated){
|
|
axis_->controller_.anticogging_valid_ = true;
|
|
}
|
|
} else {
|
|
// We can't use the update_offset facility in set_circular_count because
|
|
// we also set the linear count before there is a chance to update. Therefore:
|
|
// Invalidate offset calibration that may have happened before idx search
|
|
is_ready_ = false;
|
|
}
|
|
index_found_ = true;
|
|
}
|
|
|
|
// Disable interrupt
|
|
index_gpio_.unsubscribe();
|
|
}
|
|
|
|
void Encoder::set_idx_subscribe(bool override_enable) {
|
|
if (config_.use_index && (override_enable || !config_.find_idx_on_lockin_only)) {
|
|
if (!index_gpio_.subscribe(true, false, enc_index_cb_wrapper, this)) {
|
|
odrv.misconfigured_ = true;
|
|
}
|
|
} else if (!config_.use_index || config_.find_idx_on_lockin_only) {
|
|
index_gpio_.unsubscribe();
|
|
}
|
|
}
|
|
|
|
void Encoder::update_pll_gains() {
|
|
pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time
|
|
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped
|
|
|
|
// Check that we don't get problems with discrete time approximation
|
|
if (!(current_meas_period * pll_kp_ < 1.0f)) {
|
|
set_error(ERROR_UNSTABLE_GAIN);
|
|
}
|
|
}
|
|
|
|
void Encoder::check_pre_calibrated() {
|
|
// TODO: restoring config from python backup is fragile here (ACIM motor type must be set first)
|
|
if (!is_ready_ && axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_ACIM)
|
|
config_.pre_calibrated = false;
|
|
if (mode_ == MODE_INCREMENTAL && !index_found_)
|
|
config_.pre_calibrated = false;
|
|
}
|
|
|
|
// Function that sets the current encoder count to a desired 32-bit value.
|
|
void Encoder::set_linear_count(int32_t count) {
|
|
// Disable interrupts to make a critical section to avoid race condition
|
|
uint32_t prim = cpu_enter_critical();
|
|
|
|
// Update states
|
|
shadow_count_ = count;
|
|
pos_estimate_counts_ = (float)count;
|
|
tim_cnt_sample_ = count;
|
|
|
|
//Write hardware last
|
|
timer_->Instance->CNT = count;
|
|
|
|
cpu_exit_critical(prim);
|
|
}
|
|
|
|
// Function that sets the CPR circular tracking encoder count to a desired 32-bit value.
|
|
// Note that this will get mod'ed down to [0, cpr)
|
|
void Encoder::set_circular_count(int32_t count, bool update_offset) {
|
|
// Disable interrupts to make a critical section to avoid race condition
|
|
uint32_t prim = cpu_enter_critical();
|
|
|
|
if (update_offset) {
|
|
config_.offset += count - count_in_cpr_;
|
|
config_.offset = mod(config_.offset, config_.cpr);
|
|
}
|
|
|
|
// Update states
|
|
count_in_cpr_ = mod(count, config_.cpr);
|
|
pos_cpr_counts_ = (float)count_in_cpr_;
|
|
|
|
cpu_exit_critical(prim);
|
|
}
|
|
|
|
bool Encoder::run_index_search() {
|
|
config_.use_index = true;
|
|
index_found_ = false;
|
|
set_idx_subscribe();
|
|
|
|
bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin, false);
|
|
return status;
|
|
}
|
|
|
|
bool Encoder::run_direction_find() {
|
|
int32_t init_enc_val = shadow_count_;
|
|
|
|
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 status = axis_->run_lockin_spin(lockin_config, false);
|
|
|
|
if (status) {
|
|
// Check response and direction
|
|
if (shadow_count_ > init_enc_val + 8) {
|
|
// motor same dir as encoder
|
|
config_.direction = 1;
|
|
} else if (shadow_count_ < init_enc_val - 8) {
|
|
// motor opposite dir as encoder
|
|
config_.direction = -1;
|
|
} else {
|
|
config_.direction = 0;
|
|
}
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
// @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.
|
|
bool Encoder::run_offset_calibration() {
|
|
const float start_lock_duration = 1.0f;
|
|
|
|
// Require index found if enabled
|
|
if (config_.use_index && !index_found_) {
|
|
set_error(ERROR_INDEX_NOT_FOUND_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_;
|
|
|
|
CRITICAL_SECTION() {
|
|
// Reset state variables
|
|
axis_->open_loop_controller_.Id_setpoint_ = NAN;
|
|
axis_->open_loop_controller_.Iq_setpoint_ = NAN;
|
|
axis_->open_loop_controller_.Vd_setpoint_ = NAN;
|
|
axis_->open_loop_controller_.Vq_setpoint_ = NAN;
|
|
axis_->open_loop_controller_.phase_ = 0.0f;
|
|
axis_->open_loop_controller_.phase_vel_ = NAN;
|
|
|
|
float max_current_ramp = axis_->motor_.config_.calibration_current / start_lock_duration * 2.0f;
|
|
axis_->open_loop_controller_.max_current_ramp_ = max_current_ramp;
|
|
axis_->open_loop_controller_.max_voltage_ramp_ = max_current_ramp;
|
|
axis_->open_loop_controller_.max_phase_vel_ramp_ = INFINITY;
|
|
axis_->open_loop_controller_.target_current_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? axis_->motor_.config_.calibration_current : 0.0f;
|
|
axis_->open_loop_controller_.target_voltage_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? 0.0f : axis_->motor_.config_.calibration_current;
|
|
axis_->open_loop_controller_.target_vel_ = 0.0f;
|
|
axis_->open_loop_controller_.total_distance_ = 0.0f;
|
|
|
|
axis_->motor_.current_control_.enable_current_control_src_ = (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL);
|
|
axis_->motor_.current_control_.Id_setpoint_src_ = &axis_->open_loop_controller_.Id_setpoint_;
|
|
axis_->motor_.current_control_.Iq_setpoint_src_ = &axis_->open_loop_controller_.Iq_setpoint_;
|
|
axis_->motor_.current_control_.Vd_setpoint_src_ = &axis_->open_loop_controller_.Vd_setpoint_;
|
|
axis_->motor_.current_control_.Vq_setpoint_src_ = &axis_->open_loop_controller_.Vq_setpoint_;
|
|
axis_->motor_.current_control_.phase_src_ =
|
|
axis_->async_estimator_.rotor_phase_src_ =
|
|
&axis_->open_loop_controller_.phase_;
|
|
axis_->motor_.phase_vel_src_ =
|
|
axis_->motor_.current_control_.phase_vel_src_ =
|
|
axis_->async_estimator_.rotor_phase_vel_src_ =
|
|
&axis_->open_loop_controller_.phase_vel_;
|
|
}
|
|
axis_->wait_for_control_iteration();
|
|
|
|
axis_->motor_.arm(&axis_->motor_.current_control_);
|
|
|
|
// go to motor zero phase for start_lock_duration to get ready to scan
|
|
for (size_t i = 0; i < (size_t)(start_lock_duration * 1000.0f); ++i) {
|
|
if (!axis_->motor_.is_armed_) {
|
|
return false; // TODO: return "disarmed" error code
|
|
}
|
|
if (axis_->requested_state_ != Axis::AXIS_STATE_UNDEFINED) {
|
|
axis_->motor_.disarm();
|
|
return false; // TODO: return "aborted" error code
|
|
}
|
|
osDelay(1);
|
|
}
|
|
|
|
|
|
int32_t init_enc_val = shadow_count_;
|
|
uint32_t num_steps = 0;
|
|
int64_t encvaluesum = 0;
|
|
|
|
CRITICAL_SECTION() {
|
|
axis_->open_loop_controller_.target_vel_ = config_.calib_scan_omega;
|
|
axis_->open_loop_controller_.total_distance_ = 0.0f;
|
|
}
|
|
|
|
// scan forward
|
|
while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) {
|
|
bool reached_target_dist = axis_->open_loop_controller_.total_distance_ >= config_.calib_scan_distance;
|
|
if (reached_target_dist) {
|
|
break;
|
|
}
|
|
encvaluesum += shadow_count_;
|
|
num_steps++;
|
|
osDelay(1);
|
|
}
|
|
|
|
// Check response and direction
|
|
if (shadow_count_ > init_enc_val + 8) {
|
|
// motor same dir as encoder
|
|
config_.direction = 1;
|
|
} else if (shadow_count_ < init_enc_val - 8) {
|
|
// motor opposite dir as encoder
|
|
config_.direction = -1;
|
|
} else {
|
|
// Encoder response error
|
|
set_error(ERROR_NO_RESPONSE);
|
|
axis_->motor_.disarm();
|
|
return false;
|
|
}
|
|
|
|
//TODO avoid recomputing elec_rad_per_enc every time
|
|
// Check CPR
|
|
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
|
|
float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
|
|
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
|
|
if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
|
|
set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
|
|
axis_->motor_.disarm();
|
|
return false;
|
|
}
|
|
|
|
CRITICAL_SECTION() {
|
|
axis_->open_loop_controller_.target_vel_ = -config_.calib_scan_omega;
|
|
}
|
|
|
|
// scan backwards
|
|
while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) {
|
|
bool reached_target_dist = axis_->open_loop_controller_.total_distance_ <= 0.0f;
|
|
if (reached_target_dist) {
|
|
break;
|
|
}
|
|
encvaluesum += shadow_count_;
|
|
num_steps++;
|
|
osDelay(1);
|
|
}
|
|
|
|
// Motor disarmed because of an error
|
|
if (!axis_->motor_.is_armed_) {
|
|
return false;
|
|
}
|
|
|
|
axis_->motor_.disarm();
|
|
|
|
config_.offset = encvaluesum / (num_steps * 2);
|
|
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
|
|
config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
|
|
|
|
is_ready_ = true;
|
|
return true;
|
|
}
|
|
|
|
static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
|
|
switch (hall_state) {
|
|
case 0b001: *hall_cnt = 0; return true;
|
|
case 0b011: *hall_cnt = 1; return true;
|
|
case 0b010: *hall_cnt = 2; return true;
|
|
case 0b110: *hall_cnt = 3; return true;
|
|
case 0b100: *hall_cnt = 4; return true;
|
|
case 0b101: *hall_cnt = 5; return true;
|
|
default: return false;
|
|
}
|
|
}
|
|
|
|
void Encoder::sample_now() {
|
|
switch (mode_) {
|
|
case MODE_INCREMENTAL: {
|
|
tim_cnt_sample_ = (int16_t)timer_->Instance->CNT;
|
|
} break;
|
|
|
|
case MODE_HALL: {
|
|
// do nothing: samples already captured in general GPIO capture
|
|
} break;
|
|
|
|
case MODE_SINCOS: {
|
|
sincos_sample_s_ = (get_adc_voltage(get_gpio(config_.sincos_gpio_pin_sin)) / 3.3f) - 0.5f;
|
|
sincos_sample_c_ = (get_adc_voltage(get_gpio(config_.sincos_gpio_pin_cos)) / 3.3f) - 0.5f;
|
|
} break;
|
|
|
|
case MODE_SPI_ABS_AMS:
|
|
case MODE_SPI_ABS_CUI:
|
|
case MODE_SPI_ABS_AEAT:
|
|
case MODE_SPI_ABS_RLS:
|
|
{
|
|
abs_spi_start_transaction();
|
|
// Do nothing
|
|
} break;
|
|
|
|
default: {
|
|
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
|
|
} break;
|
|
}
|
|
|
|
for (size_t i = 0; i < sizeof(ports_to_sample) / sizeof(ports_to_sample[0]); ++i) {
|
|
port_samples_[i] = ports_to_sample[i]->IDR;
|
|
}
|
|
}
|
|
|
|
bool Encoder::read_sampled_gpio(Stm32Gpio gpio) {
|
|
for (size_t i = 0; i < sizeof(ports_to_sample) / sizeof(ports_to_sample[0]); ++i) {
|
|
if (ports_to_sample[i] == gpio.port_) {
|
|
return port_samples_[i] & gpio.pin_mask_;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void Encoder::decode_hall_samples() {
|
|
hall_state_ = (read_sampled_gpio(hallA_gpio_) ? 1 : 0)
|
|
| (read_sampled_gpio(hallB_gpio_) ? 2 : 0)
|
|
| (read_sampled_gpio(hallC_gpio_) ? 4 : 0);
|
|
}
|
|
|
|
bool Encoder::abs_spi_start_transaction() {
|
|
if (mode_ & MODE_FLAG_ABS){
|
|
if (Stm32SpiArbiter::acquire_task(&spi_task_)) {
|
|
spi_task_.ncs_gpio = abs_spi_cs_gpio_;
|
|
spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_;
|
|
spi_task_.rx_buf = (uint8_t*)abs_spi_dma_rx_;
|
|
spi_task_.length = 1;
|
|
spi_task_.on_complete = [](void* ctx, bool success) { ((Encoder*)ctx)->abs_spi_cb(success); };
|
|
spi_task_.on_complete_ctx = this;
|
|
spi_task_.next = nullptr;
|
|
|
|
spi_arbiter_->transfer_async(&spi_task_);
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
uint8_t ams_parity(uint16_t v) {
|
|
v ^= v >> 8;
|
|
v ^= v >> 4;
|
|
v ^= v >> 2;
|
|
v ^= v >> 1;
|
|
return v & 1;
|
|
}
|
|
|
|
uint8_t cui_parity(uint16_t v) {
|
|
v ^= v >> 8;
|
|
v ^= v >> 4;
|
|
v ^= v >> 2;
|
|
return ~v & 3;
|
|
}
|
|
|
|
void Encoder::abs_spi_cb(bool success) {
|
|
uint16_t pos;
|
|
|
|
if (!success) {
|
|
goto done;
|
|
}
|
|
|
|
switch (mode_) {
|
|
case MODE_SPI_ABS_AMS: {
|
|
uint16_t rawVal = abs_spi_dma_rx_[0];
|
|
// check if parity is correct (even) and error flag clear
|
|
if (ams_parity(rawVal) || ((rawVal >> 14) & 1)) {
|
|
goto done;
|
|
}
|
|
pos = rawVal & 0x3fff;
|
|
} break;
|
|
|
|
case MODE_SPI_ABS_CUI: {
|
|
uint16_t rawVal = abs_spi_dma_rx_[0];
|
|
// check if parity is correct
|
|
if (cui_parity(rawVal)) {
|
|
goto done;
|
|
}
|
|
pos = rawVal & 0x3fff;
|
|
} break;
|
|
|
|
case MODE_SPI_ABS_RLS: {
|
|
uint16_t rawVal = abs_spi_dma_rx_[0];
|
|
pos = (rawVal >> 2) & 0x3fff;
|
|
} break;
|
|
|
|
default: {
|
|
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
|
|
goto done;
|
|
} break;
|
|
}
|
|
|
|
pos_abs_ = pos;
|
|
abs_spi_pos_updated_ = true;
|
|
if (config_.pre_calibrated) {
|
|
is_ready_ = true;
|
|
}
|
|
|
|
done:
|
|
Stm32SpiArbiter::release_task(&spi_task_);
|
|
}
|
|
|
|
void Encoder::abs_spi_cs_pin_init(){
|
|
// Decode and init cs pin
|
|
abs_spi_cs_gpio_ = get_gpio(config_.abs_spi_cs_gpio_pin);
|
|
abs_spi_cs_gpio_.config(GPIO_MODE_OUTPUT_PP, GPIO_PULLUP);
|
|
|
|
// Write pin high
|
|
abs_spi_cs_gpio_.write(true);
|
|
}
|
|
|
|
bool Encoder::update() {
|
|
// update internal encoder state.
|
|
int32_t delta_enc = 0;
|
|
int32_t pos_abs_latched = pos_abs_; //LATCH
|
|
|
|
switch (mode_) {
|
|
case MODE_INCREMENTAL: {
|
|
//TODO: use count_in_cpr_ instead as shadow_count_ can overflow
|
|
//or use 64 bit
|
|
int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;
|
|
delta_enc = (int32_t)delta_enc_16; //sign extend
|
|
} break;
|
|
|
|
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);
|
|
pos_estimate_ = NAN;
|
|
vel_estimate_ = NAN;
|
|
phase_ = NAN;
|
|
phase_vel_ = NAN;
|
|
return false;
|
|
}
|
|
}
|
|
} break;
|
|
|
|
case MODE_SINCOS: {
|
|
float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_);
|
|
int fake_count = (int)(1000.0f * phase);
|
|
//CPR = 6283 = 2pi * 1k
|
|
|
|
delta_enc = fake_count - count_in_cpr_;
|
|
delta_enc = mod(delta_enc, 6283);
|
|
if (delta_enc > 6283/2)
|
|
delta_enc -= 6283;
|
|
} break;
|
|
|
|
case MODE_SPI_ABS_RLS:
|
|
case MODE_SPI_ABS_AMS:
|
|
case MODE_SPI_ABS_CUI:
|
|
case MODE_SPI_ABS_AEAT: {
|
|
if (abs_spi_pos_updated_ == false) {
|
|
// Low pass filter the error
|
|
spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
|
|
if (spi_error_rate_ > 0.005f) {
|
|
set_error(ERROR_ABS_SPI_COM_FAIL);
|
|
pos_estimate_ = NAN;
|
|
vel_estimate_ = NAN;
|
|
phase_ = NAN;
|
|
phase_vel_ = NAN;
|
|
return false;
|
|
}
|
|
} else {
|
|
// Low pass filter the error
|
|
spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
|
|
}
|
|
|
|
abs_spi_pos_updated_ = false;
|
|
delta_enc = pos_abs_latched - count_in_cpr_; //LATCH
|
|
delta_enc = mod(delta_enc, config_.cpr);
|
|
if (delta_enc > config_.cpr/2) {
|
|
delta_enc -= config_.cpr;
|
|
}
|
|
|
|
}break;
|
|
default: {
|
|
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
|
|
pos_estimate_ = NAN;
|
|
vel_estimate_ = NAN;
|
|
phase_ = NAN;
|
|
phase_vel_ = NAN;
|
|
return false;
|
|
} break;
|
|
}
|
|
|
|
shadow_count_ += delta_enc;
|
|
count_in_cpr_ += delta_enc;
|
|
count_in_cpr_ = mod(count_in_cpr_, config_.cpr);
|
|
|
|
if(mode_ & MODE_FLAG_ABS)
|
|
count_in_cpr_ = pos_abs_latched;
|
|
|
|
// Memory for pos_circular
|
|
float pos_cpr_counts_last = pos_cpr_counts_;
|
|
|
|
//// run pll (for now pll is in units of encoder counts)
|
|
// Predict current pos
|
|
pos_estimate_counts_ += current_meas_period * vel_estimate_counts_;
|
|
pos_cpr_counts_ += current_meas_period * vel_estimate_counts_;
|
|
// 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_));
|
|
delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, 0.5f * (float)(config_.cpr));
|
|
// 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;
|
|
pos_cpr_counts_ = fmodf_pos(pos_cpr_counts_, (float)(config_.cpr));
|
|
vel_estimate_counts_ += current_meas_period * pll_ki_ * delta_pos_cpr_counts;
|
|
bool snap_to_zero_vel = false;
|
|
if (std::abs(vel_estimate_counts_) < 0.5f * current_meas_period * pll_ki_) {
|
|
vel_estimate_counts_ = 0.0f; //align delta-sigma on zero to prevent jitter
|
|
snap_to_zero_vel = true;
|
|
}
|
|
|
|
// Outputs from Encoder for Controller
|
|
pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
|
|
vel_estimate_ = vel_estimate_counts_ / (float)config_.cpr;
|
|
pos_circular_ += wrap_pm((pos_cpr_counts_ - pos_cpr_counts_last) / (float)config_.cpr, 0.5f);
|
|
pos_circular_ = fmodf_pos(pos_circular_, axis_->controller_.config_.circular_setpoint_range);
|
|
|
|
//// run encoder count interpolation
|
|
int32_t corrected_enc = count_in_cpr_ - config_.offset;
|
|
// if we are stopped, make sure we don't randomly drift
|
|
if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
|
|
interpolation_ = 0.5f;
|
|
// reset interpolation if encoder edge comes
|
|
// TODO: This isn't correct. At high velocities the first phase in this count may very well not be at the edge.
|
|
} else if (delta_enc > 0) {
|
|
interpolation_ = 0.0f;
|
|
} else if (delta_enc < 0) {
|
|
interpolation_ = 1.0f;
|
|
} else {
|
|
// Interpolate (predict) between encoder counts using vel_estimate,
|
|
interpolation_ += current_meas_period * vel_estimate_counts_;
|
|
// don't allow interpolation indicated position outside of [enc, enc+1)
|
|
if (interpolation_ > 1.0f) interpolation_ = 1.0f;
|
|
if (interpolation_ < 0.0f) interpolation_ = 0.0f;
|
|
}
|
|
float interpolated_enc = corrected_enc + interpolation_;
|
|
|
|
//// compute electrical phase
|
|
//TODO avoid recomputing elec_rad_per_enc every time
|
|
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
|
|
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
|
|
// ph = fmodf(ph, 2*M_PI);
|
|
if (is_ready_) {
|
|
phase_ = wrap_pm_pi(ph) * config_.direction;
|
|
phase_vel_ = (2*M_PI) * vel_estimate_ * axis_->motor_.config_.pole_pairs * config_.direction;
|
|
} else {
|
|
phase_ = NAN;
|
|
phase_vel_ = NAN;
|
|
}
|
|
|
|
return true;
|
|
}
|