旋转的世界：从CW32L012看CORDIC算法

云深无际

发布于 2026-01-07 13:50:34

1620

文章被收录于专栏：云深之无迹云深之无迹

不知道有没有人知道数值算法或者分析这个分支（是属于应用数学下面的，研究生也开这个课），那这个课研究的也很简单，就是如何让计算机这种东西算我们的数学运算，现实一个主要流派是手算：

hhh，就是这样有具体的算法

那高级的一些算法就是图形结合，或者∫什么的，就是课本上的，但是在计算机里面呢。他们的可以完成的元操作是不多的：加减，移位，逻辑运算（请看计算机原理）。

但是计算机只能通过元操作来完成我们所有书面的科学计算，那我今天就要说说CORDIC它一种仅通过加减运算计算三角函数的数字算术系统。

为什么我会突然说这个算法，在前几日的 CW32L012 的文章里面说了这个数学加速外设（寄存器没多少），但是考虑到，在使用的时候需要深刻理解算法本身，所以会较为详细的给出算法本身的不少推理。

CORDIC 是一种经典的迭代旋转算法，通过一系列移位与加减操作实现三角函数、双曲函数、平方根与向量旋转等运算；它特别适合在无乘法器的 MCU 上实现 sin、cos、tan、atan、sinh、cosh 等；CW32L012 把这一算法固化为硬件协处理器，无需软件循环。

就像开着一辆车从 (1,0) 出发，要转到 θ 角方向；不可能一次把方向盘打准；于是采用一种“分段修正法”，先打个大角度，再小修正，再更小的修正……最后车头方向就非常接近目标角度，这整个“修正过程”就像 CORDIC 的迭代过程。

CORDIC 模块可进行以下计算（通过控制寄存器选择）：

模式	计算内容	输入	输出
正弦/余弦	Y = sin(θ)，X = cos(θ)	角度 θ	X/Y
反正切	θ = atan(Y/X)	X, Y	θ
向量旋转	(X',Y') = R·(cosθ, sinθ)	X, Y, θ	X',Y'
双曲函数	sinh, cosh, tanh	Z	X,Y
模长计算	√(X²+Y²)	X,Y	模长

假设要计算 sin(θ) 和 cos(θ)，在数学上，你知道单位圆上某个角度 θ 的坐标是：

θθ

而 CORDIC 算法做的，就是从一个起点（比如 (1,0)）开始，不断地把这个向量 旋转一些“已知的小角度”，每旋转一步就靠加减和移位算出新的坐标，最后，旋转的总角度就是 θ，新坐标的 x、y 分量，就是 cosθ 和 sinθ。

为什么叫“坐标旋转”？

想象一根长度为 1 的向量从 x 轴开始：

初始向量:  (1, 0)

CORDIC 说：我不会乘法，但是我会做加减和移位！

于是它做这样的事情：

1. 旋转 +45°  ->  得到一个近似方向
2. 再旋转 -26.565° (tan⁻¹(1/2))
3. 再旋转 +14.036° (tan⁻¹(1/4))
4. 再旋转 -7.125° (tan⁻¹(1/8))
...

每次旋转角度越来越小，是固定的、预先存表的（例如 arctan(1/2^i)）。每旋转一步，只需要做：

x_new = x - y >> i
y_new = y + x >> i

（注意是移位 >> i，而不是乘除法）

旋转完多次以后，累计角度 ≈ θ，此时的 (x, y) 就接近 (cosθ, sinθ)。

为什么它“快”？

普通三角函数计算要用级数展开（泰勒展开）或查表 + 插值，这些需要乘法、除法和浮点运算。（宝贝儿，计算机的运算就是这么粗，确实是泰勒展开），CORDIC 不用乘除法，只用：加法、减法；移位（>>）；比较（符号判断），这些运算在 MCU 上非常快（M0+ 一条指令一个周期），所以即使没有 FPU，也能高精度计算 sin、cos、tan、atan、sqrt 等。

每次迭代都会让误差减半，做 10~15 次迭代就能达到 10⁻⁴～10⁻⁵ 精度，足够满足电机控制、滤波、测角等实时应用；在 CW32L012 中，CORDIC 硬件一次迭代只要 1~2 时钟，全部算完仅 6~10 µs，比软件快 30~40 倍。

