FOC计算公式
1. PMSM.C中的ADC1Interrupt()AD中断处理函数中调MeasCurr.S中MeasCompCurr: 读入ACD1,ACD2（带符号小数）
校正：ADC1*CarrADC1-offset=qIa ADC2*CarrADC2-offset=qIb
2. PMSM.C中的ADC1Interrupt()中调CalculateParkAngle()对相角θ处理 先调用smcpos.c滑模观测器中SMC_Position_Estimation()，之中 ①又调用smc.s中的CalcEstI 计算估算电流I：
EstIalpha=Gsmopos*(Valpha-Ealpha-Zalpha)+Fsmopos*EstIalpha ---- Isα(n+1)=G*(Vα(n)-Eα(n)-Zα(n))+F*αIsα(n)
EstI beta=Gsmopos*(V beta-E beta-Z beta)+Fsmopos*EstI beta ---- Isβ(n+1)=G*(Vβ(n)-Eβ(n)-Zβ(n))+F*Isβ(n)
②调smc.s中CalcIError计算实测电流与估算电流的偏差： IalphaError = EstIalpha – Ialpha Ibeta Error = EstIbeta – Ibeta
---- ΔIα= Isα- Iα ΔIβ= Isβ- Iβ
③调smc.s中CalcZalpha，CalcZbeta计算Zα, Zβ：
当abs(IalphaError)< MaxSMCError时
Zalpha = (Kslide * IalphaError) / MaxSMCError 当abs(IalphaError)≥ MaxSMCError时
Zalpha为正时Zalpha= Kslide 为负时Zalpha=－Kslide 当abs(I betaError)< MaxSMCError时
Zbeta = (Kslide * IbetaError) / MaxSMCError 当abs(I beta Error)≥ MaxSMCError时
Z beta为正时Z beta = Kslide 为负时Z beta=－Kslide ---- 当|ΔIα|<Δmax时：Zα= Ks*ΔIα/Δmax
当|ΔIα|≥Δmax时：｜Zα｜= Ks
当|ΔIβ|<Δmax时：Zβ= Ks*ΔIβ/Δmax 当|ΔIβ|≥Δmax时：｜Zβ｜= Ks
④调smc.s中CalcBEM按低通滤波器公式计算反电动势：
Ealpha = Ealpha + Kslf * (Zalpha - Ealpha) Ebeta = Ebeta + Kslf * (Zbeta – Ebeta) 两级滤波，后级滤波系数为KslfFinal
---- Eα(n)= Eα(n-1)+Ksf*(Zα(n-1)- Eα(n-1))
Eβ(n)= Eβ(n-1)+Ksf* (Zβ(n-1)- Eβ(n-1))
⑤调atan2CORDIC.s中反正切函数atan2CORDIC计算相角
Theta = atan2CORDIC(-EalphaFinal, EbetaFinal) ---- θ=atan(-Eα/ Eβ)
⑥在smcpos.c中的SMC_Position_Estimation函数中进行速度预算
PrevTheta = s->Theta AccumThetaCnt++
if (AccumThetaCnt == IRP_PERCALC) Omega = AccumTheta
---- 求累加角Σθ+=θ(n)-θ(n-1)
更新θ(n-1)= θ(n) 累加次数m(θCnt)+=1
当m = M (速度计算需要的PWM定时次数)时，速度预算值ω=Σθ 此前仅算出ω(Omega)，未算 V(速度Speed)，即未找到V=Kv*ω
Kv=60/T(测算速度时间)/磁极对数
⑦调smc.s中CalcOmegaFltred对ω进行数字滤波
OmegaFltred=OmegaFltred+FiltOmCoef*(Omega-OmegaFltred) ---- ωfl(n)= ωfl(n-1)+ Kωfl *[ω(n)- ωfl(n-1)]
Kωfl:滤波系数=2πFc/Fpwm Fpwm：脉宽调制频率 Fc: 滤波器截止频率（电机最低转速*磁极对数/60）
⑧在smcpos.c中的SMC_Position_Estimation函数中计算滤波器系数 ??
Kslf = OMEGA0 * _PI / IRP_PERCALC; KslfFinal = OMEGA0 * _PI / IRP_PERCALC
⑨在smcpos.c中的SMC_Position_Estimation函数中进行相位补偿计算
ThetaOffset=OmegaFltred*(SLOPEFRACn+SLOPEINTn)+CONSTANTn
不同的速度有不同的补偿，程序分几个频段,式中3常数取不同值（n不同）。
---- θΔ =ωfl *（Kscn+Kstn）+ Kcn
3. PMSM.C中的ADC1Interrupt()中调clrkepark.s的ClarkePark进行座标变化
Ialpha = Ia
Ibeta = Ia*dOneBySq3 + 2*Ib*dOneBySq3; Id = Ialpha*cos(Angle) + Ibeta*sin(Angle) Iq = -Ialpha*sin(Angle) + Ibeta*cos(Angle)
─
---- Iα= Ia Iβ=(Ia+2Ib)/√3
Id = Iα*Cosθ+Iβ*Sinθ Iq = -Iα*Sinθ+Iβ*Cosθ
4. PMSM.C中的ADC1Interrupt()中调DoControl()进行Id,Iq,Speed的PI迭代
5. 调trig.s中的SinCos计算qSinθ q Cosθ值
6. 调InvPark.s中的InvPark 计算 qVα qVβ值
7. 调CalcRef.s中的CalcRefVec 由qVα qVβ计算Vr1 Vr2 Vr3 值
8. 调SVGEN.s中的CalcSVGen 由Vr1 Vr2 Vr3计算3相PWM占空比。
强制角ParkParm.qAngle（输出PWM角）和滑模估算角Smc1.Theta 电机启动时只用强制角线性递增（称斜坡法） Startup_Lock += 1 达到转速后计算强制角和估算角偏差，对强制角进行修正
计算保存Δθ：Theta_error = ParkParm.qAngle - smc1.Theta 修正量：Startup_Ramp += DELTA_STARTUP_RAMP 对强制角修正：ParkParm.qAngle += Startup_Ramp
Δθ每次±0.05°直到≤0.05°：Theta_error += _0_05DEG