```c++ // 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 const int asinTable[]={ 0, 5, 11, 17, 23, 30, 36, 44, 53, 64, 90 }; //0-90度反余弦表 #define HLTC_ABS(x) (x>=0? x : -x) //定义绝对值宏 /************************************* * 函数：HLTC_CalPhaseTable * 功能：查找表计算相位 * @param1：归一化后的幅度 -HLTC_AMP ~ HLTC_AMP * 返回值：量化的相位 ************************************/ int HLTC_CalPhaseTable(int a){ if(a > HLTC_AMP) return 0; if(a < -HLTC_AMP ) return 0; int rt = 0; int temp=HLTC_ABS(a); int tDown = temp*10/HLTC_AMP; if(temp==HLTC_AMP) return (a>=0) ? asinTable[tDown] : -asinTable[tDown]; int tUp = tDown + 1 ; rt = (asinTable[tUp] - asinTable[tDown])*(temp-tDown*HLTC_AMP/10)/(HLTC_AMP/10) + asinTable[tDown]; //计算相位值 rt = (a>=0) ? rt : -rt ; return rt; } /******************************************************** * 函数：HLTC_CalPhaseD * 功能：量化相位差 +：相位超前 -:相位滞后 * @param1：归一化后的正弦信号幅度 最大值 HLTC_AMP * @param2：信号相对于与上一次数据的变化趋势 1：递增 0 :递减 * @param3:归一化后的参考正弦信号幅度 最大值 HLTC_AMP * @param4：参考信号相对于与上一次数据的变化趋势 1：递增 0 :递减 * 返回值：量化的相位差 *******************************************************/ #define HLTC_PHASE_REAL int HLTC_CalPhaseD(int a,int dir,int aRef,int dirRef){ #ifdef HLTC_PHASE_REAL //查找表插值量化 int rt=0; int t1 = 0; int t2 = 0; int tRef; if(dirRef == 1){ if(dir == 1) { // 1,1 rt = HLTC_CalPhaseTable(a) - HLTC_CalPhaseTable(aRef); } else { // 1,0 tRef = HLTC_CalPhaseTable(aRef); int tmp = HLTC_CalPhaseTable(a); if( a >= 0 ) t1 = 90 + (90-tmp) ; else t1 = 180 - tmp; t2 = t1 - 360; int tDiff = (t1 - tRef) - (tRef - t2 ) ; if(tDiff >0 ) rt = t2 - tRef; else rt = t1 - tRef; } } else { if( dir ==0 ){ tRef = 180 - HLTC_CalPhaseTable(aRef); t1 = 180 - HLTC_CalPhaseTable(a); rt = t1 - tRef ; } else { tRef = 180 - HLTC_CalPhaseTable(aRef); t1 = HLTC_CalPhaseTable(a); t2 = t1 + 360 ; int tDiff = (tRef -t1 ) - (t2 - tRef ) ; if(tDiff >0 ) rt = t2 - tRef; else rt = t1 - tRef; } } return rt; #else //比值量化相位 int rt=0; int tRef = 0 ; int t1 = 0;//信号的可能位置1 int t2 = 0;//信号的可能位置2 if(dirRef == 1 ) { //参考信号在递增区域 if(dir == 1){ //都在递增区域 rt = a - aRef ; // +:信号超前 -:信号滞后 0:信号同向 } else{ //参考递增，信号递减 //计算参考信号的位置 tRef = aRef; if( a >= 0 ) t1 = HLTC_AMP + (HLTC_AMP-a) ; else t1 = (HLTC_AMP<<1) - a ; // 200 - a t2 = t1 - (HLTC_AMP<<2); //t1 - 400 int tDiff = (t1 - tRef) - (tRef - t2 ) ; if(tDiff >=0 ){ rt = t2 - tRef; } else rt = t1 - tRef; } } else { //参考信号在递减区 if(dir == 0 ) { // down rt = aRef - a ; } else { //up tRef = -aRef; t1 = (HLTC_AMP<<1) + a; // 200 + a t2 = t1 - (HLTC_AMP<<2); // t1 - 400 int tDiff = (t1 - tRef) - (tRef - t2 ) ; if(tDiff >=0 ){ rt = t2 - tRef; } else rt = t1 - tRef; } } return rt; #endif } ``` 使用对相位精度不是很高的场合，可以依据这个实现同频信号的相位PID控制。