QFOC

Field-Oriented Control (FOC) framework for embedded motor control.

QFOC 是一个三层 FOC 框架，覆盖从数学原语到完整级联控制器的全部需求。框架不绑定硬件、不依赖 RTOS、不占用定时器，所有与时序相关的部分由调用者驱动，因此可运行于 PWM 中断或任意任务上下文中。

概览

框架封装 FOC 中固定的数学运算（坐标变换、SVM 调制、保护、级联调度），并把与硬件、应用相关的部分留作回调由用户实现。

三层按需取用：

层	输入	输出	用户需要实现
`qfoc_core`	$(v_q, v_d)$、电角度	三路 PWM 占空比	全部控制逻辑
`qfoc_base`	$(i_q^, i_d^)$、电角度	三路 PWM 占空比	电流控制器
`qfoc`	位置 / 速度 / 电流给定	三路 PWM 占空比	各环控制器、参数表

典型用法（最高层 qfoc）：

qfoc_init(&foc, ctx, &motor, &ops, &limits);

/* 慢任务 */
qfoc_kinematics_update(&foc, pos, vel);
qfoc_vloop_update(&foc);                        /* 速度环 → 电流给定 */

/* 快任务（PWM 中断） */
qfoc_iloop_update(&foc, vdc, edeg, ia, ib, ic); /* 电流环 → PWM */

三条关键约束：

qfoc_iloop_update() 必须在外环之后调用，它消费外环产生的电流给定；
vdc 必须始终大于 0，SVM 内部以它作除数；
故障后 foc->error 会保持置位并封锁输出，恢复前需调用 qfoc_error_clear()。

1. 框架组成

1.1 FOC 控制周期

FOC 的核心是把三相电流变换到跟随转子旋转的 d/q 坐标系，在 d/q 上完成控制后再变换回三相输出。一个完整的电流控制周期包含以下步骤：

步骤	操作	依赖
1. 采样	读取三相电流 $i_a, i_b, i_c$	ADC
2. 变换	Clarke + Park，得到 $i_q, i_d$	电角度
3. 控制	由误差计算 $v_q, v_d$	用户算法
4. 反变换	逆 Park + 逆 Clarke	电角度
5. 调制	SVM，得到三相占空比	母线电压
6. 输出	写入 PWM 比较寄存器	定时器

步骤 1、6 依赖硬件，步骤 3 依赖应用，步骤 2、4、5 是确定的数学运算。QFOC 封装步骤 2、4、5，并将 1、3、6 以接口形式留给用户：

内容	归属
电流采样、编码器读取、PWM 寄存器操作	用户（硬件相关）
电流环、速度环、位置环算法	用户（应用相关）
Clarke/Park 变换、SVM、限幅、保护、级联调度	框架

1.2 三层结构

框架按功能范围划分为三层，每层均可独立使用：

graph TD
    C["qfoc_core<br/>数学原语 + SVM<br/>(vq, vd) → PWM<br/>无电机参数"]
    B["qfoc_base<br/>最小电流环<br/>(iq*, id*) → PWM<br/>无电机参数"]
    T["qfoc<br/>完整控制器<br/>位置/速度/电流 → PWM<br/>含电机参数、限值、错误处理"]
    C --> B --> T

层	文件	输入 → 输出	电机参数
`qfoc_core`	`qfoc_core.h/c`	$(v_q, v_d)$ → PWM	否
`qfoc_base`	`qfoc_base.h/c`	$(i_q^, i_d^)$ → PWM	否
`qfoc`	`qfoc.h/c`	位置 / 速度 / 电流 → PWM	是

上层复用下层的实现，不重复其逻辑。使用时可以只采用某一层，也可以在使用上层的同时直接调用下层接口。

1.3 层选择

graph TD
    Q1{"需要速度环或位置环吗?"}
    Q1 -->|是| L3["qfoc<br/>各环回调 + 电机参数表"]
    Q1 -->|否| Q2{"需要框架处理<br/>坐标变换、idc 和保护吗?"}
    Q2 -->|是| L2["qfoc_base<br/>仅需电流控制器"]
    Q2 -->|否| L1["qfoc_core<br/>自行组织控制流程"]

场景	建议	需实现
特殊拓扑、完全自定义	`qfoc_core`	变换、控制器、调制
仅电流环（转矩控制）	`qfoc_base`	电流控制器回调
需要速度环或位置环	`qfoc`	各环回调 + 电机参数表
使用现成外环，自行控制电流	`qfoc`	同上，配合 `qfoc_iqd_set()`

1.4 三层对比

同一段电流环中断，在三层中分别写作：

/* qfoc_core：自行组织变换、控制与调制 */
void isr(void)
{
    fp_t ialpha, ibeta, iq, id;
    fp_t vq, vd, va, vb, vc;

    qfoc_clarke(ia, ib, ic, &ialpha, &ibeta);
    qfoc_park(ialpha, ibeta, edeg, &iq, &id);

    vq = my_pi_q(iq_ref - iq);
    vd = my_pi_d(id_ref - id);

    qfoc_svm(vdc, vq, vd, edeg, &va, &vb, &vc);
    pwm_set(va, vb, vc);
}

/* qfoc_base：仅实现控制器 */
void isr(void)
{
    qfoc_base_update(&foc, vdc, edeg, ia, ib, ic);
}

/* qfoc：含外环、保护与错误处理 */
void isr(void)
{
    qfoc_iloop_update(&foc, vdc, edeg, ia, ib, ic);
}

各层提供的能力：

能力	`qfoc_core`	`qfoc_base`	`qfoc`
Clarke/Park 变换	—	框架	框架
控制器调用	—	框架	框架
母线电流 `idc`	—	框架	框架
安全保护	—	可选回调	内置，可覆盖
速度环 / 位置环	—	—	可选
错误锁存	—	—	框架
电机参数管理	—	—	框架

