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参数时，看着波形从震荡到稳定，比看数据爽多了。