Permalink
Browse files

add coherent sampling and 1.5 cycle pwm phase advance

  • Loading branch information...
madcowswe committed Feb 2, 2019
1 parent 7384f2a commit a86fb3e59b2df134b9ece10717ce100f40c9ba61
@@ -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_, &current_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_, &current_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;
});
@@ -252,14 +252,35 @@ 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;
switch (config_.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)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

@@ -57,6 +57,7 @@ class Encoder {

bool run_index_search();
bool run_offset_calibration();
void sample_now();
bool update();

void update_pll_gains();
@@ -78,8 +79,11 @@ class Encoder {
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() {
@@ -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;
}
}

@@ -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);
@@ -47,6 +47,8 @@ class Motor {
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 @@ class Motor {
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 @@ class Motor {
.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 @@ class Motor {
make_protocol_property("final_v_beta", &current_control_.final_v_beta),
make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
make_protocol_property("Iq_measured", &current_control_.Iq_measured),
make_protocol_property("Id_measured", &current_control_.Id_measured),
make_protocol_property("I_measured_report_filter_k", &current_control_.I_measured_report_filter_k),
make_protocol_ro_property("max_allowed_current", &current_control_.max_allowed_current),
make_protocol_ro_property("overcurrent_trip_level", &current_control_.overcurrent_trip_level)
),
@@ -106,6 +106,7 @@ def did_close(evt):
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)

0 comments on commit a86fb3e

Please sign in to comment.