上层电流控制器与下层使用同一 typedef（QFocBaseCtrl），因此从 qfoc_base 迁移到 qfoc 时控制器代码无需修改。

2. 设计思想

2.1 机制与策略分离

框架按照控制系统的通用原则划分职责：

$$ \text{机制（如何计算）} ;\neq; \text{策略（计算结果）} $$

类别	内容	归属	变化频率
机制	Clarke/Park 变换、SVM、限幅、调制、保护框架	框架	固定
策略	各环 PI 参数、前馈解耦、观测器	用户	随电机和应用变化

因此 qfoc_base 不包含任何电机参数——参数属于策略层。qfoc 引入 QFocMotor，是因为它承担了策略层配置的职责。

采用这一划分后，机制代码无需为适配不同电机而修改，策略代码也无需重复实现数学运算。

2.2 回调注入

框架将需要决策的部分以函数指针形式开放给用户：

层	回调	必需	作用
`qfoc_core`	`pwm_set`	是	将归一化占空比写入硬件
`qfoc_base`	`controller`	是	电流环算法
`qfoc_base`	`safety`	否（NULL = 不启用）	故障检测
`qfoc`	`cur_ctrl`	是	电流环算法
`qfoc`	`vel_ctrl`	否（NULL = 不启用）	速度环算法
`qfoc`	`pos_ctrl`	否（NULL = 不启用）	位置环算法
`qfoc`	`safety`	否（NULL = 使用内置保护）	故障检测

可选回调为 NULL 时不产生任何语法或运行开销，未使用的控制环不会被执行。

框架在回调之外还统一执行错误门控：foc->error 非零时不再输出。这一行为对用户透明，只需按普通回调编写即可。

2.3 只读状态快照

回调不直接访问框架内部状态，而是接收一个 const 快照：

typedef int (*QFocBaseCtrl)(void *ctx, const QFocBaseStates *states, fp_t *vq, fp_t *vd);
/*                             ↑ 用户数据       ↑ 框架数据，只读           ↑ 输出 */

各参数的归属与可写性：

参数	归属	可写	内容
`ctx`	用户	是	用户状态（积分器、增益等）
`states`	框架	否	本周期的全部输入信号
`vq`, `vd`	输出	是	控制器计算结果

这一约定带来三点保证：

封装完整：回调无法修改框架状态；
保护不可绕过：控制器只能通过出参提供电压，安全检查与限幅在框架出口统一执行；
const 语义正确：states 属框架，应为只读；ctx 属用户，必须可写，否则控制器无法更新积分状态。

2.4 上下文指针 `ctx`

C 语言中函数指针不携带状态，回调所需的跨周期数据（积分器、增益、限幅）没有存放位置。若使用全局变量，则同一套代码无法驱动多个电机实例。

框架通过 ctx 参数解决：

static int my_ctrl(void *ctx, const QFocBaseStates *s, fp_t *vq, fp_t *vd)
{
    AxisCtrl *c = ctx;
    *vq = pid_calc(&c->pid_q, s->qref - s->iq);
    return 0;
}

AxisCtrl axis1, axis2;
qfoc_base_init(&foc1, &axis1, ...);   /* 同一函数服务两个实例 */
qfoc_base_init(&foc2, &axis2, ...);   /* 各自持有独立状态 */

框架对 ctx 的约定：

约定	说明
框架不解释 `ctx`	仅保存与回传，不引入任何依赖
允许为 NULL	无状态需求可忽略
不加 `const`	回调可能需要写回自身状态
生命周期由用户保证	通常指向静态对象或长生命周期结构体

qfoc_init() 传入的上下文即为全框架唯一的 ctx，cur_ctrl、safety、vel_ctrl、pos_ctrl 收到的都是同一个指针。（qfoc_base 单独使用时由 qfoc_base_init() 传入，含义相同。）

2.5 无调度

框架不包含线程、定时器、延时或中断服务程序。所有 *_update() 均为纯计算函数，调用频率由用户决定。这样可以适配裸机主循环、PWM 中断和 RTOS 任务等任意环境，也便于在不依赖硬件的情况下进行测试。

代价是调用顺序与频率需由用户保证，参见第 6 章。

3. 第一层：qfoc_core

3.1 职责

将 $(v_q, v_d)$ 电压指令与电角度转换为三路 PWM 占空比。无状态、无电机参数。

3.2 数据类型

typedef float fp_t;          /* 全框架统一浮点类型 */

typedef struct {
    fp_t vdc;   /* 母线电压，必须 > 0 */
    fp_t edeg;  /* 电角度，单位：度 */
    fp_t vq;    /* q 轴电压指令 */
    fp_t vd;    /* d 轴电压指令 */
    void (*pwm_set)(fp_t va_norm, fp_t vb_norm, fp_t vc_norm);  /* 占空比 [0,1] */
} QFocCore;

3.3 API

int qfoc_core_init(QFocCore *core, void (*pwm_set)(fp_t, fp_t, fp_t));
int qfoc_core_update(QFocCore *core, fp_t vdc, fp_t edeg);   /* 逆Park + SVM + 输出 */
int qfoc_core_vqd_set(QFocCore *core, fp_t vq, fp_t vd);     /* 设置电压指令 */
int qfoc_core_axis_align(QFocCore *core, fp_t vdc, fp_t edeg, fp_t vd);  /* 强制 vq=0 */

3.4 Clarke 变换约定

alpha = (a - 0.5*(b+c)) * 2/3;
beta  = (√3/2)*(b - c) * 2/3;

系数 $2/3$ 使 $\alpha\beta$ 幅值等于相幅值，称为幅值不变型（另一种约定为功率不变型，系数 $\sqrt{2/3}$）。QFOC 全程采用幅值不变型，以下三处均依赖该约定：

依赖项	表达式	所在层
相电流峰值（过流保护）	$\sqrt{i_d^2+i_q^2}$	`qfoc`
母线电流（功率平衡）	$i_{dc}=\dfrac{3}{2}\cdot\dfrac{v_di_d+v_qi_q}{v_{dc}}$	`qfoc_base`
SVM 线性区上限	$v_{dc}/\sqrt3$	`qfoc_core`