wiki 这个非常好看

原点右侧 (1,0) 是第 0 步（初始向量）；每一步都是在“往目标角方向”小修正，点越来越接近目标角度那条射线；折线就是“CORDIC 一步步旋转”的静态轨迹；步数标号看到：旋转幅度越来越小（早期几步偏转角大，后面微调）。

0 → 初始方向，在 x 轴正方向；1,2,3... → 每次根据剩余角度 z 的正负，向上或向下旋转一点点；最后一步的点 (x_n, y_n) 就是 CORDIC 算法算出来的 cos(θ) 和 sin(θ)（乘上一个常数 K，不影响方向）。

误差的快速收敛

CORDIC 逼近误差随迭代次数变化

横轴：迭代步数（0 步、1 步、2 步……）；

纵轴：|sin误差|、|cos误差|，用对数坐标画出来；

刚开始几步误差掉得很猛；后面每增加一步，误差大约变成之前的一半左右；10~15 步时，误差已经到 1e-4、1e-5 量级（对 MCU 来说已经很香了），这就是“多几步就多一点精度”的定量化直观。

轨迹图：展示 (1,0) 如何一步步转到目标方向；
误差图：展示“走了 N 步以后，已经离真值有多近”。

合起来理解就是：

轨迹图 = 几何视角： “每一步都在围着单位圆往目标角度逼近”；

误差图 = 数值视角： “再多加几步，误差小几个数量级”。

补偿增益

上面画的是“方向正确但长度被放大了一点”的向量，如果把 CORDIC 固有增益 K 补偿掉，就能看到 (x, y) 真正收敛到单位圆上的点。

在 MCU 的寄存器里面有一项：

这个很不好理解，通过可视化才可以

单位圆 + 真实 (cosθ, sinθ) + 补偿后的 CORDIC 点

已知一个点 (x, y)，CORDIC 一步步“旋转回 x 轴”，最后得到：

x 轴上的长度 ≈ √(x² + y²)

累计旋转角 ≈ atan2(y, x)这可以画成：从 (x, y) 一步步“折回”到 x 轴上的折线轨迹图。

可以看到补偿后的最后一个点几乎落在单位圆上的真值点附近；中间每一个点就是每一轮迭代后的中间结果，还能看到“折线往目标角收敛”的过程；未补偿轨迹相比补偿后的那条，会整体“更长一点”（因为带了 CORDIC 固有增益）。

向量模式

这是 已知 (x0, y0) 求模长与角度 的 CORDIC 轨迹，也就是用来计算：

红点：起点 (x0, y0)，这里用的是 (0.6, 0.8)。

橙色折线：CORDIC 的旋回轨迹，每一步都“旋转一点点”，目标是让 y → 0，从而让向量回到 X 轴。

绿色点：CORDIC 最终点，在 X 轴上，坐标约等于模长 r。

黑色点：真实模长 r，可以看到 CORDIC 最终点和真值非常接近。

这张图展示 CORDIC 如何算 atan2 和 sqrt，—完全不需要乘法，靠旋转逼近得到结果。

落实到MCU：CW32L012

CORDIC 根据 func（功能） 和 scale（模式） 选择不同的数学运算方式；每种运算对应：

计算前需要填入哪些寄存器（X / Y / Z）

计算后结果在哪个寄存器中被读出

结果的数学含义（有时带 1/2 或缩放）

sin 和 cos 的 CORDIC 模式实际上都会返回 sin 和 cos，只是你使用哪个寄存器而已；以及hypot 与 atan2 过程相同，只是用途不同。

Q格式

名字叫定点数格式（Fixed-Point Format），其实就是小数的表示。在 MCU、DSP、CORDIC 这类没有浮点运算器的场景里，浮点数太慢、太复杂，所以用整数来表示小数；1.0，0.5，-0.125这些都用整数 + 缩放方式表示。

Qm.n 的含义是什么？

Qm.n 格式表示：

有 m 位整数部分（包括符号位）

有 n 位小数部分

所以总位数 = m + n 位。

