|
|
@@ -3,7 +3,7 @@
|
|
|
#include "bsp/adc.h"
|
|
|
#include "foc_core.h"
|
|
|
#include "foc_api.h"
|
|
|
-#include "foc_stm.h"
|
|
|
+#include "foc_fsm.h"
|
|
|
#include "phase_current.h"
|
|
|
#include "park_clark.h"
|
|
|
#include "hall_sensor.h"
|
|
|
@@ -78,7 +78,7 @@ static void __inline foc_update_theta(motor_foc_t *foc) {
|
|
|
#endif
|
|
|
|
|
|
|
|
|
-static void __inline Foc_Calc_Voltage(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
|
|
|
+static void __inline foc_calc_voltage(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
|
|
|
//float vd = pi_control(&foc->PI_id, foc->dq_ref.Id - sampled->Id);
|
|
|
//float vq = pi_control(&foc->PI_iq, foc->dq_ref.Iq - sampled->Iq);
|
|
|
if (foc->mode == FOC_MODE_CURRENT_LOOP || foc->mode == FOC_MODE_CLOSE_LOOP) {
|
|
|
@@ -93,7 +93,7 @@ static void __inline Foc_Calc_Voltage(motor_foc_t *foc, dq_t *sampled, dq_t *ref
|
|
|
foc->dq_last.Vq = ref_out->Vq;
|
|
|
}
|
|
|
|
|
|
-static void __inline DeadTime_Compensation(current_samp_t *c_sample, phase_time_t *time) {
|
|
|
+static void __inline deadtime_compensation(current_samp_t *c_sample, phase_time_t *time) {
|
|
|
#if 0
|
|
|
/* Dead time compensation */
|
|
|
if ( c_sample->Ia > 0)
|
|
|
@@ -146,7 +146,7 @@ static void __inline Debug_dq(dq_t *dq){
|
|
|
}
|
|
|
|
|
|
/* FOC 主控制任务 */
|
|
|
-void FOC_Fast_Task(motor_foc_t *foc){
|
|
|
+void do_motor_foc(motor_foc_t *foc){
|
|
|
current_samp_t *c_sample = &foc->current_samp;
|
|
|
alpha_beta_t sample_ab, pwm_ab;
|
|
|
dq_t sample_dq, v_dq;
|
|
|
@@ -158,21 +158,21 @@ void FOC_Fast_Task(motor_foc_t *foc){
|
|
|
/* 采集相电流 */
|
|
|
phase_current_sample(c_sample);
|
|
|
/* ABC三相坐标到alpha-beta坐标 */
|
|
|
- Clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
|
|
|
+ do_clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
|
|
|
/* alpha-beta坐标系到D-Q旋转坐标系 */
|
|
|
- Park(&sample_ab, foc->motor_stat.theta, &sample_dq);
|
|
|
+ do_park(&sample_ab, foc->motor_stat.theta, &sample_dq);
|
|
|
/* 电流环,输出电压给SVPWM */
|
|
|
- Foc_Calc_Voltage(foc, &sample_dq, &v_dq);
|
|
|
+ foc_calc_voltage(foc, &sample_dq, &v_dq);
|
|
|
/* 确保电压在6个扇区的内切圆中 */
|
|
|
- CirCle_Limitation_Process(&v_dq, foc->vbus, 1.0f);
|
|
|
+ circle_limitation(&v_dq, foc->vbus, 1.0f);
|
|
|
/* d-q坐标系到alpha-beta坐标系,输出给svpwm */
|
|
|
Rev_Park(&v_dq, foc->motor_stat.theta, &pwm_ab);
|
|
|
/* SVPWM,获取三相逆变器的开关时间,用的是pwm1模式,如果是pwm2模式,这个函数需要修改 */
|
|
|
- SVM_Get_Phase_Time(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
|
|
|
+ svpwm_get_phase_time(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
|
|
|
/* 计算三相电流的采样点 */
|
|
|
sample_point = get_phase_sample_point(c_sample, &phase_time, foc->sector);
|
|
|
/* 死区补偿 */
|
|
|
- DeadTime_Compensation(c_sample, &phase_time);
|
|
|
+ deadtime_compensation(c_sample, &phase_time);
|
|
|
/* 更新 TIM1的CCR0-2,生成互补pwm, 相电流更新采样点 */
|
|
|
pwm_update_duty(phase_time.A, phase_time.B, phase_time.C, sample_point);
|
|
|
|
|
|
@@ -182,7 +182,7 @@ void FOC_Fast_Task(motor_foc_t *foc){
|
|
|
}
|
|
|
|
|
|
/* 计算电流环的参考输入 */
|
|
|
-void Foc_Calc_Current_Ref(motor_foc_t *foc) {
|
|
|
+void foc_calc_current(motor_foc_t *foc) {
|
|
|
float speed_ref = ramp_get_target(&foc->speed_ramp);
|
|
|
float speed_feedback = foc_get_speed();
|
|
|
float vq_out = pi_control(&foc->speed_controller, speed_ref - speed_feedback);
|
|
|
@@ -195,7 +195,7 @@ void Foc_Calc_Current_Ref(motor_foc_t *foc) {
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-void Foc_Speed_Ramp(motor_foc_t *foc){
|
|
|
+void foc_speed_ramp(motor_foc_t *foc){
|
|
|
if (foc->speed_command.speed >= 0 && foc->mode != FOC_MODE_OPEN_LOOP){
|
|
|
u16 current_rpm = foc_get_speed();
|
|
|
u16 ref_rpm = foc->speed_command.speed;
|
|
|
@@ -219,16 +219,14 @@ void foc_pwm_up_handler(void){
|
|
|
}
|
|
|
|
|
|
measure_time_t g_meas_foc = {.exec_max_time = 15,};
|
|
|
+/*ADC 电流采集中断,调用FOC的核心处理函数*/
|
|
|
void mc_phase_current_irq(void) {
|
|
|
if (g_foc.current_samp.is_calibrating_offset) {
|
|
|
phase_current_offset(&g_foc.current_samp);
|
|
|
return;
|
|
|
- }else if (adc_is_trigged_vbus()) {
|
|
|
- phase_Rds_calibrate(&g_foc.current_samp);
|
|
|
- return;
|
|
|
}
|
|
|
time_measure_start(&g_meas_foc);
|
|
|
- FOC_Fast_Task(&g_foc);
|
|
|
+ do_motor_foc(&g_foc);
|
|
|
time_measure_end(&g_meas_foc);
|
|
|
if (g_meas_foc.intval_time < 32 || g_meas_foc.intval_time > 34) {
|
|
|
//log_chan_value(1, g_meas_foc.intval_time);
|
|
|
@@ -247,8 +245,9 @@ void foc_pwm_start(bool start) {
|
|
|
g_foc.mosfec_gate = start;
|
|
|
}
|
|
|
|
|
|
+/*10ms 定时任务,主要处理foc状态机(里面包含速度环)*/
|
|
|
void foc_normal_task(void) {
|
|
|
- FOC_Normal_Task(&g_foc);
|
|
|
+ foc_fsm(&g_foc);
|
|
|
}
|
|
|
|
|
|
static void foc_measure_task(void *args){
|