若改成功率不变型（系数 $\sqrt{2/3}$），上表三处必须同时修改，否则功率估算与保护阈值将成比例失准。

3.5 SVM

qfoc_svm() 以最小-最大零序注入实现空间矢量调制（SVPWM）。下面给出其数学原理与实现对应。

3.5.1 空间矢量

三相桥的三个桥臂共 6 个开关管，组合出 8 种开关状态，对应 6 个非零矢量与 2 个零矢量。每个非零矢量的方向固定、幅值相同：

$$ V_k = \frac{2}{3}V_{dc},e^{j\frac{k\pi}{3}}, \qquad k = 0 \ldots 5 $$

在一个 PWM 周期 $T_s$ 内，目标矢量 $V_{ref}$ 由相邻两个非零矢量与零矢量按时间平均合成：

$$ V_{ref},T_s = V_k,T_k + V_{k+1},T_{k+1} + V_0,T_0, \qquad T_k + T_{k+1} + T_0 = T_s $$

6 个非零矢量的端点构成一个正六边形。六边形的内切圆半径即线性调制区的上限：

$$ \lvert V_{ref} \rvert_{max} = \frac{2}{3}V_{dc}\cos 30^\circ = \frac{2}{3}V_{dc}\cdot\frac{\sqrt3}{2} = \frac{V_{dc}}{\sqrt3} $$

3.5.2 最小-最大注入与 SVPWM 的等价性

逆 Clarke 得到的三相电压之和恒为零：

$$ v_a = v_\alpha,\qquad v_b = -\tfrac12 v_\alpha + \tfrac{\sqrt3}{2}v_\beta,\qquad v_c = -\tfrac12 v_\alpha - \tfrac{\sqrt3}{2}v_\beta $$

由于线电压只取决于相间差值，可以给三相同时叠加一个共模（零序）电压 $v_0$ 而不改变电机所感受到的电压：

$$ v_x' = v_x + v_0, \qquad x \in {a, b, c} $$

唯一的约束是归一化占空比必须落在 $[0,1]$：

$$ 0 \le \frac{v_x + v_0}{V_{dc}} + 0.5 \le 1, \qquad \forall x $$

取

$$ v_0 = -\frac{v_{max}+v_{min}}{2} $$

即可让三相电压关于零对称，并在可用区间内居中：

$$ v_{max}' = \frac{v_{max}-v_{min}}{2}, \qquad v_{min}' = -\frac{v_{max}-v_{min}}{2} $$

此时两个极端占空比满足 $d_{max} + d_{min} = 1$，即关于 0.5 对称，这正是零矢量在开关周期两端均分的七段式 SVPWM 的特征。因此最小-最大注入与 SVPWM 在输出上完全等价。

注入的零序分量是一个三倍频（triplen）波形，周期为基波的 $1/3$。设相电压幅值为 $A$，则在 $\theta \in [0^\circ, 60^\circ]$ 内

$$ v_0(\theta) = -\frac{A}{2}\cos(\theta + 60^\circ) $$

峰值约为 $A/4$。由于该波形只含 3 的倍数次谐波，它不产生任何线电压畸变，仅改变各相相对于母线中点的电位。

3.5.3 线性区上限

上节的约束要求 $\lvert v_x' \rvert \le V_{dc}/2$。对幅值为 $A$ 的对称三相电压，三相电压最大值与最小值之差在整周期内的最大值为 $\sqrt3 A$，居中后极值为其一半：

$$ \lvert v_x' \rvert_{max} = \frac{\sqrt3,A}{2} \le \frac{V_{dc}}{2} \quad\Longrightarrow\quad A \le \frac{V_{dc}}{\sqrt3} $$

因此允许的电压矢量模长上限为 $V_{dc}/\sqrt3 \approx 0.577,V_{dc}$。与纯正弦 PWM（$A \le V_{dc}/2$）相比：

$$ \frac{V_{dc}/\sqrt3}{V_{dc}/2} = \frac{2}{\sqrt3} \approx 1.155 $$

即线性区提升约 15.5%，这是零序注入带来的全部收益。

3.5.4 实现对应

int qfoc_svm(fp_t vdc, fp_t vq, fp_t vd, fp_t edeg, fp_t *va, fp_t *vb, fp_t *vc)
{
    /* 1. 圆形限幅：模长上限 vdc/sqrt(3)，等比缩放以保持方向 */
    fp_t vlim = vdc * QFOC_ONE_BY_SQRT3_;
    fp_t vmag = sqrt_(vq * vq + vd * vd);
    if(vmag > vlim) {
        vq = vlim * vq / vmag;
        vd = vlim * vd / vmag;
    }

    /* 2. 逆 Park：(d, q) -> (alpha, beta) */
    fp_t alpha = -vq * sin_(edeg) + vd * cos_(edeg);
    fp_t beta  =  vq * cos_(edeg) + vd * sin_(edeg);

    /* 3. 逆 Clarke + 最小-最大零序注入 */
    return svm_minmax_(vdc, alpha, beta, va, vb, vc);
}

各步与公式的对应：

步骤	对应关系	依据
1. 限幅	模长上限 $V_{dc}/\sqrt3$	3.5.3
2. 逆 Park	$(v_d, v_q) \to (v_\alpha, v_\beta)$	旋转坐标系回到静止坐标系
3. 注入	$v_0 = -\dfrac{v_{max}+v_{min}}{2}$	3.5.2

几点实现说明：