Q1.15 是什么意思？（最常用）

Q1.15 = 1 位符号位 + 15 位小数位 = 16 位整数表示

小数精度 = 1 / 2^15 ≈ 0.0000305

符号位占 1 位，所以数值范围是：

[-1.0, +0.99997]

想表示 0.5：

0.5 × 2^15 = 16384   → 0x4000

想表示 -0.25：

-0.25 × 2^15 = -8192  → 0xE000

也就是说：浮点 → 定点：乘以 2^15定点 → 浮点：除以 2^15，CORDIC 内部就是这样处理的。

Q1.31 是什么意思？（更高精度）

Q1.31 = 1 位符号 + 31 位小数 = 32 位整数表示

小数精度 = 1 / 2^31 = 4.65e-10（非常高）；同样是 [-1, 1) 之间。

同样表示 0.5：

0.5 × 2^31 = 1073741824  → 0x40000000

为什么 CORDIC 会用 Q1.15 或 Q1.31？

CORDIC 的算法本质是加减 + 移位,不做浮点，只做整数;所以要用定点格式来表示小数.Q 格式可以让sin, cos, atan 这些“本来是小数”的值用整数来运算，不需要浮点单元这样在 MCU 上 执行速度非常快。

数据手册

当表中写：

用 s16/s32 表示 [-1, 1) 之间的数字

意思就是：Q1.15 = s16 / 2^15， Q1.31 = s32 / 2^31

如果寄存器给你一个 16 位数，要除以 2^15 才是真正的浮点值；如果它要求你写入一个 32 位数，你要先乘以 2^31 才能写进去，而最后CORDIC 计算结果会自动保持相同的 Q 格式。

Q1.15 就是把 [-1,1] 之间的小数放大 32768 倍变成整数；Q1.31 就是放大 2^31 倍变成整数；CORDIC 用这种格式就能用整数运算模拟浮点三角函数运算。

如何使用

那很简单，就是直接打开例子修改：

5 个

需要配置结构体,可以对照头文件填写

func = CORDIC_FUNC_COS 选择余弦计算。

scale = 0 不扩展输入范围（角度范围按默认约定）。

format = CORDIC_FORMAT_Q1_15 输入/输出采用 Q1.15 定点格式（16 位有符号，1 位整数 + 15 位小数）。

iter = CORDIC_ITER_20 迭代 20 次，提升精度。

comp = 1 使能硬件缩放因子补偿，抵消 CORDIC 固有增益。

ie = 0 禁用中断，采用轮询完成标志。

dmaeoc = 0 、 dmaidle = 0 禁用 DMA 相关特性。

任君选择

计算是自动，所以会自动的提醒：

EOC

在头文件里面是封装了一个结构体

代码运行很简单

就是先开启这个外设，接着输入我们的数据，自动就算了；接着下面变真，就把寄存器里面的数据读取出来，最后进行一些后处理就 ok 了。

两个语法

attribute((at(addr))) 把变量放到指定 RAM 地址。用于让调试器或外设在已知地址读取输出。

Q1.15 是有符号 16 位格式，用 result & 0xFFFF 取低 16 位在显示为 %d ；更稳妥的写法通常是先强制转型为 int16_t 再参与显示或转换，例如将参数改为 (int16_t)result 。因为用了 result & 0xFFFF，这样会把符号位“抹掉”，导致负数变成一个很大的正数；对 cos(π/4) 正数没问题，但如果算 cos(3/4*π) 就会出错。最后计算目标是 cos(π/4) ，定点结果应接近 0x5A82 （十进制约 23170），浮点打印约 0.7071 。通过内存查看 0x20000200 处的字符串可验证输出。

多写几个demo

CW32L012 的 CORDIC + EAU（扩展计算单元：除法/开方）；可以替代一大部分【数学库 + 软件浮点运算】。

传感器信号处理算法中加速度计/陀螺仪倾角计算

比如三轴重力：

用到：atan2，hypot

电子罗盘（磁传感器）：

再测一个 sin 模式

