SimpleFOC源码学习04(v2.3.2) - 数学基础层foc_utils.cpp与foc_utils.h

导言

---

github源码：https://github.com/simplefoc/Arduino-FOC/blob/master/src/common/foc_utils.cpp

为什么需要foc_utils.cpp与foc_utils.h？
foc_utils.h/.cpp 是一个纯工具库，不含任何状态，不含任何类，只提供以下几类东西：

1. 宏定义 --- 常用数学常量 + 小工具宏
2. 数据结构 --- 描述电机信号的结构体
3. 近似数学函数 --- _sin, _cos, _atan2, _sqrt（全部为嵌入式优化版本）
4. 角度工具函数 --- _normalizeAngle, _electricalAngle

嵌入式 MCU（Arduino、STM32）的标准库 sin()/cos() 用的是浮点软件库 ，在 AVR 这类没有 FPU 的芯片上要消耗几百微秒，而 FOC 控制环需要在 50μs 内 完成所有计算。

另外，STM32G4、F4芯片建议用ARM-CMSIS的DSP库来重构foc_utils.cpp的代码，进一步提高精度与计算速度。

一、foc_util.h

---

foc_utils.h 的宏区域，用一张图整理结构：

二、foc_util.cpp

---

2.1、_sin() 的 LUT 近似算法（核心！）

这是整个文件最精妙的部分，代码值得逐行分析：

static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768};

为什么是 65 个元素? 它只存储了第一象限（0 到 π/2）的正弦值，然后通过对称性推导其他三个象限。32768 = 2^15，对应 sin(π/2) = 1.0，所以实际精度是 1/32768 ≈ 0.000030。

unsigned int i = (unsigned int)(a * (64 * 4 * 256.0 / _2PI));
int t1, t2, frac = i & 0xff;
i = (i >> 8) & 0xff;

这里 a 被映射成一个定点数：

• 高 8 位 (i >> 8) & 0xff → 表格下标（0~255，对应整个 2π）
• 低 8 位 i & 0xff → 插值分数（256 等分的小数部分）
  四个 if-else 分支处理四个象限：
  

2.2、_atan2() 与 _sqrtApprox() 的数学原理

_atan2() --- 多项式近似（来自 ODrive）

float a = min(abs_x, abs_y) / max(abs_x, abs_y);  // a ∈ [0,1]
float s = a * a;
float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;

这是 Horner 方法的三次多项式，拟合 atan(a) 在 [0,1] 上的值。然后通过象限变换扩展到完整的 [-π, π]：

if (abs_y > abs_x)  r = π/2 - r;   // 超过45°时翻转
if (x < 0)          r = π - r;     // 第二/三象限
if (y < 0)          r = -r;        // 下半平面

_sqrtApprox() --- 快速逆平方根变体

union { float f; uint32_t i; } y = { .f = number };
y.i = 0x5f375a86 - (y.i >> 1);
return number * y.f;   // number × (1/√number) = √number

这是著名的 Quake III 快速逆平方根 （0x5f3759df）的改进版，通过操作 IEEE 754 浮点数的位模式来估算 1/√x，再乘以 x 得到 √x。在无 FPU 的 MCU 上速度比标准库快 3-10 倍。

三、各函数在 FOC 控制环中的位置

---