限幅在 $d/q$ 系中完成。Park 是纯旋转，不改变矢量模长，因此在 $(d,q)$ 或 $(\alpha,\beta)$ 中限幅等价。在逆 Park 之前限幅可以省去一次反三角运算。
限幅采用圆而非六边形。圆限幅实现简单、任意角度下最大输出一致，且限幅时保持方向不变。代价是放弃六边形顶角处额外的 $\frac{2}{3}V_{dc}$ 输出能力（相对内切圆再提升约 15%），该区域属于过调制。
不支持过调制。$V_{ref}$ 的模长被限制在内切圆内，不会进入圆与六边形之间的过调制区。
不检查除零以外的 $vdc$ 异常。svm_minmax_() 在 $vdc \le 0$ 时输出三相为 0 并返回 -1，因此母线掉电时不会产生 NaN。

3.6 数学辅助

函数	说明
`qfoc_sin` / `qfoc_cos`	1° 分辨率查表加线性插值，含符号折叠
`qfoc_deg_norm` / `qfoc_rad_norm`	角度归一化到 $[0,360)$ / $[0,2\pi)$
`qfoc_abs` / `qfoc_clamp` / `qfoc_fmod`	基本算术，避免依赖 libm
`qfoc_deg2rad` / `qfoc_rad2deg`	角度单位换算

所有角度参数均为电角度、单位为度，函数名含 rad 者除外。

4. 第二层：qfoc_base

4.1 职责

将 qfoc_core 的无状态原语组织为可用的电流环：给定 $i_q^, i_d^$ 输出 PWM。

4.2 状态快照

typedef struct {
    fp_t vdc;          /* 母线电压 */
    fp_t ia, ib, ic;   /* 三相实测电流 */
    fp_t iq, id;       /* d/q 实测电流 */
    fp_t idc;          /* 母线电流（由功率平衡反推） */

    fp_t qref, dref;   /* d/q 电流给定 */
} QFocBaseStates;

4.3 类型与实例

typedef void (*QFocPwmSet)(fp_t va_norm, fp_t vb_norm, fp_t vc_norm);
typedef int (*QFocBaseCtrl)(void *ctx, const QFocBaseStates *states, fp_t *vq, fp_t *vd);
typedef int (*QFocBaseSafe)(void *ctx, const QFocBaseStates *states);

typedef struct {
    QFocCore core;
    QFocBaseStates states;
    QFocBaseCtrl controller;  /* 必需 */
    QFocBaseSafe safety;      /* 可选，NULL 表示不启用保护 */
    void *ctx;
} QFocBase;

4.4 API

int qfoc_base_init(QFocBase *foc, void *ctx, QFocPwmSet pwm_set,
                   QFocBaseCtrl controller, QFocBaseSafe safety);
int qfoc_base_update(QFocBase *foc, fp_t vdc, fp_t edeg, fp_t ia, fp_t ib, fp_t ic);
int qfoc_base_iqd_set(QFocBase *foc, fp_t iq, fp_t id);

4.5 `qfoc_base_update()` 执行顺序

graph TD
    A["1. 保存 ia/ib/ic 与 vdc"] --> B["2. Clarke + Park，得 iq/id"]
    B --> C["3. controller()，得 vq/vd"]
    C --> D["4. 由 vq/vd 与 iq/id 反推 idc"]
    D --> E["5. safety()"]
    E --> F["6. 逆 Park + SVM + pwm_set"]

`iq`/`id` 由框架计算

回调直接获得变换后的 iq/id，无需在每个控制器中重复实现 Clarke/Park，也避免不同实现之间变换系数不一致。

`idc` 由功率平衡反推

三相逆变器从母线获取的功率等于三相瞬时功率之和，在幅值不变的 d/q 坐标系下表示为：

$$i_{dc}=\frac{p}{v_{dc}}=\frac{3}{2}\cdot\frac{v_d i_d+v_q i_q}{v_{dc}}$$

该式计算量仅为数次乘法与一次除法，无需再次调用 SVM，适合在电流环中断内执行。

`idc` 计算位置

idc 在控制器之后、safety 之前计算。这一顺序不可调换。

safety 跳闸时会将 vq/vd 置零，而置零后的值才是最终写入 core 的值。若改用 core.vq/core.vd（即上一周期 safety 处理后的值）反推当前 idc：

周期	`core.vq/vd`	反推 `idc`	safety 判断	结果
k	正常	偏大	跳闸	电压置零
k+1	0	0	正常	不跳闸，恢复输出
k+2	正常	偏大	跳闸	电压置零
k+3	0	0	正常	恢复

此时保护间隔一个周期触发一次，平均输出仍为额定值的一半，保护实际失效。

当前实现使用本周期、safety 置零之前的局部变量 vq/vd：

周期	本周期 `vq/vd`	反推 `idc`	safety 判断
k	正常	偏大	跳闸
k+1	控制器仍输出正常电压	偏大	保持跳闸
k+2	同上	偏大	保持跳闸

保护一旦触发即保持，不会自行抖动。

由此产生两点时序特性：

控制器在 idc 计算之前执行，因此读取到的是上一周期的 idc，存在一个周期延迟，对前馈或软保护足够；
safety 读取的是本周期的 idc，无延迟。

其它约定

项	说明
`idc` 符号	正值为电动（从母线取电），负值为回馈制动；阈值判断建议取绝对值
计算前提	假设逆变器无损且调制器未饱和。饱和时估算值偏大，对过流保护偏保守

5. 第三层：qfoc

5.1 职责

在 qfoc_base 基础上提供完整电机控制器所需的要素：级联控制结构、电机参数、安全限值与错误处理。

5.2 电机参数

typedef struct {
    /* 机械 */
    size_t PP;   /* 极对数 (required) */
    fp_t GR;     /* 减速比，电机轴→输出轴 (required, 无则 1.0) */
    fp_t Jm;     /* 转动惯量 kg·m² (optional) */

    /* 电气 */
    fp_t Kt;     /* 转矩常数 Nm/A (required) */
    fp_t Ke;     /* 反电动势常数 V·s/rad (required) */
    fp_t Ls;     /* 相电感 H (optional) */
    fp_t Rs;     /* 相电阻 Ω (optional) */
    fp_t psi;    /* 磁链 Wb (optional) */
} QFocMotor;

