void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { (void)p; (void)flags; TIM12->CNT = 0; bool is_v7 = !(TIM1->CR1 & TIM_CR1_DIR); if (!m_samples.measure_inductance_now) { #ifdef HW_HAS_PHASE_SHUNTS if (!m_conf->foc_sample_v0_v7 && is_v7) { return; } #else if (is_v7) { return; } #endif } WWDG_SetCounter(100); int curr0 = ADC_Value[ADC_IND_CURR1]; int curr1 = ADC_Value[ADC_IND_CURR2]; #ifdef HW_HAS_3_SHUNTS int curr2 = ADC_Value[ADC_IND_CURR3]; #endif m_curr0_sum += curr0; m_curr1_sum += curr1; #ifdef HW_HAS_3_SHUNTS m_curr2_sum += curr2; #endif curr0 -= m_curr0_offset; curr1 -= m_curr1_offset; #ifdef HW_HAS_3_SHUNTS curr2 -= m_curr2_offset; #endif m_curr_samples++; ADC_curr_norm_value[0] = curr0; ADC_curr_norm_value[1] = curr1; #ifdef HW_HAS_3_SHUNTS ADC_curr_norm_value[2] = curr2; #else ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]); #endif #ifdef HW_HAS_3_SHUNTS if (m_conf->foc_sample_high_current) { const float i0_abs = fabsf(ADC_curr_norm_value[0]); const float i1_abs = fabsf(ADC_curr_norm_value[1]); const float i2_abs = fabsf(ADC_curr_norm_value[2]); if (i0_abs > i1_abs && i0_abs > i2_abs) { ADC_curr_norm_value[0] = -(ADC_curr_norm_value[1] + ADC_curr_norm_value[2]); } else if (i1_abs > i0_abs && i1_abs > i2_abs) { ADC_curr_norm_value[1] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[2]); } else if (i2_abs > i0_abs && i2_abs > i1_abs) { ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]); } } else { #ifdef HW_HAS_PHASE_SHUNTS if (m_conf->foc_sample_v0_v7 && is_v7) { if (TIM1->CCR1 < TIM1->CCR2 && TIM1->CCR1 < TIM1->CCR3) { ADC_curr_norm_value[0] = -(ADC_curr_norm_value[1] + ADC_curr_norm_value[2]); } else if (TIM1->CCR2 < TIM1->CCR1 && TIM1->CCR2 < TIM1->CCR3) { ADC_curr_norm_value[1] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[2]); } else if (TIM1->CCR3 < TIM1->CCR1 && TIM1->CCR3 < TIM1->CCR2) { ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]); } } else { if (TIM1->CCR1 > TIM1->CCR2 && TIM1->CCR1 > TIM1->CCR3) { ADC_curr_norm_value[0] = -(ADC_curr_norm_value[1] + ADC_curr_norm_value[2]); } else if (TIM1->CCR2 > TIM1->CCR1 && TIM1->CCR2 > TIM1->CCR3) { ADC_curr_norm_value[1] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[2]); } else if (TIM1->CCR3 > TIM1->CCR1 && TIM1->CCR3 > TIM1->CCR2) { ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]); } } #else if (TIM1->CCR1 > TIM1->CCR2 && TIM1->CCR1 > TIM1->CCR3) { ADC_curr_norm_value[0] = -(ADC_curr_norm_value[1] + ADC_curr_norm_value[2]); } else if (TIM1->CCR2 > TIM1->CCR1 && TIM1->CCR2 > TIM1->CCR3) { ADC_curr_norm_value[1] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[2]); } else if (TIM1->CCR3 > TIM1->CCR1 && TIM1->CCR3 > TIM1->CCR2) { ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]); } #endif } #endif 0 when the motor is undriven m_motor_state.i_alpha = 0.0f; m_motor_state.i_beta = 0.0f; m_motor_state.id = 0.0f; m_motor_state.iq = 0.0f; m_motor_state.id_filter = 0.0f; m_motor_state.iq_filter = 0.0f; m_motor_state.i_bus = 0.0f; m_motor_state.i_abs = 0.0f; m_motor_state.i_abs_filter = 0.0f; // Run observer observer_update(m_motor_state.v_alpha, m_motor_state.v_beta, m_motor_state.i_alpha, m_motor_state.i_beta, dt, &m_observer_x1, &m_observer_x2, &m_phase_now_observer); switch (m_conf->foc_sensor_mode) { case FOC_SENSOR_MODE_ENCODER: float ia = ADC_curr_norm_value[0] * FAC_CURRENT; float ib = ADC_curr_norm_value[1] * FAC_CURRENT; if (m_samples.measure_inductance_now) { if (!is_v7) { return; } static int inductance_state = 0; const uint32_t duty_cnt = (uint32_t)((float)TIM1->ARR * m_samples.measure_inductance_duty); const uint32_t samp_time = duty_cnt - MCPWM_FOC_INDUCTANCE_SAMPLE_CNT_OFFSET; if (inductance_state == 0) { TIMER_UPDATE_DUTY_SAMP(0, 0, 0, samp_time); start_pwm_hw(); } else if (inductance_state == 2) { TIMER_UPDATE_DUTY(duty_cnt, 0, duty_cnt); } else if (inductance_state == 3) { m_samples.avg_current_tot += -((float)curr1 * FAC_CURRENT); m_samples.avg_voltage_tot += GET_INPUT_VOLTAGE(); m_samples.sample_num++; TIMER_UPDATE_DUTY(0, 0, 0); } else if (inductance_state == 5) { TIMER_UPDATE_DUTY(0, duty_cnt, duty_cnt); } else if (inductance_state == 6) { m_samples.avg_current_tot += -((float)curr0 * FAC_CURRENT); m_samples.avg_voltage_tot += GET_INPUT_VOLTAGE(); m_samples.sample_num++; TIMER_UPDATE_DUTY(0, 0, 0); } else if (inductance_state == 8) { #ifdef HW_HAS_3_SHUNTS TIMER_UPDATE_DUTY(duty_cnt, duty_cnt, 0); #else TIMER_UPDATE_DUTY(0, 0, duty_cnt); #endif } else if (inductance_state == 9) { #ifdef HW_HAS_3_SHUNTS m_samples.avg_current_tot += -((float)curr2 * FAC_CURRENT); #else m_samples.avg_current_tot += -((float)curr0 * FAC_CURRENT + (float)curr1 * FAC_CURRENT); #endif m_samples.avg_voltage_tot += GET_INPUT_VOLTAGE(); m_samples.sample_num++; stop_pwm_hw(); TIMER_UPDATE_SAMP(MCPWM_FOC_CURRENT_SAMP_OFFSET); } else if (inductance_state == 10) { inductance_state = 0; m_samples.measure_inductance_now = false; return; } inductance_state++; return; } #ifdef HW_HAS_PHASE_SHUNTS float dt; if (m_conf->foc_sample_v0_v7) { dt = 1.0f / m_conf->foc_f_sw; } else { dt = 1.0f / (m_conf->foc_f_sw / 2.0f); } #else const float dt = 1.0f / (m_conf->foc_f_sw / 2.0f); #endif UTILS_LP_FAST(m_motor_state.v_bus, GET_INPUT_VOLTAGE(), 0.1f); float enc_ang = 0; if (encoder_is_configured()) { enc_ang = encoder_read_deg(); float phase_tmp = enc_ang; if (m_conf->foc_encoder_inverted) { phase_tmp = 360.0f - phase_tmp; } phase_tmp *= m_conf->foc_encoder_ratio; phase_tmp -= m_conf->foc_encoder_offset; utils_norm_angle((float*)&phase_tmp); m_phase_now_encoder = phase_tmp * (M_PI / 180.0f); } static float phase_before = 0.0f; const float phase_diff = utils_angle_difference_rad(m_motor_state.phase, phase_before); phase_before = m_motor_state.phase; if (m_state == MC_STATE_RUNNING) { m_motor_state.i_alpha = ia; m_motor_state.i_beta = ONE_BY_SQRT3 * ia + TWO_BY_SQRT3 * ib; const float duty_abs = fabsf(m_motor_state.duty_now); float id_set_tmp = m_id_set; float iq_set_tmp = m_iq_set; m_motor_state.max_duty = m_conf->l_max_duty; static float duty_filtered = 0.0f; UTILS_LP_FAST(duty_filtered, m_motor_state.duty_now, 0.1f); utils_truncate_number(&duty_filtered, -1.0f, 1.0f); float duty_set = m_duty_cycle_set; bool control_duty = m_control_mode == CONTROL_MODE_DUTY; if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE && fabsf(duty_filtered) < 0.03f) { control_duty = true; duty_set = 0.0f; } if (m_control_mode == CONTROL_MODE_SPEED && fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) { control_duty = true; duty_set = 0.0f; } if (control_duty) { static float duty_i_term = 0.0f; if (fabsf(duty_set) < (duty_abs - 0.05f) || (SIGN(m_motor_state.vq) * m_motor_state.iq) < m_conf->lo_current_min) { float scale = 1.0f / GET_INPUT_VOLTAGE(); float error = duty_set - m_motor_state.duty_now; float p_term = error * m_conf->foc_duty_dowmramp_kp * scale; duty_i_term += error * (m_conf->foc_duty_dowmramp_ki * dt) * scale; utils_truncate_number(&duty_i_term, -1.0f, 1.0f); float output = p_term + duty_i_term; utils_truncate_number(&output, -1.0f, 1.0f); iq_set_tmp = output * m_conf->lo_current_max; } else { duty_i_term = 0.0f; m_motor_state.max_duty = duty_set; if (duty_set > 0.0f) { iq_set_tmp = m_conf->lo_current_max; } else { iq_set_tmp = -m_conf->lo_current_max; } } } else if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE) { iq_set_tmp = fabsf(iq_set_tmp); if (phase_diff > 0.0f) { iq_set_tmp = -iq_set_tmp; } else if (phase_diff == 0.0f) { iq_set_tmp = 0.0f; } } if (!m_phase_override) { observer_update(m_motor_state.v_alpha, m_motor_state.v_beta, m_motor_state.i_alpha, m_motor_state.i_beta, dt, &m_observer_x1, &m_observer_x2, &m_phase_now_observer); } switch (m_conf->foc_sensor_mode) { case FOC_SENSOR_MODE_ENCODER: if (encoder_index_found()) { m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed); } else { m_motor_state.phase = m_phase_now_encoder_no_index; } if (!m_phase_override) { id_set_tmp = 0.0f; } break; case FOC_SENSOR_MODE_HALL: m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt); m_motor_state.phase = m_phase_now_observer; if (!m_phase_override) { id_set_tmp = 0.0f; } break; case FOC_SENSOR_MODE_SENSORLESS: if (m_phase_observer_override) { m_motor_state.phase = m_phase_now_observer_override; } else { m_motor_state.phase = m_phase_now_observer; } if (!m_phase_override) { if (duty_abs < m_conf->foc_sl_d_current_duty) { id_set_tmp = utils_map(duty_abs, 0.0f, m_conf->foc_sl_d_current_duty, fabsf(m_motor_state.iq_target) * m_conf->foc_sl_d_current_factor, 0.0f); } else { id_set_tmp = 0.0f; } } break; } if (m_control_mode == CONTROL_MODE_HANDBRAKE) { m_motor_state.phase = 0.0f; } else if (m_control_mode == CONTROL_MODE_OPENLOOP) { static float openloop_angle = 0.0f; openloop_angle += dt * m_openloop_speed; utils_norm_angle_rad(&openloop_angle); m_motor_state.phase = openloop_angle; } if (m_phase_override) { m_motor_state.phase = m_phase_now_override; } const float mod_q = m_motor_state.mod_q; if (mod_q > 0.001f) { utils_truncate_number(&iq_set_tmp, m_conf->lo_in_current_min / mod_q, m_conf->lo_in_current_max / mod_q); } else if (mod_q < -0.001f) { utils_truncate_number(&iq_set_tmp, m_conf->lo_in_current_max / mod_q, m_conf->lo_in_current_min / mod_q); } if (mod_q > 0.0f) { utils_truncate_number(&iq_set_tmp, m_conf->lo_current_min, m_conf->lo_current_max); } else { utils_truncate_number(&iq_set_tmp, -m_conf->lo_current_max, -m_conf->lo_current_min); } utils_saturate_vector_2d(&id_set_tmp, &iq_set_tmp, utils_max_abs(m_conf->lo_current_max, m_conf->lo_current_min)); m_motor_state.id_target = id_set_tmp; m_motor_state.iq_target = iq_set_tmp; control_current(&m_motor_state, dt); } else { #ifdef HW_HAS_3_SHUNTS float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2); float Vb = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2); float Vc = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2); #else float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2); float Vb = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2); float Vc = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2); #endif m_motor_state.v_alpha = (2.0f / 3.0f) * Va - (1.0f / 3.0f) * Vb - (1.0f / 3.0f) * Vc; m_motor_state.v_beta = ONE_BY_SQRT3 * Vb - ONE_BY_SQRT3 * Vc; float c, s; utils_fast_sincos_better(m_motor_state.phase, &s, &c); float vd_tmp = c * m_motor_state.v_alpha + s * m_motor_state.v_beta; float vq_tmp = c * m_motor_state.v_beta - s * m_motor_state.v_alpha; UTILS_NAN_ZERO(m_motor_state.vd); UTILS_NAN_ZERO(m_motor_state.vq); UTILS_LP_FAST(m_motor_state.vd, vd_tmp, 0.2f); UTILS_LP_FAST(m_motor_state.vq, vq_tmp, 0.2f); m_motor_state.vd_int = m_motor_state.vd; m_motor_state.vq_int = m_motor_state.vq; m_motor_state.mod_d = m_motor_state.vd / ((2.0f / 3.0f) * m_motor_state.v_bus); m_motor_state.mod_q = m_motor_state.vq / ((2.0f / 3.0f) * m_motor_state.v_bus); break; case FOC_SENSOR_MODE_HALL: m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt); m_motor_state.phase = m_phase_now_observer; break; case FOC_SENSOR_MODE_SENSORLESS: m_motor_state.phase = m_phase_now_observer; break; } } m_motor_state.duty_now = SIGN(m_motor_state.vq) * sqrtf(m_motor_state.mod_d * m_motor_state.mod_d + m_motor_state.mod_q * m_motor_state.mod_q) / SQRT3_BY_2; pll_run(m_motor_state.phase, dt, &m_pll_phase, &m_pll_speed); float ph_tmp = m_motor_state.phase; utils_norm_angle_rad(&ph_tmp); int step = (int)floorf((ph_tmp + M_PI) / (2.0f * M_PI) * 6.0f); utils_truncate_number_int(&step, 0, 5); static int step_last = 0; int diff = step - step_last; step_last = step; if (diff > 3) { diff -= 6; } else if (diff < -2) { diff += 6; } m_tachometer += diff; m_tachometer_abs += abs(diff); float angle_now = 0.0f; if (encoder_is_configured()) { angle_now = enc_ang; } else { angle_now = m_motor_state.phase * (180.0f / M_PI); } if (m_conf->p_pid_ang_div > 0.98f && m_conf->p_pid_ang_div < 1.02f) { m_pos_pid_now = angle_now; } else { static float angle_last = 0.0f; float diff_f = utils_angle_difference(angle_now, angle_last); angle_last = angle_now; m_pos_pid_now += diff_f / m_conf->p_pid_ang_div; utils_norm_angle((float*)&m_pos_pid_now); } if (m_state == MC_STATE_RUNNING) { run_pid_control_pos(m_pos_pid_now, m_pos_pid_set, dt); } mc_interface_mc_timer_isr(); last_inj_adc_isr_duration = (float) TIM12->CNT / 10000000.0f; } 分析这段代码的作用，以文字的形式描述出来。随后生成工作流程说明。最右生成带每行注释的代码。