init.func = CORDIC_FUNC_SIN;
CORDIC_Init(&init);
CW_CORDIC->Z = float_to_q1_15(0.5f);      // π/2
while (!CORDIC_GetStatus().eoc);
int16_t sin_q15 = (int16_t)CW_CORDIC->Y;  // 或者看 X/Y 两个寄存器定义

相位 + 模长（atan2 + hypot）

// 求 atan2(y, x)
init.func  = CORDIC_FUNC_ATAN2;
init.scale = 0;
CORDIC_Init(&init);

int16_t x_q15 = float_to_q1_15(0.6f);
int16_t y_q15 = float_to_q1_15(0.8f);

CW_CORDIC->X = x_q15;
CW_CORDIC->Y = y_q15;
while (!CORDIC_GetStatus().eoc);

int16_t r_q15   = (int16_t)CW_CORDIC->X; // ≈ √(x²+y²)/2，注意有 1/2
int16_t ang_q15 = (int16_t)CW_CORDIC->Z; // 以 π 为单位的角度

配合 q1_15_to_float() 再乘 π，就能在 MCU 上做完整的 极坐标转换。

三轴重力计算demo

先说数学公式（标准“三轴重力解算”）

假设三轴加速度计输出为：

（单位随便：g、m/s² 都行，只要三个轴量纲一致）

典型的姿态角（机体坐标）定义：

横滚角 roll（绕 X 轴）

俯仰角 pitch（绕 Y 轴）

注意：只用加速度做姿态，假设“只有重力”，“无剧烈线加速度”。

算法迁移

CW32L012 的 CORDIC 已经给了：

atan2：输入 X, Y → 输出 Z = atan(Y/X)，以 π 为单位，Q1.15

hypot：输入 X, Y → 输出 X = √(X²+Y²)/2，Z = atan(Y/X)（同样以 π 为单位）

所以：

roll

直接用 CORDIC 的 atan2：

写入：X = a_z，Y = a_y

读回：Z（Q1.15，单位为 “π 的份数”）

pitch

分两步：

用 hypot 算写入 X = ay，Y = az 读回 X = r_half = √(ay²+az²)/2 我们要的是 r = √(ay²+az²)，所以：r = r_half << 1（乘 2）
再用 atan2 算 pitch：写入 X = r，Y = -ax 读回 Z，就是 pitch（同样以 π 为单位的 Q1.15）

输入输出格式：全部用 Q1.15

假设加速度已经归一化到 [-1, 1]g（或者直接用满量程映射到 [-1,1]）：

都用 int16_t 的 Q1.15 表示：

float_to_q1_15(f)：(int16_t)(f * 32768.0f)

q1_15_to_float(q)：(float)q / 32768.0f

角度输出，CORDIC 的 Z 是：以 π 为单位的 Q1.15。也就是：

Z_q15 = 1.0（Q1.15） ⇒ 角度是 π 弧度

Z_q15 = 0.5 ⇒ π/2

想要角度（度）：

float angle_deg = q1_15_to_float(Z_q15) * 180.0f;

先写一个“等待完成”的小函数

static void cordic_wait_eoc(void)
{
    while (!CORDIC_GetStatus().eoc) {
        ; // 等待 CORDIC 运算完成
    }
}

三轴重力姿态解算（Q1.15 版本）

extern int16_t float_to_q1_15(float x);
extern float   q1_15_to_float(int16_t q);

