游标算法 

/*该算法的所有计算过程中的数均需要进行四舍五入*/ PWM-P_pc=P_DutyCycle*65535； //将 P 信号的占空比(单位%)转化成数值的形式 mean_OS2IS=PWM-P_pc-8192; if (mean_OS2IS<0) mean_OS2IS=mean_OS2IS+49152; if (mean_OS2IS>49152) mean_OS2IS=mean_OS2IS-49152; p_ch_s32=mean_OS2IS*341/1024；//有符号 32 位转换，便于 ECU 计算 mean_OS2IS=p_ch_s32； PWM-S_pc=S_DutyCycle*65535; //将 S 信号的占空比(单位%)转化成数值的形式 ch3=PWM-S_pc-8192; if (ch3<0) ch3=ch3+49152; if (ch3>49152) ch3=ch3-49152; s_ch_s32=(ch3*20187)/8192; //有符号 32 位转换 ch3=s_ch_s32; ptrack=mean_OS2IS; strack=ch3; vd=ptrack-strack; //中间变量 if(vd<0) vd_pos=vd+121122; else vd_pos=vd; vd_pos=vd_pos+1637; //1637~122758 vr_index=vd_pos*5125/2^24 //范围 0~37, vr_index 唯一向下取整。 LUT(vr_index) { vr_index -> vr_sort //查表对应关系 

0->0, 1->22, 2->7, 3->29, 4->14, 5->36, 6->21, 7->6, 8->28, 9->13, 10->35, 11->20, 12->5, 13->27, 14->12, 15->34, 16->19, 17->4, 18->26,19->11, 20->33, 21->18, 22->3, 23->25, 24->10, 25->32, 26->17, 27->2, 28->24, 29->9, 30->31, 31->16, 32->1, 33->23, 34->8, 35->30, 36->15, 37->0; }//查表后 vr_sort 确定 vernier_angle=ptrack+vr_sort*16368 //0~605616 vernier_angle_out=vernier_angle-302808 //-302808~302808 vernier_angle_out=vernier_angle_out/409.2; //由数值转化为最终的绝对角度值，范围-740°~740° vd_nominal=vr_index*32736+16368 vd_real=vd_pos*10 if(vd_nominal>vd_real) vdiff_nom=vd_nominal-vd_real; else vdiff_nom=vd_real-vd_nominal; if(vdiff_nom>16368) vdiff_nom=16368; Vres=16368-vdiff_nom Vres=(Vres/4092/4)*100; //最终的 Vres 值（单位%） if(vdiff_nom>12276) Vres_FD=1; //即 Vres<25%的情况，游标角度不可信，需要待转速降低后重新计算 else Vres_FD=0; //Vres>25%的情况