From a86fb3e59b2df134b9ece10717ce100f40c9ba61 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 1 Feb 2019 21:01:45 -0800 Subject: [PATCH] add coherent sampling and 1.5 cycle pwm phase advance --- Firmware/MotorControl/axis.cpp | 9 ++++---- Firmware/MotorControl/encoder.cpp | 28 +++++++++++++++++++----- Firmware/MotorControl/encoder.hpp | 4 ++++ Firmware/MotorControl/low_level.cpp | 21 ++++++++++++++---- Firmware/MotorControl/motor.cpp | 34 +++++++++++++++++------------ Firmware/MotorControl/motor.hpp | 12 +++++++--- tools/odrive/utils.py | 1 + 7 files changed, 79 insertions(+), 30 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 5b35d938..8be43d15 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -148,7 +148,7 @@ bool Axis::run_sensorless_spin_up() { float phase = wrap_pm_pi(config_.ramp_up_distance * x); float I_mag = config_.spin_up_current * x; x += current_meas_period / config_.ramp_up_time; - if (!motor_.update(I_mag, phase)) + if (!motor_.update(I_mag, phase, 0.0f)) return error_ |= ERROR_MOTOR_FAILED, false; return x < 1.0f; }); @@ -162,7 +162,7 @@ bool Axis::run_sensorless_spin_up() { vel += config_.spin_up_acceleration * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); float I_mag = config_.spin_up_current; - if (!motor_.update(I_mag, phase)) + if (!motor_.update(I_mag, phase, vel)) return error_ |= ERROR_MOTOR_FAILED, false; return vel < config_.spin_up_target_vel; }); @@ -184,7 +184,7 @@ bool Axis::run_sensorless_control_loop() { float current_setpoint; if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, ¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; - if (!motor_.update(current_setpoint, sensorless_estimator_.phase_)) + if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_)) return false; // set_error should update axis.error_ return true; }); @@ -200,7 +200,8 @@ bool Axis::run_closed_loop_control_loop() { float current_setpoint; if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error - if (!motor_.update(current_setpoint, encoder_.phase_)) + float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; + if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel)) return false; // set_error should update axis.error_ return true; }); diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 9b3acdec..6a04dd4f 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -252,6 +252,27 @@ void Encoder::update_pll_gains() { } } +void Encoder::sample_now() { + switch (config_.mode) { + case MODE_INCREMENTAL: { + tim_cnt_sample_ = (int16_t)hw_config_.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(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f; + sincos_sample_c_ = get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f; + } break; + + default: { + set_error(ERROR_UNSUPPORTED_ENCODER_MODE); + } break; + } +} + bool Encoder::update() { // update internal encoder state. int32_t delta_enc = 0; @@ -259,7 +280,7 @@ bool Encoder::update() { 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)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_; + int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_; delta_enc = (int32_t)delta_enc_16; //sign extend } break; @@ -279,10 +300,7 @@ bool Encoder::update() { } break; case MODE_SINCOS: { - float c = get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f; - float s = get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f; - - float phase = fast_atan2(s, c); + float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_); int fake_count = (int)(1000.0f * phase); //CPR = 6283 = 2pi * 1k diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index d8d279b1..1cda9800 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -57,6 +57,7 @@ public: bool run_index_search(); bool run_offset_calibration(); + void sample_now(); bool update(); void update_pll_gains(); @@ -78,8 +79,11 @@ public: float pll_kp_ = 0.0f; // [count/s / count] float pll_ki_ = 0.0f; // [(count/s^2) / count] + int16_t tim_cnt_sample_ = 0; // // Updated by low_level pwm_adc_cb uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC + float sincos_sample_s_ = 0.0f; + float sincos_sample_c_ = 0.0f; // Communication protocol definitions auto make_protocol_definitions() { diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index bf35c7a3..6125c99c 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -539,6 +539,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC; } // Prepare hall readings + // TODO move this to inside encoder update function decode_hall_samples(axis.encoder_, GPIO_port_samples[axis_num]); // Trigger axis thread axis.signal_current_meas(); @@ -553,18 +554,30 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } void tim_update_cb(TIM_HandleTypeDef* htim) { - int portsamples_arr; + + // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current + // If we are counting down, we just sampled in SVM vector 7, with zero current + bool counting_down = htim->Instance->CR1 & TIM_CR1_DIR; + if (counting_down) + return; + + int sample_ch; + Axis* axis; if (htim == &htim1) { - portsamples_arr = 0; + sample_ch = 0; + axis = axes[0]; } else if (htim == &htim8) { - portsamples_arr = 1; + sample_ch = 1; + axis = axes[1]; } else { low_level_fault(Motor::ERROR_UNEXPECTED_TIMER_CALLBACK); return; } + axis->encoder_.sample_now(); + for (int i = 0; i < num_GPIO; ++i) { - GPIO_port_samples[portsamples_arr][i] = GPIOs_to_samp[i]->IDR; + GPIO_port_samples[sample_ch][i] = GPIOs_to_samp[i]->IDR; } } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 0a742e63..6fa01cc8 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -323,15 +323,15 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { } // We should probably make FOC Current call FOC Voltage to avoid duplication. -bool Motor::FOC_voltage(float v_d, float v_q, float phase) { - float c = our_arm_cos_f32(phase); - float s = our_arm_sin_f32(phase); +bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) { + float c = our_arm_cos_f32(pwm_phase); + float s = our_arm_sin_f32(pwm_phase); float v_alpha = c*v_d - s*v_q; float v_beta = c*v_q + s*v_d; return enqueue_voltage_timings(v_alpha, v_beta); } -bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { +bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase) { // Syntactic sugar CurrentControl_t& ictrl = current_control_; @@ -349,11 +349,12 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); // Park transform - float c = our_arm_cos_f32(phase); - float s = our_arm_sin_f32(phase); - float Id = c * Ialpha + s * Ibeta; - float Iq = c * Ibeta - s * Ialpha; - ictrl.Iq_measured = Iq; + float c_I = our_arm_cos_f32(I_phase); + float s_I = our_arm_sin_f32(I_phase); + float Id = c_I * Ialpha + s_I * Ibeta; + float Iq = c_I * Ibeta - s_I * Ialpha; + ictrl.Iq_measured += ictrl.I_measured_report_filter_k * (Iq - ictrl.Iq_measured); + ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured); // Current error float Ierr_d = Id_des - Id; @@ -387,8 +388,10 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { ictrl.Ibus = mod_d * Id + mod_q * Iq; // Inverse park transform - float mod_alpha = c * mod_d - s * mod_q; - float mod_beta = c * mod_q + s * mod_d; + float c_p = our_arm_cos_f32(pwm_phase); + float s_p = our_arm_sin_f32(pwm_phase); + float mod_alpha = c_p * mod_d - s_p * mod_q; + float mod_beta = c_p * mod_q + s_p * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl.final_v_alpha = mod_to_V * mod_alpha; @@ -403,19 +406,22 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { } -bool Motor::update(float current_setpoint, float phase) { +bool Motor::update(float current_setpoint, float phase, float phase_vel) { current_setpoint *= config_.direction; phase *= config_.direction; + phase_vel *= config_.direction; + + float pwm_phase = phase + 1.5f * current_meas_period * phase_vel; // Execute current command // TODO: move this into the mot if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if(!FOC_current(0.0f, current_setpoint, phase)){ + if(!FOC_current(0.0f, current_setpoint, phase, pwm_phase)){ return false; } } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) { //In gimbal motor mode, current is reinterptreted as voltage. - if(!FOC_voltage(0.0f, current_setpoint, phase)) + if(!FOC_voltage(0.0f, current_setpoint, pwm_phase)) return false; } else { set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 3f50f42c..9742835e 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -47,6 +47,8 @@ public: float final_v_beta; // [V] float Iq_setpoint; // [A] float Iq_measured; // [A] + float Id_measured; // [A] + float I_measured_report_filter_k; float max_allowed_current; // [A] float overcurrent_trip_level; // [A] }; @@ -119,9 +121,9 @@ public: bool run_calibration(); bool enqueue_modulation_timings(float mod_alpha, float mod_beta); bool enqueue_voltage_timings(float v_alpha, float v_beta); - bool FOC_voltage(float v_d, float v_q, float phase); - bool FOC_current(float Id_des, float Iq_des, float phase); - bool update(float current_setpoint, float phase); + bool FOC_voltage(float v_d, float v_q, float pwm_phase); + bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase); + bool update(float current_setpoint, float phase, float phase_vel); const MotorHardwareConfig_t& hw_config_; const GateDriverHardwareConfig_t gate_driver_config_; @@ -160,6 +162,8 @@ public: .final_v_beta = 0.0f, .Iq_setpoint = 0.0f, .Iq_measured = 0.0f, + .Id_measured = 0.0f, + .I_measured_report_filter_k = 1.0f, .max_allowed_current = 0.0f, .overcurrent_trip_level = 0.0f, }; @@ -190,6 +194,8 @@ public: make_protocol_property("final_v_beta", ¤t_control_.final_v_beta), make_protocol_property("Iq_setpoint", ¤t_control_.Iq_setpoint), make_protocol_property("Iq_measured", ¤t_control_.Iq_measured), + make_protocol_property("Id_measured", ¤t_control_.Id_measured), + make_protocol_property("I_measured_report_filter_k", ¤t_control_.I_measured_report_filter_k), make_protocol_ro_property("max_allowed_current", ¤t_control_.max_allowed_current), make_protocol_ro_property("overcurrent_trip_level", ¤t_control_.overcurrent_trip_level) ), diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 25a1cfb5..28d1d737 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -106,6 +106,7 @@ def start_liveplotter(get_var_callback): while not cancellation_token.is_set(): plt.clf() plt.plot(vals) + plt.legend(list(range(len(vals)))) fig.canvas.draw() fig.canvas.start_event_loop(1/plot_rate)