a. 速度计算 inline void SMC_Position_Estimation_Inline (SMC *s) { 。。。 AccumThetaCnt++; if (AccumThetaCnt == IRP_PERCALC) { s->Omega = AccumTheta; AccumThetaCnt = 0; AccumTheta = 0; } 。。。 CalcOmegaFltred(); 。。。 } 为 确 保 在 速 度 计 算 时 获 得 较 为 平 滑 的 信 号，可 在Omega （ ω*）上 施 加 一 个 一 阶 滤 波 器，以 获 得FilteredOmega（ ω*filtered）值。 一阶滤波器的拓扑与用于反电动势滤波的滤波器相同。 滤波公式：
y( n) = y (n – 1) + T2πfc • (x(n)-y(n) ) =y (n – 1) + 2πfc/fpwm • (x(n)-y(n) )
fpwm = 计算数字滤波器时的 PWM 频率
fc = 滤波器的截止频率 s->OmegaFltred = s->OmegaFltred + s->FiltOmCoef * s->Omega - s->FiltOmCoef * s->OmegaFltred
= s->OmegaFltred + s->FiltOmCoef *( s->Omega -s->OmegaFltred) smcpos.c
void SMCInit(SMC *s)
{
s->Kslide = Q15(SMCGAIN);
s->MaxSMCError = Q15(MAXLINEARSMC);
s->FiltOmCoef = Q15(OMEGA0 * _PI / IRP_PERCALC); // / Cutoff frequency for omega filter is minimum omega, or OMEGA0
} 2πfc/fpwm = Q15(OMEGA0 * _PI / IRP_PERCALC)
fc = fpwm * Q15(OMEGA0 * _PI / IRP_PERCALC) /2π
= Q15(OMEGA0 ） * _PI * Fspeedloop / 2π
= Q15(OMEGA0 ） * Fspeedloop / 2
= SPEED0 * LOOPTIMEINSEC * IRP_PERCALC * POLEPAIRS * 2.0 / 60.0 * Fspeedloop / 2
= SPEED0 (Define this in RPMs) * POLEPAIRS / 60.0
= 60 f * POLEPAIRS / 60.0 = f （机械角频率）* POLEPAIRS = f(电角频率）
即 fc 对应 MINSPEEDINRPM 对应的电角频率 至于低通滤波器的的截止频率为什么用SPEED0对应的电角频率，还需要研究？？？？？？？？？？？？ b. #define LOOPTIMEINSEC (1.0/PWMFREQUENCY) // PWM Period = 1.0 / PWMFREQUENCY PWM周期即电流环周期
#define IRP_PERCALC (unsigned int)(SPEEDLOOPTIME/LOOPTIMEINSEC) // PWM loops per velocity calculation 速度环时间比电流环 MINSPEEDINRPM–>SPEED0–>OMEGA0 #define MINSPEEDINRPM 2000 // Minimum speed in RPM. Closed loop will operate at this speed. Open loop will transition to
// closed loop at this minimum speed. #define SPEED0 MINSPEEDINRPM // Define this in RPMs0 #define OMEGA0 (float)(SPEED0 * LOOPTIMEINSEC * IRP_PERCALC * POLEPAIRS * 2.0 / 60.0)// Define this in Degrees, from 0 to 360 OMEGA0的实际意义是一个速度环周期时间走过的标幺化的电角度。 ω = 2 π n/60 ω = 2 π n/60 (n 每分钟多少转，w每秒多少弧度） http://longer.spaces.eepw.com.cn/articles/article/item/43501#
θ（机械角度） = ω * SpeedLoopTime = ω * LOOPTIMEINSEC * IRP_PERCALC = 2 π n * LOOPTIMEINSEC * IRP_PERCALC/60
以 π 为角度基准，对θ标幺化
θpu（机械角度） = 2 n * LOOPTIMEINSEC * IRP_PERCALC/60 （又因为Theta_electric = Theta_machinery * POLEPAIRS ）
θpu（电角度） = 2 n * LOOPTIMEINSEC * IRP_PERCALC* POLEPAIRS /60 = RPM * LOOPTIMEINSEC * IRP_PERCALC* POLEPAIRS* 2 /60 (这里的2是2pi标幺化留下的） 前面的推导先求机械角速度再求电角速度，太烦了。 用n=60f/p 直接从转速切换到电角速度。 以单位时间内走过的路程来表征速度是一个重要思想。 // Q15(Omega) * 60 // Speed RPMs = ----------------------------- // SpeedLoopTime * Motor Poles // For example: // Omega = 0.5 Omega=一个速度环走过的电角度（弧度）的标幺值。 // SpeedLoopTime = 0.001 // Motor Poles (pole pairs * 2) = 10 // Then: // Speed in RPMs is 3,000 RPMs c.
θ = 2 π ft
每一个速度环周期内的
delta_θ = 2 π f Tspeedloop
delta_θ_pu_Q15 = 2 π f Tspeedloop /π * Q15 = f * Tspeedloop * Q16 =f/ Fspeedloop * Q16 =(f/100Hz *Q12 )/( Fspeedloop/100Hz *Q12) *Q16 (f/100Hz Q12 )=delta_θ_pu_Q15 ( Fspeedloop/100Hz Q12) /Q16 = smc1.Omega ( 1600/100/16) = smc1.Omega = Fdpp *10(Hall中的Fdpp） 即：一个速度环周期时间走过的标幺化Q值化的电角度 就是我们需要标幺化Q值化的反馈电角速度 d.
RPS是转速单位的缩写，意思是“xx转每秒”。 eRPS = 电机的电气转速（电角转速），单位为 RPS
eRPS = (RPM * Pole_Pair)/60 推导RPS和f（机械角频率)之间的关系
w = 2πf = 2 π n/60 —> f=n/60=RPM/60=eRPS/Pole_Pair 推导RPS和f（电角频率)之间的关系
w电= 2πf电 =Pole_Pair * w机械 = Pole_Pair * 2 π n/60 —>f电=Pole_Pair n/60 = Pole_Pair RPM/60=eRPS (电机的电气转速) Hz —每秒的周期次数(周期/秒) 每秒的电转速 the electrical revolutions per seconds FUCK 原来eRPS就是f电（HZ） 根本用不着推导： n = 60f/p —> f=np/60= eRPS e.
推导Kslf (es (n +1) -es (n ) )/Tpwm = w0 *(zs (n ) - es (n )) es (n +1 ) =es (n ) + Tpwm w0 (zs (n ) - es (n )) —> Kslf = Tpwm*2πfc Fc is the cutoff frequency of our filter. We want the cutoff frequency be the frequency of the drive currents and voltages of the motor,
which is the electrical revolutions per second, or eRPS.
我们取电机的电气转速f电（每秒的电角度变化）来作为我们滤波器的截止频率
则 Kslf = Tpwm. 2. PI. eRPS 以下推导过程描述了低通滤波器增益的计算，其中：
Kslf = Tpwm. 2. PI. eRPS — ( 3 )
eRPS = (RPM. Pole_Pair)/60 — ( 4)
且，
RPM = (Q15(Omega). 60)/(SpeedLoopTime. Motor Poles) — ( 5 )
将 (5) 代入到 (4) 中，
eRPS = (Q15(Omega). 60. Pole Pairs)/(SpeedLoopTime. Pole Pairs. 2. 60)
eRPS = Q15(Omega)/(SpeedLoopTime. 2) — (6)
将 (6) 代入到 (3) 中，
Kslf = Tpwm. 2. PI. Q15(Omega)/(SpeedLoopTime. 2) — ( 7 )
现在，
IRP_PERCALC = SpeedLoopTime/Tpwm — ( 8 )
将 (8) 代入到 (7) 中，
Kslf = Tpwm. 2. Q15(Omega). PI/(IRP_PERCALC. Tpwm. 2)
简化得到：
Kslf = Q15(Omega). PI/IRP_PERCALC
我们使用具有同样系数的第二个滤波器来获取更加干净的信号：
Kslf = KslfFinal = Q15(Omega). PI/IRP_PERCALC // What this allows us at the end is a fixed phase delay for theta compensation
// in all speed range, since cutoff frequency is changing as the motor speeds up.
//
// Phase delay: Since cutoff frequency is the same as the input frequency, we can
// define phase delay as being constant of -45 DEG per filter. This is because
// the equation to calculate phase shift of this low pass filter is
// arctan(Fin/Fc), and Fin/Fc = 1 since they are equal, hence arctan(1) = 45 DEG.
// A total of -90 DEG after the two filters implemented (Kslf and KslfFinal). f.
开环状态下： // Enter initial torque demand in Amps using REFINAMPS() macro.
// Maximum Value for reference is defined by shunt resistor value and
// differential amplifier gain. Use this equation to calculate
// maximum torque in Amperes:
//
// Max REFINAMPS = (VDD/2)/(RSHUNT*DIFFAMPGAIN)
//
// For example:
//
// RSHUNT = 0.005
// VDD = 3.3
// DIFFAMPGAIN = 75
//
// Maximum torque reference in Amps is:
//
// (3.3/2)/(.005*75) = 4.4 Amperes, or REFINAMPS(4.4)
//
// If motor requires more torque than Maximum torque to startup, user
// needs to change either shunt resistors installed on the board,
// or differential amplifier gain.
CtrlParm.qVqRef = REFINAMPS(INITIALTORQUE); 为什么PI是电流进入电压输出？ answer: pi 就是一个调节 // Vector limitation
// Vd is not limited
// Vq is limited so the vector Vs is less than a maximum of 95%.
// The 5% left is needed to be able to measure current through
// shunt resistors.
// Vs = SQRT(Vd^2 + Vq^2) < 0.95
// Vq = SQRT(0.95^2 - Vd^2)
qVdSquared = FracMpy(PIParmD.qOut, PIParmD.qOut);
PIParmQ.qOutMax = Q15SQRT(Q15(0.95*0.95) - qVdSquared);
PIParmQ.qOutMin = -PIParmQ.qOutMax; 12.
由于截止频率在电机速度不断上升的过程中始终在变化，设计自适应滤波器时，保留了一个固定的相位延时，以补偿所有速度范围内的 theta。由于实现了两个自适应滤波器，因而存在 90° 的固定延时，该延时作为惟一的常数偏移量被加入到计算所得的 theta 中。 Phase delay: Since cutoff frequency is the same as the input frequency, we can define phase delay as being constant of -45 DEG per filter. This is because the equation to calculate phase shift of this low pass filter is arctan(Fin/Fc), and Fin/Fc = 1 since they are equal, hence arctan(1) = 45 DEG. A total of -90 DEG after the two filters implemented (Kslf and KslfFinal). 逆时针为正，顺时针为负。
逆时针加16384，顺时针减16384.
v->EalphaFinal前面全为负。
v->Theta = _IQatan2PU(v->EalphaFinal,v->EbetaFinal)+16384; 14.
截止频率的值被设置为等于驱动电流和电机电压的频率，该频率等于每秒的电气旋转圈数。由于自适应滤波器的实现方式，会有一个固定的相位延时（每个滤波器-45°）， 用于所有速度范围内的 θ补偿 ，因为截止频率会随着电机提速而改变。