// 使用 CORDIC 求 roll / pitch，输入输出都是 Q1.15 格式
// ax_q15, ay_q15, az_q15: 归一化加速度，Q1.15
// roll_q15_pi, pitch_q15_pi: 输出角度，以 pi 为单位的 Q1.15
void tilt_from_acc_q15_cordic(int16_t ax_q15,
                              int16_t ay_q15,
                              int16_t az_q15,
                              int16_t *roll_q15_pi,
                              int16_t *pitch_q15_pi)
{
    cordic_init_t init = {
        .scale  = 0,
        .format = CORDIC_FORMAT_Q1_15,
        .iter   = CORDIC_ITER_20,
        .comp   = 1,    // 自动补偿伸缩因子
        .ie     = 0,
        .dmaeoc = 0,
        .dmaidle= 0
    };

    // -------- 1. roll = atan2(ay, az) --------
    init.func = CORDIC_FUNC_ATAN2;
    CORDIC_Init(&init);

    CW_CORDIC->X = az_q15;
    CW_CORDIC->Y = ay_q15;
    cordic_wait_eoc();

    int16_t roll_pi_q15 = (int16_t)CW_CORDIC->Z;   // 以 pi 为单位的 Q1.15

    // -------- 2. r = sqrt(ay^2 + az^2) --------
    init.func = CORDIC_FUNC_HYPOT;
    CORDIC_Init(&init);

    CW_CORDIC->X = ay_q15;
    CW_CORDIC->Y = az_q15;
    cordic_wait_eoc();

    int16_t r_half_q15 = (int16_t)CW_CORDIC->X;    // = sqrt(ay^2+az^2)/2
    int16_t r_q15      = (int16_t)(r_half_q15 << 1); // ×2 恢复真正的 r

    // -------- 3. pitch = atan2(-ax, r) --------
    init.func = CORDIC_FUNC_ATAN2;
    CORDIC_Init(&init);

    CW_CORDIC->X = r_q15;
    CW_CORDIC->Y = (int16_t)(-ax_q15);
    cordic_wait_eoc();

    int16_t pitch_pi_q15 = (int16_t)CW_CORDIC->Z;

    if (roll_q15_pi)  *roll_q15_pi  = roll_pi_q15;
    if (pitch_q15_pi) *pitch_q15_pi = pitch_pi_q15;
}

写一个 float 的大函数

// 输入 ax,ay,az 为 float（单位随意），输出 roll/pitch（单位：度）
void tilt_from_acc_float_cordic(float ax, float ay, float az,
                                float *roll_deg,
                                float *pitch_deg)
{
    // 1) 先归一化到 [-1, 1]，避免超出 Q1.15
    float g = sqrtf(ax*ax + ay*ay + az*az);
    if (g < 1e-6f) {
        // 重力太小，可能是自由落体，直接返回 0
        if (roll_deg)  *roll_deg  = 0.0f;
        if (pitch_deg) *pitch_deg = 0.0f;
        return;
    }

    float ax_n = ax / g;
    float ay_n = ay / g;
    float az_n = az / g;

    // 2) 转为 Q1.15
    int16_t ax_q15 = float_to_q1_15(ax_n);
    int16_t ay_q15 = float_to_q1_15(ay_n);
    int16_t az_q15 = float_to_q1_15(az_n);

    // 3) 用上面的 CORDIC Q15 版本算角度
    int16_t roll_pi_q15, pitch_pi_q15;
    tilt_from_acc_q15_cordic(ax_q15, ay_q15, az_q15,
                             &roll_pi_q15, &pitch_pi_q15);

    // 4) 转成 float 角度（单位：度）
    if (roll_deg) {
        float roll_frac_pi = q1_15_to_float(roll_pi_q15); // 角度是几倍 pi
        *roll_deg = roll_frac_pi * 180.0f;               // 乘 180 得度
    }
    if (pitch_deg) {
        float pitch_frac_pi = q1_15_to_float(pitch_pi_q15);
        *pitch_deg = pitch_frac_pi * 180.0f;
    }
}

修改 demo

现在的 demo 已经有：

CORDIC_Init(&init);

float_to_q1_15() / q1_15_to_float()

CW_CORDIC 寄存器定义

CORDIC_GetStatus() 的 eoc 标志

把上面的 cordic_wait_eoc、tilt_from_acc_q15_cordic、tilt_from_acc_float_cordic 拷贝进工程；

在 main 里替换成上面的函数：

int main(void)
{
    float ax = 0.0f, ay = 0.0f, az = 1.0f;  // 静止放在桌面上

    float roll_deg, pitch_deg;
    tilt_from_acc_float_cordic(ax, ay, az, &roll_deg, &pitch_deg);

    sprintf(RamBuf, "roll=%.2f deg, pitch=%.2f deg\n", roll_deg, pitch_deg);
    while (1);
}

如果一切正常：

水平放置时：roll ≈ 0°, pitch ≈ 0°

机头向下（x 轴朝地）：pitch ≈ +90° 或 -90°（视坐标定义）