基于STM32硬件浮点运算芯片的6us一轮控制算法，全手写代码与注释讲解：有感FOC速度环、电...

6us一轮，基于Stm32硬件浮点运算芯片 ，代码全手写，每一句代码都有注释 讲解代码流程，包含有感foc 速度环、电流环，位置环 注释多

最近用STM32F4搞了个有感FOC驱动器，手搓代码的过程简直酸爽。这货带硬件浮点单元（FPU），算Clarke变换的时候直接飙到6us一轮，实测比软件浮点快三倍不止。上点硬核代码，带你们看看怎么把数学公式拍进寄存器里。

先整电机角度捕获，霍尔信号处理这关必须过：

//霍尔信号边沿中断服务函数 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){ static uint8_t last_hall = 0; if(GPIO_Pin == HALL_U_Pin | HALL_V_Pin | HALL_W_Pin){ //任意霍尔引脚触发 uint8_t hall_state = (HALL_U_READ() << 2) | (HALL_V_READ() << 1) | HALL_W_READ(); int8_t hall_delta = hall_steps[last_hall][hall_state]; //查表得机械角度变化量 motor.mech_angle += hall_delta * MECH_ANGLE_PER_STEP; //0.523rad/步 last_hall = hall_state; TIM1->CNT = 0; //重置转速计时器 } }

这段代码用查表法把霍尔信号转换成机械角度，注意那个MECHANGLEPER_STEP是根据极对数算出来的。当电机转得飞起时，这个中断每1.6ms就要冲进来一次，所以函数里连个除法都不敢放，全是移位和查表操作。

电流环才是FOC的核心战斗力，看这段ADC中断服务程序：

void ADC_IRQHandler(void){ static float id_target = 0, iq_target = 0; // Clarke变换 float i_alpha = adc_val_u - 0.5f*adc_val_v - 0.5f*adc_val_w; float i_beta = _SQRT3_2 * (adc_val_v - adc_val_w); // Park变换 float sin_theta = arm_sin_f32(e_angle); float cos_theta = arm_cos_f32(e_angle); float id = i_alpha * cos_theta + i_beta * sin_theta; float iq = i_beta * cos_theta - i_alpha * sin_theta; // PI控制器 id_target = pid_run(&pid_id, id_target - id); iq_target = pid_run(&pid_iq, iq_target - iq); // 逆Park变换 float v_alpha = id_target * cos_theta - iq_target * sin_theta; float v_beta = id_target * sin_theta + iq_target * cos_theta; // SVPWM调制 svpwm_generate(v_alpha, v_beta); }

这里用ARM的DSP库加速三角函数运算，实测单次变换只要28个时钟周期。注意那个SQRT32是预计算的√3/2，避免实时计算耗时间。PID控制器自己手写的增量式，比位置式的少两次浮点运算。

速度环和位置环在后台循环里跑：

while(1){ // 速度计算：每转60步，用定时器计数间隔推算 float speed = (M_PI * 60) / (TIM1->CNT * POLE_PAIRS * 1e-6); // 位置环外环 if(mode == POSITION_MODE){ target_speed = pid_run(&pid_pos, target_position - motor.position); } // 速度环中环 target_iq = pid_run(&pid_spd, target_speed - speed); // 电流环内环已在ADC中断处理 __WFI(); //等下次中断唤醒 }

这里有个坑：速度计算用定时器CNT值的时候要注意溢出处理，我用了32位累加计数器。位置环的pidpos参数要调得很软，否则容易过冲。那个_WFI()让CPU休眠省电，实测整机空载功耗从120mA降到70mA。

最后说下调试骚操作：在GPIO上拉个PWM当示波器用，抓关键变量波形：

// 用TIM3_CH1输出iq电流值波形 void debug_plot(float value){ static uint16_t val = 0; val = (uint16_t)((value + 20) * 4095 / 40); //-20A~+20A映射到0-3.3V TIM3->CCR1 = val; }

接上示波器看这个引脚，比用JScope什么的直接多了。调PI参数时，看着波形从震荡到稳定，比看数据爽多了。