在每个 PWM 周期的 SDFM ISR 内，六个独立 FOC 环路由 AxisXFocLoopHandlerX() 函数调用（对于 6 轴，此处 X = 1 至 6）。图 2-3 描述了 PWM 更新和电流反馈的时序。在每个 PWM 周期内会生成两次 SDFM ISR。对于单更新，通过判断 EPWM0 ISR 中设置的软件标志 gEpwmSyncFlag 和 IEP_CMP0 事件 ISR 中设置的 gUpdateNextRisingEdgeCmpValue 来调用 FOC 环路。对于双更新，FOC 环路可以在每个 PWM 周期中调用两次。

AxisXFocLoopHandlerX() 函数包括：

• 在开始时通过工业以太网 SORTE_G 接收通过位置反馈板解码的最新机械角，原始角度数据格式为 Q23，需要转换为浮点格式作为变量 mechThetaX（对于 6 轴，此处 X = 1 至 6）
• 通过以下方式校准机械角和用于 FOC 计算的电角之间的偏移：
  • 首先，使用纯开环旋转电机几个周期，以便通过位置板获得正确的角度，因为 I 脉冲可触发电机角度达到原始 0 度。
  • 第二，将电角（变量 elecTheta）强制设置为 0 度，并将 q 轴电流（变量 parkIqOut）强制设置为 0。为 d 轴电流设置一个恒定值（变量 parkIdOut），然后为电机仅生成没有反电动势 (EMF) 的扭矩电流。此时，电机不会旋转，但强制设置为 0 度电角，以便通过使用 fmod() 函数计算 mechThetaX 和 90.0 度之间的余数来了解机械偏移。然后，将机械偏移存储为变量 mechAngleOffsetX（对于 6 轴，此处 X = 1 至 6）。
• 使用纯开环、闭合电流环路和闭合速度环路选项进行的 FOC 计算：
  • 用于 FOC 计算的 elecTheta 需要通过 mechAngleOffsetX 进行补偿，然后增大 4 倍，因为电机具有 4 对电极。确保 elecTheta 处于 0 至 360 度范围内，作为 ti_r5fmath_sincos() 函数的输入。
  • Clarke 和 Park 通过 CLARKE_run_twoInput() 和 PARK_run() 函数运行变换。
  • Park 逆变换使用 IPARK_run() 函数，空间向量生成由 SVGEN_runCom() 函数组成。
  • 对于纯开环，增量电机角度由固定值 myMechDeltaX 给出，q 轴电流由 gIq 给出。
  • 对于闭环，电流和速度的 PI 控制器都通过 DCL_runPIParallel() 函数实现。电流和速度目标在 gIqArray 和 gSpdArray 数组中定义。PI 常量可以在 init_pids() 函数中调整。