Rs/Ls/psi 标记为可选，因为它们仅在前馈解耦等算法中需要，纯 PI 控制器不使用。

5.3 安全限值

typedef struct {
    fp_t idc_max;    /* 最大相电流 A，<=0 = 不限 */
    fp_t vdc_max;    /* 最大母线电压 V，<=0 = 不限 */
    fp_t vel_max;    /* 最大速度，<=0 = 不限 */
    fp_t pos_max[2]; /* 位置范围 [下限, 上限]，[0,0] = 不限 */

    size_t i2t_period; /* 热积累窗口，单位：控制周期数 */
    fp_t i2t_max;      /* 最大 I²t 平均值，<=0 = 不限 */
} QFocLimits;

5.4 回调表

typedef struct {
    QFocPwmSet  pwm_set;    /* 必需 */
    QFocBaseSafe safety;    /* 可选，NULL 表示使用内置联锁 */
    QFocBaseCtrl cur_ctrl;  /* 必需 */
    QFocVelCtrl  vel_ctrl;  /* 可选 */
    QFocPosCtrl  pos_ctrl;  /* 可选 */
} QFocOps;

typedef int (*QFocVelCtrl)(void *ctx, const QFocStates *states, fp_t *iq);
typedef int (*QFocPosCtrl)(void *ctx, const QFocStates *states, fp_t *vref);

电流环的三个回调直接复用 qfoc_base 的 typedef，因此签名与内层保持一致，修改 qfoc_base 时 qfoc 会自动同步。

5.5 外环状态

typedef struct {
    fp_t pos, vel;      /* 实测位置 / 速度 */
    fp_t pref, vref;    /* 位置给定 / 速度给定（同时作为位置环输出） */
    const QFocBaseStates *base;  /* 只读的电流环状态 */
} QFocStates;

base 为 const 指针，因此外环回调既不能修改外环状态，也无法访问内环的可写状态。

5.6 顶层实例

typedef struct {
    QFocBase base;            /* 内嵌电流环机制，用户上下文也保存在其中 */
    QFocStates states;        /* 外环状态 */
    const QFocLimits *limits;
    const QFocMotor *motor;
    const QFocOps *ops;
    QFocError error;          /* 锁存错误 */
    fp_t i2t_i2;              /* I²t 累加器 */
    size_t i2t_cnt;
} QFoc;

三张配置表均为 const 指针，配置由用户持有，框架只保存引用。用户上下文保存在内嵌的 QFocBase 中，见 2.4。

5.7 API

int qfoc_init(QFoc *foc, void *ctx, const QFocMotor *motor,
              const QFocOps *ops, const QFocLimits *limits);

/* 外环更新 */
int qfoc_kinematics_update(QFoc *foc, fp_t pos, fp_t vel);
int qfoc_pos_set(QFoc *foc, fp_t pos);
int qfoc_vel_set(QFoc *foc, fp_t vel);
int qfoc_iqd_set(QFoc *foc, fp_t iq, fp_t id);

/* 控制环 */
int qfoc_ploop_update(QFoc *foc);
int qfoc_vloop_update(QFoc *foc);
int qfoc_iloop_update(QFoc *foc, fp_t vdc, fp_t edeg, fp_t ia, fp_t ib, fp_t ic);

/* 辅助 */
int qfoc_align(QFoc *foc, fp_t vdc, fp_t edeg, fp_t vd);
int qfoc_error_clear(QFoc *foc);

5.8 级联结构

graph LR
    P["qfoc_ploop_update<br/>位置误差 → vref<br/>最慢"] --> V["qfoc_vloop_update<br/>速度误差 → iq<br/>居中"]
    V --> I["qfoc_iloop_update<br/>电流环 → PWM<br/>最快 / ISR"]

每环均可独立使用：

应用	需要调用
转矩控制	`qfoc_iloop_update()`
调速	加 `qfoc_vloop_update()`
定位	加 `qfoc_ploop_update()`

5.9 回调行为

cur_ctrl、safety、vel_ctrl、pos_ctrl 都是普通回调，接收到 qfoc_init() 传入的上下文。相对于直接调用，框架额外附加以下行为：

回调	附加行为
`cur_ctrl`	`foc->error` 非零时不被调用，输出保持为零
`safety`	`ops->safety` 为 NULL 时改用内置联锁；返回非 0 时锁存跳闸
`vel_ctrl` / `pos_ctrl`	`foc->error` 非零时不更新

5.10 电角度处理

qfoc_iloop_update() 由调用者传入 edeg，而非内部推导。原因是机械位置 pos 可能指转子轴，也可能指输出轴（取决于 GR 的定义），其换算关系随应用而异。

同理，qfoc_align() 不保存对齐得到的角度偏移。对齐完成后由调用者重建基准，例如编码器清零或重置电角度参考。

6. 数据流与调用时序

6.1 完整数据流

graph TD
    HW["硬件<br/>ADC 电流 / 编码器 / 母线电压"]
    HW --> K["qfoc_kinematics_update<br/>pos, vel"]
    HW --> I["qfoc_iloop_update<br/>vdc, edeg, ia, ib, ic"]

    K --> PL["pos_ctrl → vref"]
    PL --> VL["vel_ctrl → iq"]
    VL --> REF["qfoc_base_iqd_set<br/>qref/dref"]
    I --> REF

    REF --> B["qfoc_base_update"]
    B --> CC["cur_ctrl → vq,vd"]
    CC --> IDC["功率平衡 → idc"]
    IDC --> SF["safety → 可能跳闸"]
    SF --> SVM["SVM"]
    SVM --> PWM["pwm_set → 定时器"]

6.2 建议调用频率

