飞控算法从入门到精通 · 109 · 代码优化：定点数运算与浮点数运算
从一次炸机说起
去年夏天，我在调试一款自研的四旋翼飞控，MCU是STM32F405，主频168MHz。姿态解算用的Mahony互补滤波，全部float运算，跑在FreeRTOS上，任务周期1kHz。地面站看着一切正常，角度响应平滑，PID输出也稳。结果一上天，悬停不到三秒就开始抖，越抖越厉害，最后直接翻了过去。还好是在草地上试的，桨叶断了两根，机架没事。
查了一整天，最后定位到问题：浮点运算导致的任务超时。1kHz的周期，实际执行时间在中断里被拉长到了1.3ms左右，偶尔还会跳到1.5ms。姿态环和位置环都在同一个高优先级任务里，浮点运算的延迟让控制输出滞后了将近半个周期，系统自然就发散。
从那以后，我对飞控里的浮点数运算就格外敏感。不是说不能用，而是要知道什么时候该用，什么时候必须换成定点数。
浮点数的代价
很多人觉得STM32F4有FPU，浮点运算就是一条指令的事，快得很。这话对了一半。单精度浮点乘法确实是一条指令，但问题出在别的地方。
首先是寄存器上下文切换。飞控里跑RTOS，任务切换时要保存和恢复FPU寄存器。Cortex-M4的FPU有32个单精度寄存器，每个32位，加上FPSCR状态寄存器，一次切换就要保存132字节。如果任务切换频繁，这个开销会累积得很可观。我实测过，在FreeRTOS里开启FPU的LAZY STACKING和关闭它，任务切换时间差了将近40个时钟周期。40个周期看起来不多，但1kHz的任务切换，每秒就是4万周期，占掉168MHz主频的0.0
