一句话结论：Q15 只有四条硬规则——乘法必须扩到 32 位再右移、「1.0 怎么表示」这个问题本身就是陷阱（Q15 最大只有 0.99997）、(a+b)/2 一定会溢出，要用 (a&b)+((a^b)>>1)、舍入只值 1 个 LSB，但堆起来就是控制器的噪声底。本文实测：64 点正弦表不插值误差 801 LSB，加线性插值降到 4 LSB；a=b=30000 时 (a+b)/2 得到 -2768 而不是 30000。
一、完整工程下载
压缩包内含全部源码、platformio.ini、Makefile、README.md，解压即用，不需要额外配置。
下载 fixed-q15.zip （15.8 KB，共 8 个文件）
.gitignore
Makefile
README.md
include/
fixed.h
platformio.ini
src/
fixed.c
main.c
test/
test_fixed.c一、Q15 到底是什么
Q15 就是把 int16_t 的 16 个位全部当成小数：最高位是符号位，剩下 15 位是小数。
bit15 bit14 .............................. bit0
│ └──────────── 15 位小数 ────────────┘
└─ 符号（1 = 负数）
0x7FFF = 32767 -> +0.999969...
0x4000 = 16384 -> +0.5
0x2000 = 8192 -> +0.25
0x0001 = 1 -> +0.0000305
0x0000 = 0 -> 0
0xFFFF = -1 -> -0.0000305
0xC000 = -16384 -> -0.5
0x8000 = -32768 -> -1.0 ← 注意只有负方向能到 -1三件事必须一开始就认清：
| 事实 | 推论 |
|---|---|
| 范围是 [-1, +0.99997) | 1.0 表示不了。写 if (x >= Q15_ONE) 永远不会成立 |
| 分辨率是 1/32768 ≈ 3.05e-5 | 1 个 LSB 在这套尺度里就是最小的误差单位 |
| π 装不下（π > 1） | 角度不能直接用 Q15 弧度，必须换一种角度表示 |
第三条是最容易被忽视的。所以真实固件里常用二进制角度： 用 uint16_t 的 0~65535 表示一整圈，0 对应 0，16384 对应 π/2，32768 对应 π。 整个圆恰好是 65536，而 65536 在 16 位里刚好自然溢出回 0—— 加角度不用取模，这是这个表示法最漂亮的地方：
typedef uint16_t bam_t;
#define BAM_QUARTER 16384u /* pi/2 */
#define BAM_HALF 32768u /* pi */
/* a + b 天然就是模 2*pi 的加法，不用写 % */二、乘法：不扩位就是垃圾
这是定点数第一大坑。看这行代码：
q15_t bad(q15_t a, q15_t b) { return (q15_t)(a * b) >> 15; } /* 错！ */a * b 两个 Q15 相乘，结果需要 30 位才能装下（±1 相乘就是 ±32768 级别的数）。 在 16 位上算，中间结果直接溢出；即使 C 帮你提升到 int（32 位）， 先 (q15_t) 截断再右移也已经把高位丢光了。
正确写法：
q15_t q15_mul(q15_t a, q15_t b)
{
/* 中间结果必须是 32 位；加 1<<14 是四舍五入 */
return (q15_t)(((int32_t)a * (int32_t)b + 16384) >> 15);
}为什么是 +16384？ 因为要右移 15 位，被丢掉的那 15 位里最高位是 0.5 个 LSB。 加上半个 LSB 再截断，就等价于四舍五入。不加就是「向下取整」， 误差永远偏负，而且最大到一个完整 LSB。
三、(a+b)/2 是个陷阱
平均值看起来最简单，其实最容易出事：
q15_t s = (q15_t)(a + b); /* 两个 30000 相加 = 60000，塞不进 int16 */
q15_t avg = (q15_t)(s / 2);a = b = 30000 时，正确的平均是 30000，上面这段算出来是 -2768（实测）。 一正一负地翻过去，控制器直接就疯了。
不用加法的写法（本文第 4 项实测）：
q15_t q15_avg_safe(q15_t a, q15_t b)
{
/* 平均 = 相同的位 + 不同位的一半。永远不产生大中间值 */
return (q15_t)((a & b) + ((a ^ b) >> 1));
}原理：a + b = (a & b) * 2 + (a ^ b)。所以 (a+b)/2 = (a & b) + (a ^ b)/2。 a & b 和 a ^ b 都不会超过原数的量级，中间结果天然不会溢出。
两个必须知道的细节：
- 右移的是有符号数，靠的是算术右移（高位补符号位）。
C 标准对负数的 >> 是「实现定义」，但所有主流编译器都做算术右移。
- 这个公式对奇数和的取整方向是向负无穷，不是向零。
如果你要「向零取整」的语义，得自己补一句。
四、不开方的平方根
没有 FPU 的芯片上，sqrtf() 是软件库，几百个周期起步。定点开方根本不需要它：
static uint32_t isqrt32(uint32_t v)
{
uint32_t r = 0;
uint32_t bit = 1u << 30;
while (bit > v) { bit >>= 2; }
while (bit != 0u) { /* 逐位试商，无除法、无浮点 */
if (v >= r + bit) { v -= r + bit; r = (r >> 1) + bit; }
else { r >>= 1; }
bit >>= 2;
}
return r;
}推导一下 Q15 该怎么调用它。要算 y = sqrt(x)：
x 是 Q15（真值 x/32768），y 也是 Q15（真值 y/32768）
要求： y/32768 = sqrt(x/32768)
=> y = 32768 * sqrt(x/32768) = sqrt(x * 32768)所以直接 isqrt32((uint32_t)x * 32768u) 就行，一次乘法加一个无除法循环。 实测最大误差 1 个 LSB（因为逐位试商是截断的）。
五、整数正弦：查表还是多项式？
Cortex-M0/M3 上算 sin() 是奢侈的。两条路：
路线 A：查表 + 线性插值——ROM 换速度，无除法。 表里只存四分之一圆（0~π/2），其余三个象限靠对称性取出来：
象限 0 (0~16384) : s = +tab(a)
象限 1 (16384~32768) : s = +tab(32768 - a)
象限 2 (32768~49152) : s = -tab(a - 32768)
象限 3 (49152~65536) : s = -tab(65536 - a)路线 B：Bhaskara I 近似——纯整数、零 ROM，代价是几次乘法和一次除法：
sin(x) ≈ 16x(pi - x) / (5pi^2 - 4x(pi - x)) , x 属于 [0, pi]这个是公元 7 世纪的正弦近似公式，最大绝对误差约 0.0016， 比 1024 点查表还准，而且不需要任何 ROM。代价是要算一次除法—— 在没有硬件除法器的 M0 上大约几十个周期。
本文实测（第 7 项，遍历全部 65536 个角度，对照 double 的 sin）：
| 方法 | ROM 占用 | 最大误差 | 误差占比 | 说明 |
|---|---|---|---|---|
| 64 点查表，不插值 | 130 字节 | 801 LSB | 2.44% | 误差就是一个完整的采样间隔，太大 |
| 64 点查表 + 线性插值 | 130 字节 | 4 LSB | 0.011% | 同样 ROM，误差降 200 倍 |
| 256 点查表，不插值 | 514 字节 | 198 LSB | 0.60% | 表大 4 倍，还是不如上面那一行 |
| 256 点查表 + 线性插值 | 514 字节 | 2 LSB | 0.005% | 已经很好了 |
| Bhaskara 多项式 | 0 字节 | 54 LSB | 0.164% | 省 ROM，但要用除法 |
这组数字最能说明工程取舍：给查表加线性插值，比把表做大 4 倍划算得多 （4 LSB vs 198 LSB，而 ROM 一个字节都没多，只多了两条指令）。 只有当插值那两条指令的周期你都付不起时，才需要把表做大。
顺带一个实测的坑：不做插值时用的是「向下取整」的采样点， 误差是一个完整间隔（801 LSB），不是半个（400 LSB）。 如果你脑子里的估算是「半个间隔」，实测会大一倍。
六、主机实测（本文数据来源）
| # | 实验 | 实测结果 |
|---|---|---|
| 1 | 乘法（截断）对照 double | 20000 组随机数，最大误差 1.00 LSB，误差非正 20000/20000（恒偏负） |
| 2 | 乘法（舍入）对照 double | 同样 20000 组，最大误差 0.50 LSB，超过 0.5 LSB 的 0 次 |
| 3 | (a+b)/2 溢出 | a=b=30000 时朴素写法得 -2768（正确值 30000） |
| 4 | 位技巧求平均 | 同样输入得 30000；49 组极值组合全部正确（偏差 ≤ 1） |
| 5 | 加减饱和 | 32000+32000 饱和到 32767，直接回绕是 -1536 |
| 6 | 除法饱和 | 0.25/0.5 = 16384；0.5/0.25 = 2.0 装不下，饱和到 32767；除零也饱和 |
| 7 | 定点开方 | 4681 个采样点最大误差 1.00 LSB，无浮点、无除法 |
| 8 | 正弦四种做法 | 801 / 4 / 198 / 2 LSB（见上表），Bhaskara 54 LSB |
第 1、2 项值得细看：截断的最大误差是 1 个 LSB 且恒偏负，舍入是 0.5 个 LSB 且正负对称。 单看一次乘法好像无所谓，但控制环里一帧要算几十次乘加， 误差不抵消、只累积——这就是为什么同样一套 PID 参数， 别人用浮点跑得平滑，你用定点就有肉眼可见的抖动。 舍入只多一条加法指令，一定要加。
第 6 项那个「0.5 / 0.25 = 2.0」也是个典型事故： Q15 最大只有 0.99997，任何超过 1 的中间结果都装不下。 定点代码里每一个除法、每一次累加，都要先问一句「这个量的量程是多少」， 量程不确定就先换成 Q16.16 或者干脆用浮点。
七、什么时候干脆用浮点
别迷信定点。判断标准很简单：
- 有 FPU（Cortex-M4F / M7 / ESP32）：直接用
float，
硬件单周期乘加，定点带来的性能优势基本消失，可读性反而差一大截
- 没有 FPU（M0 / M3 / 8051）：Q15 还是最划算的选择，
尤其控制环里那些乘加
- 需要大动态范围（比如电量计算、功率积分）：定点要非常小心，
这时更合适的是 Q 值可变的定点（Q24.8、Q16.16）或者直接上浮点软件库
- 只在初始化时算的东西（标定曲线、滤波系数）：随便用浮点，不差那点时间
还有一条经验：别在中断里做转换。q15_from_double() 这类函数只用于 启动时算系数，运行时的转换就是架构出了问题。
完整代码
Makefile
CC ?= gcc
CFLAGS ?= -std=c99 -Wall -Wextra -O2 -Iinclude
LDLIBS ?= -lm
SRC = src/fixed.c
TEST = test/test_fixed.c
ifeq ($(OS),Windows_NT)
EXT = .exe
endif
BIN = build/test$(EXT)
all: run
$(BIN): $(SRC) $(TEST)
@mkdir -p build
$(CC) $(CFLAGS) $(SRC) $(TEST) -o $(BIN) $(LDLIBS)
run: $(BIN)
@$(BIN)
clean:
rm -rf build
.PHONY: all run cleaninclude/fixed.h
/**
* fixed.h - Q15 定点数运算
*
* Q15：int16_t 的 16 位全部当小数用，范围 [-1, +0.99997)
* 0x7FFF = 0.99997 0x4000 = 0.5 0x0001 = 3.05e-5
*
* 三条铁律：
* 1) 乘法必须扩到 32 位中间结果，否则高位全丢
* 2) +1.0 表示不了；判断「大于等于 1」要用别的写法
* 3) pi 装不下（pi > 1），角度要用二进制角度（bam_t）
*/
#ifndef FIXED_H
#define FIXED_H
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef int16_t q15_t;
#define Q15_MAX 32767
#define Q15_MIN (-32768)
/* 用 32767 当 1.0 的近似，不要写 32768（它当成 int16 是 -32768） */
#define Q15_ONE 32767
/* ---- 只用于初始化 / 测试，别放在中断里 ---- */
q15_t q15_from_double(double v);
double q15_to_double(q15_t v);
/* ---- 饱和加减：宁可夹住也不要回绕 ---- */
q15_t q15_add_sat(q15_t a, q15_t b);
q15_t q15_sub_sat(q15_t a, q15_t b);
q15_t q15_wrap_add(q15_t a, q15_t b); /* 反面教材：直接回绕 */
/* ---- 乘法 ---- */
q15_t q15_mul(q15_t a, q15_t b); /* 带四舍五入 */
q15_t q15_mul_trunc(q15_t a, q15_t b); /* 截断，用来说明舍入值多少 */
/* ---- 除法：除零时饱和，不产生 UB ---- */
q15_t q15_div(q15_t a, q15_t b);
/* ---- 平均值 ---- */
q15_t q15_avg_naive(q15_t a, q15_t b); /* 反面教材：会溢出 */
q15_t q15_avg_safe(q15_t a, q15_t b); /* (a&b) + ((a^b)>>1) */
/* ---- 定点开方（无浮点、无除法） ---- */
q15_t q15_sqrt(q15_t x);
uint32_t isqrt32(uint32_t v);
/* ---- 二进制角度：0~65535 表示一整圈 ---- */
typedef uint16_t bam_t;
#define BAM_QUARTER 16384u /* pi/2 */
#define BAM_HALF 32768u /* pi */
#define BAM_3QUARTER 49152u /* 3pi/2 */
/** 64 点查表 + 线性插值。interp = 0 时只用最近点（对比误差用） */
q15_t sin_lut(bam_t a, int interp);
/** 256 点查表，参数含义同上。ROM 是 64 点版的 4 倍 */
q15_t sin_lut256(bam_t a, int interp);
q15_t cos_lut(bam_t a, int interp);
/** Bhaskara I 近似：零 ROM，纯整数，需要一次除法 */
q15_t sin_bhaskara(bam_t a);
#ifdef __cplusplus
}
#endif
#endif /* FIXED_H */platformio.ini
[platformio]
default_envs = bluepill
[env:bluepill]
platform = ststm32
board = bluepill_f103c8
framework = arduino
upload_protocol = stlink
monitor_speed = 115200
build_flags =
-Wall
-Wextra
-Isrc
lib_ldf_mode = deep+src/fixed.c
#include "fixed.h"
/* ---------------- 转换（只给初始化和测试用） ---------------- */
q15_t q15_from_double(double v)
{
double s = v * 32768.0;
if (s > 32767.0) {
s = 32767.0;
}
if (s < -32768.0) {
s = -32768.0;
}
/* 负数要向下取整，直接 (int) 截断会让 -0.5 变成 0 */
return (q15_t)((s >= 0.0) ? (s + 0.5) : (s - 0.5));
}
double q15_to_double(q15_t v)
{
return (double)v / 32768.0;
}
/* ---------------- 饱和加减 ---------------- */
q15_t q15_add_sat(q15_t a, q15_t b)
{
int32_t s = (int32_t)a + (int32_t)b;
if (s > Q15_MAX) {
return (q15_t)Q15_MAX;
}
if (s < Q15_MIN) {
return (q15_t)Q15_MIN;
}
return (q15_t)s;
}
q15_t q15_sub_sat(q15_t a, q15_t b)
{
return q15_add_sat(a, (q15_t)(-((int32_t)b)));
}
q15_t q15_wrap_add(q15_t a, q15_t b)
{
/* 反面教材：C 里两个 int16 相加会先提升到 int，
* 直接赋回 int16 就是回绕。32000 + 32000 = -1536 */
return (q15_t)(a + b);
}
/* ---------------- 乘法 ---------------- */
q15_t q15_mul(q15_t a, q15_t b)
{
/*
* 关键点有两个：
* 1) 中间结果必须是 32 位。两个 Q15 相乘，积需要 30 位。
* 2) 右移 15 位之前先加 1<<14 —— 被丢掉的那 15 位里最高位是半个 LSB，
* 加上它就等价于四舍五入。不加就是向下取整，误差恒偏负且最大 1 LSB。
* 最大值 |32768 * 32767| = 1.07e9，加上 16384 仍在 int32 范围内。
*/
return (q15_t)(((int32_t)a * (int32_t)b + 16384) >> 15);
}
q15_t q15_mul_trunc(q15_t a, q15_t b)
{
return (q15_t)(((int32_t)a * (int32_t)b) >> 15);
}
/* ---------------- 除法 ---------------- */
q15_t q15_div(q15_t a, q15_t b)
{
int32_t n;
if (b == 0) {
/* 除零：饱和，别让它变成一个随便的魔数 */
return (a >= 0) ? (q15_t)Q15_MAX : (q15_t)Q15_MIN;
}
/* (a / b) 在 Q15 下要左移 15 位补回定点精度。
* 先左移再除会溢出，所以先除，权衡一下用 32 位中间量。 */
n = ((int32_t)a << 15) / (int32_t)b;
if (n > Q15_MAX) {
return (q15_t)Q15_MAX;
}
if (n < Q15_MIN) {
return (q15_t)Q15_MIN;
}
return (q15_t)n;
}
/* ---------------- 平均值 ---------------- */
q15_t q15_avg_naive(q15_t a, q15_t b)
{
/* 反面教材：先求和再除。
* 30000 + 30000 = 60000，塞进 int16 得到 -5536，除以 2 就是 -2768。 */
q15_t s = (q15_t)(a + b);
return (q15_t)(s / 2);
}
q15_t q15_avg_safe(q15_t a, q15_t b)
{
/*
* a + b = (a & b) * 2 + (a ^ b)
* 两边除以 2：(a + b) / 2 = (a & b) + (a ^ b) / 2
*
* a & b 和 a ^ b 的量级都不会超过原数，所以中间结果永远不溢出。
* 细节：对负数的 >> 靠算术右移，C 标准里是实现定义，
* 但所有主流编译器（gcc / clang / armcc / keil）都是算术右移。
* 另外这个公式对奇数和的取整是向负无穷，不是向零。
*/
return (q15_t)((a & b) + ((a ^ b) >> 1));
}
/* ---------------- 定点开方 ---------------- */
uint32_t isqrt32(uint32_t v)
{
uint32_t r = 0;
uint32_t bit = 1u << 30;
while (bit > v) {
bit >>= 2;
}
while (bit != 0u) {
if (v >= r + bit) {
v -= r + bit;
r = (r >> 1) + bit;
} else {
r >>= 1;
}
bit >>= 2;
}
return r;
}
q15_t q15_sqrt(q15_t x)
{
/*
* 推导：
* 要 y（Q15）满足 y/32768 = sqrt(x/32768)
* => y = 32768 * sqrt(x/32768) = sqrt(x * 32768)
* x 最大 32767，乘 32768 得 1.07e9，uint32 装得下。
*/
if (x <= 0) {
return 0;
}
return (q15_t)isqrt32((uint32_t)x * 32768u);
}
/* ---------------- 整数正弦 ---------------- */
/* 两套表都只存四分之一圆（0 ~ pi/2），另外三个象限靠对称性折出来。
* 64 点表：每格 256 个 bam 单位，65 个 int16 = 130 字节
* 256 点表：每格 64 个 bam 单位，257 个 int16 = 514 字节 */
static const q15_t sin_quad64[65] = {
0, 804, 1608, 2410, 3212, 4011, 4808, 5602,
6393, 7179, 7962, 8739, 9512, 10278, 11039, 11793,
12539, 13279, 14010, 14732, 15446, 16151, 16846, 17530,
18204, 18868, 19519, 20159, 20787, 21403, 22005, 22594,
23170, 23731, 24279, 24811, 25329, 25832, 26319, 26790,
27245, 27683, 28105, 28510, 28898, 29268, 29621, 29956,
30273, 30571, 30852, 31113, 31356, 31580, 31785, 31971,
32137, 32285, 32412, 32521, 32609, 32678, 32728, 32757,
32767,
};
static const q15_t sin_quad256[257] = {
0, 201, 402, 603, 804, 1005, 1206, 1407,
1608, 1809, 2009, 2210, 2410, 2611, 2811, 3012,
3212, 3412, 3612, 3811, 4011, 4210, 4410, 4609,
4808, 5007, 5205, 5404, 5602, 5800, 5998, 6195,
6393, 6590, 6786, 6983, 7179, 7375, 7571, 7767,
7962, 8157, 8351, 8545, 8739, 8933, 9126, 9319,
9512, 9704, 9896, 10087, 10278, 10469, 10659, 10849,
11039, 11228, 11417, 11605, 11793, 11980, 12167, 12353,
12539, 12725, 12910, 13094, 13279, 13462, 13645, 13828,
14010, 14191, 14372, 14553, 14732, 14912, 15090, 15269,
15446, 15623, 15800, 15976, 16151, 16325, 16499, 16673,
16846, 17018, 17189, 17360, 17530, 17700, 17869, 18037,
18204, 18371, 18537, 18703, 18868, 19032, 19195, 19357,
19519, 19680, 19841, 20000, 20159, 20317, 20475, 20631,
20787, 20942, 21096, 21250, 21403, 21554, 21705, 21856,
22005, 22154, 22301, 22448, 22594, 22739, 22884, 23027,
23170, 23311, 23452, 23592, 23731, 23870, 24007, 24143,
24279, 24413, 24547, 24680, 24811, 24942, 25072, 25201,
25329, 25456, 25582, 25708, 25832, 25955, 26077, 26198,
26319, 26438, 26556, 26674, 26790, 26905, 27019, 27133,
27245, 27356, 27466, 27575, 27683, 27790, 27896, 28001,
28105, 28208, 28310, 28411, 28510, 28609, 28706, 28803,
28898, 28992, 29085, 29177, 29268, 29358, 29447, 29534,
29621, 29706, 29791, 29874, 29956, 30037, 30117, 30195,
30273, 30349, 30424, 30498, 30571, 30643, 30714, 30783,
30852, 30919, 30985, 31050, 31113, 31176, 31237, 31297,
31356, 31414, 31470, 31526, 31580, 31633, 31685, 31736,
31785, 31833, 31880, 31926, 31971, 32014, 32057, 32098,
32137, 32176, 32213, 32250, 32285, 32318, 32351, 32382,
32412, 32441, 32469, 32495, 32521, 32545, 32567, 32589,
32609, 32628, 32646, 32663, 32678, 32692, 32705, 32717,
32728, 32737, 32745, 32752, 32757, 32761, 32765, 32766,
32767,
};
#define SIN64_STEP_BITS 8 /* log2(16384/64) */
#define SIN256_STEP_BITS 6 /* log2(16384/256) */
static q15_t lut_lookup(const q15_t *tab, uint32_t step_bits, uint32_t nseg,
bam_t t, int interp)
{
uint32_t idx = (uint32_t)t >> step_bits;
uint32_t mask = (1u << step_bits) - 1u;
int32_t frac = (int32_t)((uint32_t)t & mask);
int32_t v0;
int32_t v1;
if (idx >= nseg) {
idx = nseg; /* 正好 pi/2 */
frac = 0;
}
v0 = tab[idx];
if (!interp || idx >= nseg) {
return (q15_t)v0;
}
v1 = tab[idx + 1u];
/* 线性插值：v0 + (v1 - v0) * frac / 2^step_bits */
return (q15_t)(v0 + (((v1 - v0) * frac) >> step_bits));
}
static q15_t sin_core(bam_t a, int interp, int fine)
{
uint32_t q = ((uint32_t)a >> 14) & 3u; /* 象限 0..3 */
bam_t t = a;
int neg = 0;
q15_t v;
/* 用对称性把 4 个象限都折到第一象限 */
switch (q) {
case 0:
break;
case 1:
t = (bam_t)(BAM_HALF - (uint32_t)a);
break;
case 2:
t = (bam_t)((uint32_t)a - BAM_HALF);
neg = 1;
break;
default:
t = (bam_t)(65536u - (uint32_t)a);
neg = 1;
break;
}
v = fine ? lut_lookup(sin_quad256, SIN256_STEP_BITS, 256u, t, interp)
: lut_lookup(sin_quad64, SIN64_STEP_BITS, 64u, t, interp);
return neg ? (q15_t)(-v) : v;
}
q15_t sin_lut(bam_t a, int interp)
{
return sin_core(a, interp, 0);
}
q15_t sin_lut256(bam_t a, int interp)
{
return sin_core(a, interp, 1);
}
q15_t cos_lut(bam_t a, int interp)
{
/* cos(a) = sin(a + pi/2)，直接用整数加法，天然回绕不用取模 */
return sin_core((bam_t)((uint32_t)a + BAM_QUARTER), interp, 0);
}
q15_t sin_bhaskara(bam_t a)
{
/*
* Bhaskara I（公元 7 世纪）：
* sin(x) ~= 16x(pi-x) / (5pi^2 - 4x(pi-x)) 对 x 属于 [0, pi]
*
* 把 pi 映射成 16384，全部用整数算，零 ROM。
* 分子最大 16*8192*8192 = 1.07e9，分母最大 5*16384^2 = 1.34e9，
* 都在 int32 范围内；只有最后乘 32768 时需要 64 位。
*/
uint32_t t;
int neg = 0;
int32_t p;
int64_t num;
int64_t den;
t = (uint32_t)a;
if (t >= BAM_HALF) {
t -= BAM_HALF; /* 折到第二、三象限的处理 */
neg = 1;
if (t >= BAM_HALF / 2u) {
t = BAM_HALF - t; /* 再折一次回到 0..pi/2 */
}
} else if (t >= BAM_QUARTER) {
t = BAM_HALF - t; /* pi/2..pi 折回 0..pi/2 */
} else {
/* 已在 0..pi/2 */
}
/* 现在 t 属于 [0, 16384]，pi 对应 16384 */
p = (int32_t)t * (int32_t)(BAM_HALF - t); /* x(pi - x)，pi 缩成 16384 */
num = (int64_t)16 * p;
den = (int64_t)5 * (int64_t)BAM_HALF * (int64_t)BAM_HALF - (int64_t)4 * p;
if (den == 0) {
return (q15_t)Q15_MAX;
}
{
int64_t r = (num * 32768) / den;
if (r > Q15_MAX) {
r = Q15_MAX;
}
return neg ? (q15_t)(-r) : (q15_t)r;
}
}src/main.c
/**
* STM32F103 + Q15 定点控制环的骨架
*
* 这个文件想说明一件事：定点数真正的用法不是在主循环里东拼西凑，
* 而是「启动时把浮点参数转成 Q15，运行时全整数」。
*/
#include <string.h>
#include "stm32f1xx_hal.h"
#include "fixed.h"
typedef struct {
q15_t kp;
q15_t ki;
q15_t kd;
q15_t integral;
q15_t prev_err;
q15_t out_min;
q15_t out_max;
} pid_q15_t;
/* 启动时算一次，之后中断里全整数 */
static void pid_q15_set_gains(pid_q15_t *p, double kp, double ki, double kd)
{
p->kp = q15_from_double(kp);
p->ki = q15_from_double(ki);
p->kd = q15_from_double(kd);
}
static q15_t pid_q15_step(pid_q15_t *p, q15_t err, q15_t dt)
{
q15_t p_term;
q15_t i_term;
q15_t d_term;
q15_t out;
p_term = q15_mul(p->kp, err);
/* 积分项：先用 32 位累加，再转换，避免中间回绕 */
p->integral = q15_add_sat(p->integral, q15_mul(err, dt));
i_term = q15_mul(p->ki, p->integral);
d_term = q15_mul(p->kd, q15_sub_sat(err, p->prev_err));
p->prev_err = err;
out = q15_add_sat(q15_add_sat(p_term, i_term), d_term);
if (out > p->out_max) {
out = p->out_max;
/* 抗积分饱和：输出夹住了就不要再累加，否则退饱和要很久 */
p->integral = q15_sub_sat(p->integral, q15_mul(err, dt));
}
if (out < p->out_min) {
out = p->out_min;
p->integral = q15_sub_sat(p->integral, q15_mul(err, dt));
}
return out;
}
int main(void)
{
pid_q15_t pid;
bam_t phase = 0;
HAL_Init();
SystemClock_Config();
memset(&pid, 0, sizeof(pid));
pid_q15_set_gains(&pid, 0.65, 0.02, 0.10);
pid.out_min = -32767;
pid.out_max = 32767;
for (;;) {
q15_t target = sin_lut(phase, 1); /* 模拟一个正弦目标 */
q15_t meas = q15_mul(target, q15_from_double(0.98));
q15_t out = pid_q15_step(&pid, q15_sub_sat(target, meas),
q15_from_double(0.01));
/* 输出给 PWM：Q15 满量程映射到定时器比较值 */
uint32_t ccr = (uint32_t)((int32_t)out + 32768) >> 5; /* 0..2047 */
TIM1->CCR1 = ccr;
phase = (bam_t)(phase + 16u); /* 一整圈 = 65536，自动回绕 */
HAL_Delay(1);
}
}test/test_fixed.c
/**
* 主机端测试：Q15 定点数与整数正弦
*
* gcc -std=c99 -Wall -Wextra -Iinclude src/fixed.c test/test_fixed.c -o build/test
*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "fixed.h"
static int failed = 0;
static void check(int cond, const char *what)
{
if (!cond) {
printf(" [FAIL] %s\n", what);
failed++;
}
}
/* 简单的确定性伪随机，保证每次跑的数字一样 */
static uint32_t g_r = 12345u;
static uint32_t rnd(void)
{
g_r = g_r * 1103515245u + 12345u;
return (g_r >> 8) & 0xFFFFu;
}
static q15_t rnd_q15(void)
{
return (q15_t)((int32_t)rnd() - 32768);
}
int main(void)
{
long i;
printf("===== Q15 定点数实测 =====\n\n");
/* ---------- [1][2] 乘法精度 ---------- */
printf("[1] 乘法：截断 vs 舍入，各跑 20000 组随机数对照 double\n");
{
double max_trunc = 0.0;
double max_round = 0.0;
long trunc_nonpos = 0;
long trunc_zero = 0;
long round_over_half = 0;
const long N = 20000;
for (i = 0; i < N; i++) {
q15_t a = rnd_q15();
q15_t b = rnd_q15();
double exact = ((double)a * (double)b) / 32768.0;
double e1 = (double)q15_mul_trunc(a, b) - exact;
double e2 = (double)q15_mul(a, b) - exact;
if (e1 <= 1e-9) {
trunc_nonpos++; /* 截断的误差永远不可能为正 */
}
if (fabs(e1) < 1e-9) {
trunc_zero++; /* 恰好整除时误差为 0 */
}
if (fabs(e1) > max_trunc) {
max_trunc = fabs(e1);
}
if (fabs(e2) > max_round) {
max_round = fabs(e2);
}
if (fabs(e2) > 0.5 + 1e-9) {
round_over_half++;
}
}
printf(" 截断：最大误差 %.2f LSB，误差非正的 %ld / %ld 次（其中恰好为 0 的 %ld 次）\n",
max_trunc, trunc_nonpos, N, trunc_zero);
printf(" 舍入：最大误差 %.2f LSB，超过 0.5 LSB 的 %ld 次\n",
max_round, round_over_half);
check(max_trunc <= 1.0, "截断误差超过 1 LSB");
check(max_round <= 0.5 + 1e-9, "舍入误差超过 0.5 LSB");
check(trunc_nonpos == N, "截断出现了正的误差（不该发生）");
printf(" -> 截断的误差恒偏负、上限 1 LSB；舍入正负对称、上限 0.5 LSB\n");
printf(" 只多一条加法，误差上限就减半\n");
}
/* ---------- [3][4] 平均值溢出 ---------- */
printf("\n[3] (a+b)/2 溢出：a = b = 30000\n");
{
q15_t a = 30000;
q15_t b = 30000;
printf(" 朴素写法 (q15_t)(a+b)/2 = %d\n", q15_avg_naive(a, b));
printf(" 位技巧 (a&b)+((a^b)>>1) = %d\n", q15_avg_safe(a, b));
printf(" 正确结果 = %d（30000）\n", 30000);
check(q15_avg_naive(a, b) != 30000, "朴素写法竟然没溢出");
check(q15_avg_safe(a, b) == 30000, "安全写法算错了");
printf(" -> a+b = 60000，塞进 int16 变成 %d，再除以 2 就是 %d\n",
(int)(int16_t)(a + b), (int)((int16_t)(a + b) / 2));
}
printf("\n[4] 位技巧求平均：遍历所有「危险」输入组合\n");
{
int bad = 0;
long tested = 0;
q15_t extremes[7];
int x, y;
extremes[0] = 32767; extremes[1] = 32700; extremes[2] = 30000;
extremes[3] = 0; extremes[4] = -30000; extremes[5] = -32700;
extremes[6] = -32768;
for (x = 0; x < 7; x++) {
for (y = 0; y < 7; y++) {
q15_t a = extremes[x];
q15_t b = extremes[y];
/* 参考值用 long 算，保证不溢出 */
long ref = ((long)a + (long)b) / 2;
long got = q15_avg_safe(a, b);
tested++;
/* 位技巧对奇数和的取整是向负无穷，允许 1 的差 */
if (got != ref && got != ref + 1 && got != ref - 1) {
bad++;
}
if (a == 30000 && b == 30000) {
printf(" a=%d b=%d -> %ld（参考 %ld）\n", a, b, got, ref);
}
}
}
printf(" 遍历 %ld 组极值组合，结果偏离超过 1 的：%d 组\n", tested, bad);
check(bad == 0, "位技巧在极值组合上算错了");
}
/* ---------- [5] 饱和 vs 回绕 ---------- */
printf("\n[5] 饱和 vs 回绕：32000 + 32000\n");
{
q15_t a = 32000;
q15_t b = 32000;
printf(" 直接回绕 (q15_t)(a+b) = %d\n", q15_wrap_add(a, b));
printf(" 饱和 q15_add_sat(a,b) = %d\n", q15_add_sat(a, b));
check(q15_wrap_add(a, b) == (q15_t)(-1536), "回绕结果不是 -1536");
check(q15_add_sat(a, b) == Q15_MAX, "饱和没夹到 32767");
printf(" 负方向：-32000 + -32000 -> 饱和 %d\n",
q15_add_sat(-32000, -32000));
check(q15_add_sat(-32000, -32000) == Q15_MIN, "负饱和不对");
printf(" -> 回绕会让「正的大数」突然变成「负的大数」，\n");
printf(" 控制器符号直接反了，这是最难查的现场故障之一\n");
}
/* ---------- [6] 除法与开方 ---------- */
printf("\n[6] 除法与定点开方\n");
{
double max_sqrt_err = 0.0;
long n = 0;
long k;
printf(" 0.25 / 0.5 = %d（应为 16384，即 0.5）\n", q15_div(8192, 16384));
check(q15_div(8192, 16384) == 16384, "除法结果不对");
printf(" 0.5 / 0.25 = %d（真值 2.0，Q15 装不下，饱和）\n",
q15_div(16384, 8192));
check(q15_div(16384, 8192) == Q15_MAX, "商溢出没有饱和");
printf(" 1 / 0 = %d（除零饱和到 Q15_MAX）\n", q15_div(32767, 0));
check(q15_div(32767, 0) == Q15_MAX, "除零没有饱和");
printf(" -1 / 0 = %d（负方向饱和）\n", q15_div(-32767, 0));
check(q15_div(-32767, 0) == Q15_MIN, "负除零没有饱和");
for (k = 1; k <= 32767; k += 7) {
q15_t x = (q15_t)k;
double ref = sqrt((double)x / 32768.0) * 32768.0;
double err = (double)q15_sqrt(x) - ref;
if (fabs(err) > max_sqrt_err) {
max_sqrt_err = fabs(err);
}
n++;
}
printf(" 定点开方：%ld 个采样点，最大误差 %.2f LSB\n", n, max_sqrt_err);
check(max_sqrt_err <= 1.0 + 1e-9, "开方误差超过 1 LSB");
printf(" -> 0.5 的平方根 = %d（0.7071 * 32768 = 23170）\n",
q15_sqrt(16384));
}
/* ---------- [7] 正弦：四种做法 ---------- */
printf("\n[7] 整数正弦：查表（插值/不插值）与 Bhaskara 多项式\n");
{
struct { const char *name; int fine; int interp; } methods[4] = {
{"64 点表 不插值 ", 0, 0},
{"64 点表 + 线性插值", 0, 1},
{"256 点表 不插值 ", 1, 0},
{"256 点表 + 线性插值", 1, 1}
};
int m;
printf(" %-22s %10s %10s %8s\n", "方法", "最大误差", "误差占比", "ROM");
for (m = 0; m < 4; m++) {
double max_err = 0.0;
uint32_t a;
int rom = methods[m].fine ? 514 : 130;
for (a = 0; a < 65536u; a++) {
q15_t got;
double ref;
double err;
if (methods[m].fine) {
got = sin_lut256((bam_t)a, methods[m].interp);
} else {
got = sin_lut((bam_t)a, methods[m].interp);
}
ref = sin(2.0 * 3.14159265358979323846 * (double)a / 65536.0)
* 32767.0;
err = fabs((double)got - ref);
if (err > max_err) {
max_err = err;
}
}
printf(" %-22s %8.0f LSB %9.4f%% %6d B\n", methods[m].name,
max_err, max_err / 327.67, rom);
if (methods[m].interp == 0) {
check(max_err > 50.0, "不插值的误差应该偏大");
} else {
check(max_err < 20.0, "线性插值后误差应该很小");
}
}
/* Bhaskara */
{
double max_err = 0.0;
uint32_t a;
for (a = 0; a < 65536u; a++) {
q15_t got = sin_bhaskara((bam_t)a);
double ref = sin(2.0 * 3.14159265358979323846 * (double)a / 65536.0)
* 32767.0;
double err = fabs((double)got - ref);
if (err > max_err) {
max_err = err;
}
}
printf(" %-22s %8.0f LSB %9.4f%%（ROM = 0）\n",
"Bhaskara 多项式", max_err, max_err / 327.67);
check(max_err < 100.0, "Bhaskara 误差过大");
}
/* 几个具体角度，肉眼对一下 */
printf(" 抽查角度（0.25 = 90 度）：\n");
printf(" sin(16384) 查表 = %6d，Bhaskara = %6d，真值 = %6.0f\n",
sin_lut(BAM_QUARTER, 1), sin_bhaskara(BAM_QUARTER), 32767.0);
printf(" sin(8192) 查表 = %6d，Bhaskara = %6d，真值 = %.0f\n",
sin_lut(8192, 1), sin_bhaskara(8192),
sin(3.14159265358979 / 4.0) * 32767.0);
printf(" cos(0) 查表 = %6d（应为 32767）\n", cos_lut(0, 1));
check(cos_lut(0, 1) == 32767, "cos(0) 不是 32767");
printf(" -> 一个完整的圆 = 65536，角度相加天然回绕，不用取模\n");
}
printf("\n===== %s =====\n",
failed == 0 ? "全部通过：以上数据由本机 gcc 实编译实运行"
: "有失败项！");
return failed == 0 ? 0 : 1;
}实测输出
下面这段输出是把上面的核心算法用 本机 gcc 真编译、真运行得到的（不含任何硬件依赖）：
===== Q15 定点数实测 =====
[1] 乘法：截断 vs 舍入，各跑 20000 组随机数对照 double
截断：最大误差 1.00 LSB，误差非正的 20000 / 20000 次（其中恰好为 0 的 9 次）
舍入：最大误差 0.50 LSB，超过 0.5 LSB 的 0 次
-> 截断的误差恒偏负、上限 1 LSB；舍入正负对称、上限 0.5 LSB
只多一条加法，误差上限就减半
[3] (a+b)/2 溢出：a = b = 30000
朴素写法 (q15_t)(a+b)/2 = -2768
位技巧 (a&b)+((a^b)>>1) = 30000
正确结果 = 30000（30000）
-> a+b = 60000，塞进 int16 变成 -5536，再除以 2 就是 -2768
[4] 位技巧求平均：遍历所有「危险」输入组合
a=30000 b=30000 -> 30000（参考 30000）
遍历 49 组极值组合，结果偏离超过 1 的：0 组
[5] 饱和 vs 回绕：32000 + 32000
直接回绕 (q15_t)(a+b) = -1536
饱和 q15_add_sat(a,b) = 32767
负方向：-32000 + -32000 -> 饱和 -32768
-> 回绕会让「正的大数」突然变成「负的大数」，
控制器符号直接反了，这是最难查的现场故障之一
[6] 除法与定点开方
0.25 / 0.5 = 16384（应为 16384，即 0.5）
0.5 / 0.25 = 32767（真值 2.0，Q15 装不下，饱和）
1 / 0 = 32767（除零饱和到 Q15_MAX）
-1 / 0 = -32768（负方向饱和）
定点开方：4681 个采样点，最大误差 1.00 LSB
-> 0.5 的平方根 = 23170（0.7071 * 32768 = 23170）
[7] 整数正弦：查表（插值/不插值）与 Bhaskara 多项式
方法 最大误差 误差占比 ROM
64 点表 不插值 801 LSB 2.4445% 130 B
64 点表 + 线性插值 4 LSB 0.0111% 130 B
256 点表 不插值 198 LSB 0.6043% 514 B
256 点表 + 线性插值 2 LSB 0.0046% 514 B
Bhaskara 多项式 54 LSB 0.1637%（ROM = 0）
抽查角度（0.25 = 90 度）：
sin(16384) 查表 = 32767，Bhaskara = 32767，真值 = 32767
sin(8192) 查表 = 23170，Bhaskara = 23130，真值 = 23170
cos(0) 查表 = 32767（应为 32767）
-> 一个完整的圆 = 65536，角度相加天然回绕，不用取模
===== 全部通过：以上数据由本机 gcc 实编译实运行 =====