函数	频率	位置
`qfoc_iloop_update()`	最高（如 10–20 kHz）	PWM / ADC 中断
`qfoc_vloop_update()`	居中（如 1–2 kHz）	定时任务
`qfoc_ploop_update()`	最低（如 1 kHz）	定时任务
`qfoc_kinematics_update()`	与位置环同频	定时任务开头

6.3 调用顺序

qfoc_iloop_update() 内部调用 qfoc_base_update()，后者以 states.qref/dref 作为电流给定。因此外环必须先于电流环更新：

/* 正确：先更新给定，再执行电流环 */
qfoc_kinematics_update(&foc, pos, vel);
qfoc_ploop_update(&foc);        /* 写入 vref */
qfoc_vloop_update(&foc);        /* 写入 qref */
qfoc_iloop_update(&foc, ...);   /* 使用 qref */

/* 错误：电流环先执行，速度环的更新延迟一个周期生效 */
qfoc_iloop_update(&foc, ...);
qfoc_vloop_update(&foc);

若电流环运行于中断、外环运行于任务，该顺序由中断抢占关系自动满足；此时外环写入 qref 的操作需要是原子的。

7. 安全与错误处理

7.1 错误码

typedef enum {
    QFOC_ERROR_NONE = 0,
    QFOC_ERROR_OVER_IDC = -1,     /* 过流 */
    QFOC_ERROR_OVER_VDC = -2,     /* 过压 */
    QFOC_ERROR_OVER_THERMAL = -3, /* I²t 过热 */
    QFOC_ERROR_OVER_VEL = -4,     /* 超速 */
    QFOC_ERROR_OVER_POS = -5,     /* 超程 */
    QFOC_ERROR_PHASE = -6,        /* 相位 / 换相故障 */
    QFOC_ERROR_PARAM = -7,        /* 参数或配置无效 */
    QFOC_ERROR_SAFETY = -8,       /* 用户 safety 回调报告故障 */
} QFocError;

NONE 为 0 表示无错误，其余取负值，可直接作为函数返回值。

7.2 safety 的三种配置

配置	行为
`ops->safety = NULL`	使用内置联锁，按 `QFocLimits` 逐项检查并锁存对应错误码
`ops->safety = fn`，返回 0	不跳闸
`ops->safety = fn`，返回非 0	本周期强制零电压并锁存；回调若已写入具体错误码则保留，否则记为 `QFOC_ERROR_SAFETY`

7.3 内置联锁检查项

检查	依据	错误码
母线过压	`states.vdc` 与 `vdc_max`	`OVER_VDC`
相电流过流	`iq² + id²` 与 `idc_max²`（比较平方值，避免在 ISR 内开方）	`OVER_IDC`
超速	`states.vel` 与 `vel_max`	`OVER_VEL`
超程	`states.pos` 与 `pos_max[]`	`OVER_POS`
过热	窗口内 `iq² + id²` 均值与 `i2t_max`	`OVER_THERMAL`

约定：某项限值为 0 或负值（位置区间为空）时，对应检查关闭。因此仅需过流保护的配置同样有效。

I²t 采用窗口平均值而非瞬时值，以反映持续过载，避免单次采样误触发。

7.4 输出门控

foc->error 置位后，框架在三处按以下方式响应：

位置	行为
电流环回调	不再被调用，输出保持为零电压
safety 回调	不再被调用；跳闸状态保持，即使故障条件已消失
速度环、位置环	不再更新，避免积分器在输出关闭期间累积

由此保证：故障时输出固定为零电压，不会周期性通断；恢复前必须先清除错误。

7.5 清除错误

int qfoc_error_clear(QFoc *foc);

除清除 error 外，还会重置 I²t 窗口，避免清除热故障后立即再次触发。

7.6 故障处理流程

graph TD
    DET["safety 检测到异常"] --> LATCH["锁存 foc->error"]
    LATCH --> ZERO["输出固定为零电压"]
    ZERO --> CHECK["用户检查 foc->error"]
    CHECK --> REPAIR["排除故障原因"]
    REPAIR --> CLR["qfoc_error_clear()"]
    CLR --> RESUME["恢复输出"]

恢复电机前必须检查并清除 foc->error，否则输出始终为零。

8. 用法与示例

8.1 最小系统（`qfoc_core`）

直接给定 $v_q$、$v_d$ 开环运行：

#include "qfoc_core.h"

static void my_pwm_set(fp_t va, fp_t vb, fp_t vc)
{
    /* 归一化占空比 [0,1] 映射到定时器比较值 */
    pwm_set_duty(TIM_CH1, (uint16_t)(va * 1000.0f));
    pwm_set_duty(TIM_CH2, (uint16_t)(vb * 1000.0f));
    pwm_set_duty(TIM_CH3, (uint16_t)(vc * 1000.0f));
}

static QFocCore core;

void foc_start(void)
{
    qfoc_core_init(&core, my_pwm_set);

    /* 施加 d 轴电压，将转子拉至已知位置 */
    qfoc_core_axis_align(&core, vbus_get(), 0.0f, vbus_get() / 3.0f);
}

/* 电流环中断 */
void isr_current_loop(void)
{
    fp_t vdc  = vbus_get();
    fp_t edeg = encoder_get_edeg();

    qfoc_core_vqd_set(&core, vdc * 0.5f, 0.0f);
    qfoc_core_update(&core, vdc, edeg);
}

8.2 电流环（`qfoc_base`）

#include "qfoc_base.h"

typedef struct {
    TinyPid pid_q;
    TinyPid pid_d;
} AxisCtrl;

static AxisCtrl axis;

static int my_controller(void *ctx, const QFocBaseStates *s, fp_t *vq, fp_t *vd)
{
    AxisCtrl *c = ctx;
    /* iq/id 已由 qfoc_base 计算完成 */
    *vq = pid_calc(&c->pid_q, s->qref - s->iq);
    *vd = pid_calc(&c->pid_d, s->dref - s->id);
    return 0;
}

