diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 987d3425..4b8cc798 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -3,22 +3,22 @@ // Because of broken cmsis_os.h, we need to include arm_math first, // otherwise chip specific defines are ommited #include -#include // Sets up the correct chip specifc defines required by arm_math +#include // Sets up the correct chip specifc defines required by arm_math #define ARM_MATH_CM4 #include #include +#include +#include #include #include -#include -#include -#include -#include #include -#include +#include +#include #include +#include #include /* Private defines -----------------------------------------------------------*/ @@ -34,39 +34,40 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (600*4) +#define ENCODER_CPR (600 * 4) #define POLE_PAIRS 7 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); // TODO: Migrate to C++, clearly we are actually doing object oriented code here... // TODO: For nice encapsulation, consider not having the motor objects public Motor_t motors[] = { - { // M0 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration + { + // M0 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, // .vel_setpoint = 800.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, .thread_ready = false, // .enable_control = true, // .do_calibration = true, // .calibration_ok = false, .motor_timer = &htim1, - .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, + .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, .control_deadline = TIM_1_8_PERIOD_CLOCKS, .last_cpu_time = 0, .current_meas = {0.0f, 0.0f}, @@ -82,13 +83,13 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .shunt_conductance = 1.0f / 0.0005f, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, .Ibus = 0.0f, @@ -100,31 +101,28 @@ Motor_t motors[] = { // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { - .encoder_timer = &htim3, - .encoder_cpr = ENCODER_CPR, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] + .encoder_timer = &htim3, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, + .motor_dir = 0, // set by calib_enc_offset + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] }, .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] }, .timing_log_index = 0, .timing_log = {0}, @@ -137,30 +135,30 @@ Motor_t motors[] = { .calib_vel_threshold = 1.0f, }, }, - { // M1 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration + { // M1 + .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t + .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] + .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, - .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance + .vel_gain = 15.0f / 10000.0f, // [A/(counts/s)] + .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] + .vel_integrator_current = 0.0f, // [A] + .vel_limit = 20000.0f, // [counts/s] + .current_setpoint = 0.0f, // [A] + .calibration_current = 10.0f, // [A] + .phase_inductance = 0.0f, // to be set by measure_phase_inductance + .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, .thread_ready = false, // .enable_control = true, // .do_calibration = true, // .calibration_ok = false, .motor_timer = &htim8, - .next_timings = {TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2, TIM_1_8_PERIOD_CLOCKS/2}, - .control_deadline = (3*TIM_1_8_PERIOD_CLOCKS)/2, + .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, + .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, .last_cpu_time = 0, .current_meas = {0.0f, 0.0f}, .DC_calib = {0.0f, 0.0f}, @@ -175,13 +173,13 @@ Motor_t motors[] = { .enableTimeOut = false, }, // .gate_driver_regs Init by DRV8301_setup - .shunt_conductance = 1.0f/0.0005f, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup + .shunt_conductance = 1.0f / 0.0005f, //[S] + .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup .current_control = { // .current_lim = 75.0f, //[A] // If setting higher than 75A, you MUST change DRV8301_ShuntAmpGain. TODO: make this automatic - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .current_lim = 10.0f, //[A] + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, .Ibus = 0.0f, @@ -191,31 +189,28 @@ Motor_t motors[] = { }, .rotor_mode = ROTOR_MODE_ENCODER, .encoder = { - .encoder_timer = &htim4, - .encoder_cpr = ENCODER_CPR, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 0, // set by calib_enc_offset - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] + .encoder_timer = &htim4, .encoder_cpr = ENCODER_CPR, .encoder_offset = 0, .encoder_state = 0, + .motor_dir = 0, // set by calib_enc_offset + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] }, .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } + .phase = 0.0f, // [rad] + .pll_pos = 0.0f, // [rad] + .pll_vel = 0.0f, // [rad/s] + .pll_kp = 0.0f, // [rad/s / rad] + .pll_ki = 0.0f, // [(rad/s^2) / rad] + .observer_gain = 1000.0f, // [rad/s] + .flux_state = {0.0f, 0.0f}, // [Vs] + .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] + .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] + .spin_up_current = 10.0f, // [A] + .spin_up_acceleration = 400.0f, // [rad/s^2] + .spin_up_target_vel = 400.0f, // [rad/s] }, .timing_log_index = 0, .timing_log = {0}, @@ -238,7 +233,7 @@ static const float current_meas_period = CURRENT_MEAS_PERIOD; static const int current_meas_hz = CURRENT_MEAS_HZ; /* Private variables ---------------------------------------------------------*/ -static float brake_resistance = 0.47f; // [ohm] +static float brake_resistance = 0.47f; // [ohm] /* Function implementations --------------------------------------------------*/ @@ -286,7 +281,7 @@ uint16_t check_timing(Motor_t* motor) { timing = TIM_1_8_PERIOD_CLOCKS + delta; } - if(++(motor->timing_log_index) == TIMING_LOG_SIZE){ + if (++(motor->timing_log_index) == TIMING_LOG_SIZE) { motor->timing_log_index = 0; } motor->timing_log[motor->timing_log_index] = timing; @@ -294,7 +289,7 @@ uint16_t check_timing(Motor_t* motor) { return timing; } -void global_fault(int error){ +void global_fault(int error) { // Disable motors NOW! for (int i = 0; i < num_motors; ++i) { __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer); @@ -309,14 +304,13 @@ void global_fault(int error){ } float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { - int adcval_bal = (int)ADCValue - (1<<11); - float amp_out_volt = (3.3f/(float)(1<<12)) * (float)adcval_bal; + int adcval_bal = (int)ADCValue - (1 << 11); + float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; float shunt_volt = amp_out_volt * motor->phase_current_rev_gain; float current = shunt_volt * motor->shunt_conductance; return current; } - //-------------------------------- // Initalisation //-------------------------------- @@ -341,42 +335,42 @@ void init_motor_control() { // Set up the gate drivers void DRV8301_setup(Motor_t* motor) { - DRV8301_Obj* gate_driver = &motor->gate_driver; - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; + DRV8301_Obj* gate_driver = &motor->gate_driver; + DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - DRV8301_enable(gate_driver); - DRV8301_setupSpi(gate_driver, local_regs); + DRV8301_enable(gate_driver); + DRV8301_setupSpi(gate_driver, local_regs); - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - // 20V/V on 500uOhm gives a range of +/- 150A - // 40V/V on 500uOhm gives a range of +/- 75A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // TODO we can use reporting only if we actually wire up the nOCTW pin + local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + // Overcurrent set to approximately 150A at 100degC. This may need tweaking. + local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + // 20V/V on 500uOhm gives a range of +/- 150A + // 40V/V on 500uOhm gives a range of +/- 75A + local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - motor->phase_current_rev_gain = 1.0f/10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - motor->phase_current_rev_gain = 1.0f/20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - motor->phase_current_rev_gain = 1.0f/40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - motor->phase_current_rev_gain = 1.0f/80.0f; - break; - } + switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: + motor->phase_current_rev_gain = 1.0f / 10.0f; + break; + case DRV8301_ShuntAmpGain_20VpV: + motor->phase_current_rev_gain = 1.0f / 20.0f; + break; + case DRV8301_ShuntAmpGain_40VpV: + motor->phase_current_rev_gain = 1.0f / 40.0f; + break; + case DRV8301_ShuntAmpGain_80VpV: + motor->phase_current_rev_gain = 1.0f / 80.0f; + break; + } - local_regs->SndCmd = true; - DRV8301_writeData(gate_driver, local_regs); - local_regs->RcvCmd = true; - DRV8301_readData(gate_driver, local_regs); + local_regs->SndCmd = true; + DRV8301_writeData(gate_driver, local_regs); + local_regs->RcvCmd = true; + DRV8301_readData(gate_driver, local_regs); } -void start_adc_pwm(){ +void start_adc_pwm() { // Enable ADC and interrupts __HAL_ADC_ENABLE(&hadc1); __HAL_ADC_ENABLE(&hadc2); @@ -396,7 +390,7 @@ void start_adc_pwm(){ start_pwm(&htim1); start_pwm(&htim8); // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS/2 - 1*128); + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); // Motor output starts in the disabled state __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); @@ -404,14 +398,14 @@ void start_adc_pwm(){ // Start brake resistor PWM in floating output configuration htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); } -void start_pwm(TIM_HandleTypeDef* htim){ +void start_pwm(TIM_HandleTypeDef* htim) { // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS/2; + int half_load = TIM_1_8_PERIOD_CLOCKS / 2; htim->Instance->CCR1 = half_load; htim->Instance->CCR2 = half_load; htim->Instance->CCR3 = half_load; @@ -429,8 +423,7 @@ void start_pwm(TIM_HandleTypeDef* htim){ } void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, - uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { - + uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { // Store intial timer configs uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); @@ -475,7 +468,6 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, htim_b->Instance->BDTR |= MOE_store_b; } - //-------------------------------- // IRQ Callbacks //-------------------------------- @@ -485,30 +477,30 @@ void step_cb(uint16_t GPIO_Pin) { GPIO_PinState dir_pin; float dir; switch (GPIO_Pin) { - case GPIO_1_Pin: - //M0 stepped - if (motors[0].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[0].pos_setpoint += dir * motors[0].counts_per_step; - } - break; - case GPIO_3_Pin: - //M1 stepped - if (motors[1].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[1].pos_setpoint += dir * motors[1].counts_per_step; - } - break; - default: - global_fault(ERROR_UNEXPECTED_STEP_SRC); - break; + case GPIO_1_Pin: + //M0 stepped + if (motors[0].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[0].pos_setpoint += dir * motors[0].counts_per_step; + } + break; + case GPIO_3_Pin: + //M1 stepped + if (motors[1].enable_step_dir) { + dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); + dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + motors[1].pos_setpoint += dir * motors[1].counts_per_step; + } + break; + default: + global_fault(ERROR_UNEXPECTED_STEP_SRC); + break; } } void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * 11.0f / (float)(1<<12); + static const float voltage_scale = 3.3f * 11.0f / (float)(1 << 12); // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; @@ -517,11 +509,11 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. // TODO: Document how the phasing is done, link to timing diagram void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - #define calib_tau 0.2f //@TOTO make more easily configurable +#define calib_tau 0.2f //@TOTO make more easily configurable static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; // Ensure ADCs are expected ones to simplify the logic below - if (!(hadc == &hadc2 || hadc == &hadc3)){ + if (!(hadc == &hadc2 || hadc == &hadc3)) { global_fault(ERROR_ADC_FAILED); return; }; @@ -532,7 +524,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // If we are counting down, we just sampled in SVM vector 7, with zero current Motor_t* motor = injected ? &motors[0] : &motors[1]; bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; - + bool current_meas_not_DC_CAL; if (motor == &motors[1] && counting_down) { // We are measuring M1 DC_CAL here @@ -610,19 +602,18 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } } - //-------------------------------- // Measurement and calibration //-------------------------------- // TODO check Ibeta balance to verify good motor connection bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; //[(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + static const float kI = 10.0f; //[(V/s)/A] + static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s float test_voltage = 0.0f; for (int i = 0; i < num_test_cycles; ++i) { osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); - if (evt.status != osEventSignal){ + if (evt.status != osEventSignal) { motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; return false; } @@ -636,7 +627,7 @@ bool measure_phase_resistance(Motor_t* motor, float test_current, float max_volt // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_PHASE_RESISTANCE_TIMING; return false; } @@ -672,7 +663,7 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_PHASE_INDUCTANCE_TIMING; return false; } @@ -687,7 +678,7 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // However, the discretisation in the current control loop inverts the same discrepancy float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); float L = v_L / dI_by_dt; - + motor->phase_inductance = L; // TODO arbitrary values set for now if (L < 1e-6f || L > 500e-6f) { @@ -702,15 +693,15 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float start_lock_duration = 1.0f; static const int num_steps = 1024; - static const float dt_step = 1.0f/500.0f; + static const float dt_step = 1.0f / 500.0f; static const float scan_range = 4.0f * M_PI; - const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) + const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; int32_t encvaluesum = 0; // go to encoder zero phase for start_lock_duration to get ready to scan - for (int i = 0; i < start_lock_duration*current_meas_hz; ++i) { + for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; @@ -719,13 +710,13 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } // scan forwards for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { - for (int i = 0; i < dt_step*(float)current_meas_hz; ++i) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; @@ -744,13 +735,13 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } // scan backwards for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { - for (int i = 0; i < dt_step*(float)current_meas_hz; ++i) { + for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; @@ -761,7 +752,7 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { return true; } -bool motor_calibration(Motor_t* motor){ +bool motor_calibration(Motor_t* motor) { motor->error = ERROR_NO_ERROR; // #warning(hardcoded values for SK3-5065-280kv!) @@ -777,18 +768,18 @@ bool motor_calibration(Motor_t* motor){ if (!calib_enc_offset(motor, motor->calibration_current * motor->phase_resistance)) return false; } - + // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] + float current_control_bandwidth = 1000.0f; // [rad/s] motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; float plant_pole = motor->phase_resistance / motor->phase_inductance; motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; // Calculate encoder pll gains - float encoder_pll_bandwidth = 1000.0f; // [rad/s] + float encoder_pll_bandwidth = 1000.0f; // [rad/s] motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)){ + if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { motor->error = ERROR_CALIBRATION_TIMING; return false; } @@ -812,8 +803,8 @@ bool motor_calibration(Motor_t* motor){ bool anti_cogging_calibration(Motor_t* motor) { if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { float pos_err = motor->anticogging.index - motor->encoder.pll_pos; - if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && - fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { + if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && + fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; } if (motor->anticogging.index < ENCODER_CPR) { @@ -834,18 +825,17 @@ bool anti_cogging_calibration(Motor_t* motor) { // Test functions //-------------------------------- -__attribute__((unused)) -void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { +__attribute__((unused)) void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { for (;;) { for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); + float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_SCAN_MOTOR_TIMING; return; } @@ -854,8 +844,7 @@ void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude) { } //TODO integrate as mode in main control loop -__attribute__((unused)) -void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { +__attribute__((unused)) void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { for (;;) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); update_rotor(motor); @@ -863,26 +852,24 @@ void FOC_voltage_loop(Motor_t* motor, float v_d, float v_q) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); - float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; + float v_alpha = c * v_d - s * v_q; + float v_beta = c * v_q + s * v_d; queue_voltage_timings(motor, v_alpha, v_beta); // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_FOC_VOLTAGE_TIMING; return; } } } - //-------------------------------- // Main motor control //-------------------------------- void update_rotor(Motor_t* motor) { - switch (motor->rotor_mode) { case ROTOR_MODE_ENCODER: case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { @@ -911,11 +898,10 @@ void update_rotor(Motor_t* motor) { encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; } - // Drop through to sensorless if also testing - if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - break; + // Drop through to sensorless if also testing + if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) + break; case ROTOR_MODE_SENSORLESS: { - // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf // In particular, equation 8 (and by extension eqn 4 and 6). @@ -930,8 +916,7 @@ void update_rotor(Motor_t* motor) { // Clarke transform float I_alpha_beta[2] = { -motor->current_meas.phB - motor->current_meas.phC, - one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC) - }; + one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; // alpha-beta vector operations float eta[2]; @@ -995,8 +980,8 @@ void update_rotor(Motor_t* motor) { } break; default: - //TODO error handling - break; + //TODO error handling + break; } } @@ -1016,9 +1001,9 @@ bool using_sensorless(Motor_t* motor) { } float get_rotor_phase(Motor_t* motor) { - if (using_encoder(motor)) + if (using_encoder(motor)) return motor->encoder.phase; - else if (using_sensorless(motor)) + else if (using_sensorless(motor)) return motor->sensorless.phase; else //TODO error handling @@ -1026,9 +1011,9 @@ float get_rotor_phase(Motor_t* motor) { } float get_pll_vel(Motor_t* motor) { - if (using_encoder(motor)) + if (using_encoder(motor)) return motor->encoder.pll_vel; - else if (using_sensorless(motor)) + else if (using_sensorless(motor)) return motor->sensorless.pll_vel; else //TODO error handling @@ -1052,7 +1037,6 @@ bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { } bool spin_up_sensorless(Motor_t* motor) { - static const float ramp_up_time = 0.4f; static const float ramp_up_distance = 4 * M_PI; float ramp_step = current_meas_period / ramp_up_time; @@ -1065,7 +1049,7 @@ bool spin_up_sensorless(Motor_t* motor) { for (float x = 0.0f; x < 1.0f; x += ramp_step) { phase = wrap_pm_pi(ramp_up_distance * x); I_mag = motor->sensorless.spin_up_current * x; - if(!spin_up_timestep(motor, phase, I_mag)) + if (!spin_up_timestep(motor, phase, I_mag)) return false; } @@ -1073,7 +1057,7 @@ bool spin_up_sensorless(Motor_t* motor) { while (vel < motor->sensorless.spin_up_target_vel) { vel += motor->sensorless.spin_up_acceleration * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) + if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) return false; } @@ -1103,7 +1087,7 @@ void update_brake_current(float brake_current) { // To avoid race condition, first reset timings to safe state // ch3 is low side, ch4 is high side htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS+1; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; htim2.Instance->CCR3 = low_off; htim2.Instance->CCR4 = high_on; } @@ -1134,8 +1118,8 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); float s = arm_sin_f32(phase); - float Id = c*Ialpha + s*Ibeta; - float Iq = c*Ibeta - s*Ialpha; + float Id = c * Ialpha + s * Ibeta; + float Iq = c * Ibeta - s * Ialpha; // Current error float Ierr_d = Id_des - Id; @@ -1153,9 +1137,8 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Vector modulation saturation, lock integrator if saturated // TODO make maximum modulation configurable - float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f/sqrtf(mod_d*mod_d + mod_q*mod_q); - if (mod_scalefactor < 1.0f) - { + float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); + if (mod_scalefactor < 1.0f) { mod_d *= mod_scalefactor; mod_q *= mod_scalefactor; // TODO make decayfactor configurable @@ -1174,17 +1157,17 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Above check doesn't work if last motor is executing voltage control // TODO trigger this update in control_motor_loop instead, // and make voltage control a control mode in it. - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: function will clip negative values to 0.0f - update_brake_current(-Ibus_sum); + float Ibus_sum = 0.0f; + for (int i = 0; i < num_motors; ++i) { + Ibus_sum += motors[i].current_control.Ibus; + } + // Note: function will clip negative values to 0.0f + update_brake_current(-Ibus_sum); // } // Inverse park transform - float mod_alpha = c*mod_d - s*mod_q; - float mod_beta = c*mod_q + s*mod_d; + float mod_alpha = c * mod_d - s * mod_q; + float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl->final_v_alpha = mod_to_V * mod_alpha; @@ -1195,7 +1178,7 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { // Check we meet deadlines after queueing motor->last_cpu_time = check_timing(motor); - if(!(motor->last_cpu_time < motor->control_deadline)){ + if (!(motor->last_cpu_time < motor->control_deadline)) { motor->error = ERROR_FOC_TIMING; return false; } @@ -1204,12 +1187,12 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { void control_motor_loop(Motor_t* motor) { while (*(motor->axis_legacy.enable_control)) { - if(osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal){ + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; break; } update_rotor(motor); - anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking + anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking // Position control // TODO Decide if we want to use encoder or pll position here @@ -1225,7 +1208,7 @@ void control_motor_loop(Motor_t* motor) { // Velocity limiting float vel_lim = motor->vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; + if (vel_des > vel_lim) vel_des = vel_lim; if (vel_des < -vel_lim) vel_des = -vel_lim; // Velocity control @@ -1234,12 +1217,12 @@ void control_motor_loop(Motor_t* motor) { // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == ENCODER_CPR - 1) - if(motor->anticogging.use_anticogging){ + if (motor->anticogging.use_anticogging) { Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, ENCODER_CPR)]; } float v_err = vel_des - get_pll_vel(motor); - if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { Iq += motor->vel_gain * v_err; } @@ -1265,7 +1248,7 @@ void control_motor_loop(Motor_t* motor) { } // Velocity integrator (behaviour dependent on limiting) - if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL ) { + if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { // reset integral if not in use motor->vel_integrator_current = 0.0f; } else { @@ -1279,8 +1262,8 @@ void control_motor_loop(Motor_t* motor) { motor->current_control.Iq = Iq; // Execute current command - if(!FOC_current(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_current + if (!FOC_current(motor, 0.0f, Iq)) { + break; // in case of error exit loop, motor->error has been set by FOC_current } }