static int my_safety(void *ctx, const QFocBaseStates *s)
{
    UNUSED(ctx);
    return (qfoc_abs(s->idc) > 5.0f) ? -1 : 0;   /* 母线电流超过 5A 跳闸 */
}

static QFocBase foc;

void foc_init(void)
{
    pid_init(&axis.pid_q, 1.0f, 0.05f, 0.0f, 12.0f);
    pid_init(&axis.pid_d, 1.0f, 0.05f, 0.0f, 12.0f);

    qfoc_base_init(&foc, &axis, my_pwm_set, my_controller, my_safety);
    qfoc_base_iqd_set(&foc, 1.0f, 0.0f);     /* 1A 转矩电流 */
}

void isr_current_loop(void)
{
    fp_t edeg = encoder_get_edeg();
    qfoc_base_update(&foc, vbus_get(), edeg, ia_get(), ib_get(), ic_get());
}

8.3 转矩模式（`qfoc` + 内置联锁）

#include "qfoc.h"

static int my_cur_ctrl(void *ctx, const QFocBaseStates *s, fp_t *vq, fp_t *vd)
{
    AxisCtrl *c = ctx;
    *vq = pid_calc(&c->pid_q, s->qref - s->iq);
    *vd = pid_calc(&c->pid_d, s->dref - s->id);
    return 0;
}

static const QFocMotor motor = {
    .PP = 11, .GR = 1.0f, .Kt = 0.05f, .Ke = 0.0052f,
    .Rs = 0.5f, .Ls = 0.0003f,
};

static const QFocLimits limits = {
    .idc_max = 10.0f,     /* 相电流上限 10 A */
    .vdc_max = 26.0f,
    .vel_max = 0.0f,      /* 不限速 */
    .pos_max = { 0, 0 },  /* 不限位 */
    .i2t_max = 0.0f,      /* 不启用热保护 */
};

static const QFocOps ops = {
    .pwm_set  = my_pwm_set,
    .safety   = NULL,     /* 使用内置联锁 */
    .cur_ctrl = my_cur_ctrl,
    .vel_ctrl = NULL,
    .pos_ctrl = NULL,
};

static QFoc foc;

void app_init(void)
{
    pid_init(&axis.pid_q, 1.0f, 0.05f, 0.0f, 12.0f);
    pid_init(&axis.pid_d, 1.0f, 0.05f, 0.0f, 12.0f);

    qfoc_init(&foc, &axis, &motor, &ops, &limits);

    /* 上电对齐，随后重置编码器基准 */
    qfoc_align(&foc, vbus_get(), 0.0f, 2.0f);
    os_delay(500);
    encoder_reset();
}

void app_set_torque(fp_t iq)
{
    qfoc_iqd_set(&foc, iq, 0.0f);
}

void isr_current_loop(void)
{
    fp_t edeg = encoder_get_edeg();
    qfoc_iloop_update(&foc, vbus_get(), edeg, ia_get(), ib_get(), ic_get());
}

8.4 速度模式（内置联锁）

在转矩模式基础上增加速度环，其余不变：

static TinyPid pid_vel;

static int my_vel_ctrl(void *ctx, const QFocStates *s, fp_t *iq)
{
    UNUSED(ctx);
    *iq = pid_calc(&pid_vel, s->vref - s->vel);   /* 限幅由 PID 的 olim 完成 */
    return 0;
}

static const QFocOps ops = {
    .pwm_set  = my_pwm_set,
    .safety   = NULL,          /* 内置联锁 */
    .cur_ctrl = my_cur_ctrl,
    .vel_ctrl = my_vel_ctrl,   /* 新增 */
    .pos_ctrl = NULL,
};

void app_init(void)
{
    pid_init(&pid_vel, 0.5f, 0.02f, 0.0f, 8.0f);   /* 输出限幅 8 A */
    qfoc_init(&foc, &axis, &motor, &ops, &limits);
    qfoc_vel_set(&foc, 100.0f);
}

/* 速度环任务，例如 1 kHz */
void task_velocity(void)
{
    qfoc_kinematics_update(&foc, encoder_get_pos(), encoder_get_vel());
    qfoc_vloop_update(&foc);   /* 写入 qref，供电流环使用 */
}

8.5 位置模式（完整级联与自定义 safety）

static TinyPid pid_pos;
static TinyPid pid_vel;

static int my_pos_ctrl(void *ctx, const QFocStates *s, fp_t *vref)
{
    UNUSED(ctx);
    *vref = pid_calc(&pid_pos, s->pref - s->pos);   /* 位置误差 → 速度给定 */
    return 0;
}

static int my_vel_ctrl(void *ctx, const QFocStates *s, fp_t *iq)
{
    UNUSED(ctx);
    *iq = pid_calc(&pid_vel, s->vref - s->vel);     /* 速度误差 → 电流给定 */
    return 0;
}

/* 自定义 safety：在内置检查之外附加应用规则 */
static int my_safety(void *ctx, const QFocBaseStates *s)
{
    QFoc *f = ctx;

    /* 使用框架限值 */
    if(f->limits->idc_max > 0.0f && qfoc_abs(s->idc) > f->limits->idc_max) {
        f->error = QFOC_ERROR_OVER_IDC;   /* 写入具体错误码 */
        return -1;
    }
    /* 附加规则：母线电压过低 */
    if(s->vdc < 12.0f) {
        return -1;                        /* 框架记为 QFOC_ERROR_SAFETY */
    }
    return 0;
}

static const QFocOps ops = {
    .pwm_set  = my_pwm_set,
    .safety   = my_safety,
    .cur_ctrl = my_cur_ctrl,
    .vel_ctrl = my_vel_ctrl,
    .pos_ctrl = my_pos_ctrl,
};

static QFoc foc;

/* 整个框架共用一个用户上下文；本示例中只有 safety 需要访问 QFoc
   （读取 limits、写入 error），因此将 foc 自身作为用户上下文传入。 */
void app_init(void)
{
    pid_init(&pid_pos, 20.0f, 0.0f, 0.0f, 50.0f);
    pid_init(&pid_vel,  0.5f,  0.02f, 0.0f, 8.0f);
    qfoc_init(&foc, &foc, &motor, &ops, &limits);
    qfoc_pos_set(&foc, 3.14f);
}

/* 位置环任务，例如 1 kHz */
void task_position(void)
{
    qfoc_kinematics_update(&foc, encoder_get_pos(), encoder_get_vel());
    qfoc_ploop_update(&foc);   /* 写入 vref */
    qfoc_vloop_update(&foc);   /* 写入 qref */
}

/* 电流环中断 */
void isr_current_loop(void)
{
    qfoc_iloop_update(&foc, vbus_get(), encoder_get_edeg(),
                      ia_get(), ib_get(), ic_get());
}

8.6 直接控制电流

qfoc 的内嵌 QFocBase 可直接访问，用于绕过外环。例如调试时将外环旁路，自行给定电流：

void app_set_current(fp_t iq)
{
    qfoc_iqd_set(&foc, iq, 0.0f);   /* 绕过速度环与位置环 */
}

/* 读取内层状态用于监控 */
void app_monitor(void)
{
    const QFocBaseStates *bs = foc.states.base;
    printf("iq=%.2f id=%.2f idc=%.2f vdc=%.1f\n",
           bs->iq, bs->id, bs->idc, bs->vdc);
}

9. API 速查

9.1 `qfoc_core`

函数	说明
`qfoc_core_init(core, pwm_set)`	初始化
`qfoc_core_update(core, vdc, edeg)`	逆 Park + SVM + 输出
`qfoc_core_vqd_set(core, vq, vd)`	设置 d/q 电压指令
`qfoc_core_axis_align(core, vdc, edeg, vd)`	转子对齐（强制 vq = 0）
`qfoc_svm(vdc, vq, vd, edeg, &va, &vb, &vc)`	独立 SVM
`qfoc_clarke` / `qfoc_iclarke` / `qfoc_park` / `qfoc_ipark`	坐标变换
`qfoc_sin` / `qfoc_cos` / `qfoc_abs` / `qfoc_clamp` / `qfoc_fmod`	数学辅助
`qfoc_deg_norm` / `qfoc_rad_norm` / `qfoc_deg2rad` / `qfoc_rad2deg`	角度处理

9.2 `qfoc_base`

函数	说明
`qfoc_base_init(foc, ctx, pwm_set, controller, safety)`	初始化
`qfoc_base_update(foc, vdc, edeg, ia, ib, ic)`	一次完整电流环
`qfoc_base_iqd_set(foc, iq, id)`	设置电流给定

9.3 `qfoc`

函数	说明
`qfoc_init(foc, ctx, motor, ops, limits)`	初始化
`qfoc_kinematics_update(foc, pos, vel)`	更新外环反馈
`qfoc_pos_set / qfoc_vel_set / qfoc_iqd_set`	设置各级给定
`qfoc_ploop_update / qfoc_vloop_update`	位置环 / 速度环
`qfoc_iloop_update(foc, vdc, edeg, ia, ib, ic)`	电流环（ISR）
`qfoc_align(foc, vdc, edeg, vd)`	转子对齐
`qfoc_error_clear(foc)`	清除锁存错误

10. 约定与注意事项

10.1 单位与语义约定

项	约定
浮点类型	全框架统一为 `fp_t`（当前为 `float`）
角度	电角度，单位为度（函数名含 `rad` 者除外）
占空比	归一化至 `[0, 1]`
电流、电压	标准 SI 单位（A、V）
`idc` 符号	正值为电动，负值为回馈
NULL 语义	`safety`、`vel_ctrl`、`pos_ctrl` 为 NULL 表示该功能不启用

10.2 调用者职责

以下内容不在框架范围内，需由用户实现：

电流采样与标定（量程、极性、零漂）；
电角度的计算与基准重置；
各控制环的调用频率与顺序；
PID 参数的整定；
PWM 通道映射（在 pwm_set 内完成）。

10.3 常见问题

现象	原因	解决
电机不转且无报错	`foc->error` 已置位，输出恒为零	检查 `foc->error`，排除故障后调用 `qfoc_error_clear()`
PWM 输出异常或电机啸叫	`vdc` 为 0 或负值，SVM 中出现除零产生 NaN	保证 `vdc > 0`，掉电时提前停止输出
速度环响应延迟一个周期	`qfoc_iloop_update()` 早于 `qfoc_vloop_update()` 调用	调整调用顺序，参见 6.3
保护间歇性触发	使用 `core.vq/vd` 而非本周期值反推 `idc`	保持现有顺序，参见 4.5
多电机之间存在干扰	PID 状态使用全局变量	通过 `ctx` 为每个实例保存独立状态
`qfoc_init()` 返回 -1	`motor`、`ops`、`ops->pwm_set` 或 `ops->cur_ctrl` 为 NULL	补齐必需项；该检查用于尽早暴露配置错误
配置修改未生效	配置表非静态或已被释放	`motor`、`ops`、`limits` 声明为 `static const`，生命周期需覆盖 `QFoc`
对齐后角度仍不正确	`qfoc_align()` 不保存角度偏移	对齐完成后重置编码器或电角度基准

10.4 集成检查清单

文件结构

App/qfoc/
├── QFOC.md          本文档
├── qfoc_core.h/c    第一层：数学原语与 SVM
├── qfoc_base.h/c    第二层：最小电流环
└── qfoc.h/c         第三层：完整框架

Name		Name	Last commit message	Last commit date
surprise		surprise
LICENSE		LICENSE
README.md		README.md
qfoc.c		qfoc.c
qfoc.h		qfoc.h
qfoc_base.c		qfoc_base.c
qfoc_base.h		qfoc_base.h
qfoc_core.c		qfoc_core.c
qfoc_core.h		qfoc_core.h

Latest commit

History

Folders and files

Repository files navigation