为 8PSK加入上插值同步均衡器，以收敛结果

src/a2psk_demod/main.cpp

@@ -424,13 +424,14 @@ void demod_8psk(const short (*iqdata)[2], const int splen)
 	static double fir_cache_i[25] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
 	static double fir_cache_q[25] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
 	//fir filter
-	static unsigned long long clk = 0, bad_angle = 0, last_synclk = 0;
-	static const int WINSZ = 4000;
+	static unsigned long long clk = 0,  last_synclk = 0;
+	static const int WINSZ = 4000,WINSZx4=WINSZ*4;
 	static double stati_win[WINSZ][2], sum_win[2] = {0,0};
-	static double cache_win[WINSZ][2];
+	static double cache_win[WINSZx4][2];
 
 	//smp best pos
-	static double clk_offset[4] = {0,0,0,0};
+	static double clk_offset[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+	static double clk_offsetLast[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
 	static bool bInited = false;
 	static double curr_dp = 0;
 
@@ -441,7 +442,8 @@ void demod_8psk(const short (*iqdata)[2], const int splen)
 		for (int i=0;i<WINSZ;++i)
 		{
 			stati_win[i][0] = stati_win[i][1] = 0;
-			cache_win[i][0] = cache_win[i][1] = 0;
+			for (int j=0;j<4;++j)
+				cache_win[i*4+j][0] = cache_win[i*4+j][1] = 0;
 		}
 	}
 	//iq dem buf
@@ -487,89 +489,110 @@ void demod_8psk(const short (*iqdata)[2], const int splen)
 		stati_win[clk%WINSZ][1] = fval_q;
 
 		const double dcremoved[2] {fval_i - sum_win[0]/WINSZ, fval_q - sum_win[1]/WINSZ };
-		cache_win[clk%WINSZ][0] = dcremoved[0];
-		cache_win[clk%WINSZ][1] = dcremoved[1];
-
-		//3. 最佳采样位置跟踪
-		const int curr_offv = clk % 4;
-		clk_offset[curr_offv] += (dcremoved[0]*dcremoved[0] + dcremoved[1] * dcremoved[1]);
-		double phy = 0;
-		if (curr_offv == bestOff)
-		{
-			//旋转
-			static const double beautiful_rt225[2]{cos(c_pi/8),sin(c_pi/8)};
-			double rt1[2]{cos(-curr_dp),sin(-curr_dp)};
-			double rt2[2]{cos(-curr_dp-c_pi/4),sin(-curr_dp-c_pi/4)};
-			double rt3[2]{cos(-curr_dp-c_pi/2),sin(-curr_dp-c_pi/2)};
-			double rt4[2]{cos(-curr_dp-3*c_pi/4),sin(-curr_dp-3*c_pi/4)};
-
-			const double out[2]{cache_win[(clk+4)%WINSZ][0],cache_win[(clk+4)%WINSZ][1]};
-			double rp1[2]{
-				out[0] * rt1[0] - out[1] * rt1[1],
-				out[0] * rt1[1] + out[1] * rt1[0]
-			};
-			double rp2[2]{
-				out[0] * rt2[0] - out[1] * rt2[1],
-				out[0] * rt2[1] + out[1] * rt2[0]
-			};
-			double rp3[2]{
-				out[0] * rt3[0] - out[1] * rt3[1],
-				out[0] * rt3[1] + out[1] * rt3[0]
-			};
-			double rp4[2]{
-				out[0] * rt4[0] - out[1] * rt4[1],
-				out[0] * rt4[1] + out[1] * rt4[0]
-			};
-			double rpf[2]{
-				rp1[0] * beautiful_rt225[0] - rp1[1] * beautiful_rt225[1],
-				rp1[0] * beautiful_rt225[1] + rp1[1] * beautiful_rt225[0]
-			};
-			//测角
-			double phy1 = atan_r(rp1[1],rp1[0]);
-			double phy2 = atan_r(rp2[1],rp2[0]);
-			double phy3 = atan_r(rp3[1],rp3[0]);
-			double phy4 = atan_r(rp4[1],rp4[0]);
-			phy = phy1;
-			if (fabs(phy2) < fabs(phy))
-				phy = phy2;
-			if (fabs(phy3) < fabs(phy))
-				phy = phy3;
-			if (fabs(phy4) < fabs(phy))
-				phy = phy4;
-			//调整比例以调整跟踪的步进。
-			curr_dp += phy/2;
-			while (curr_dp>2*c_pi)
-				curr_dp -= 2*c_pi;
-			while (curr_dp<-2*c_pi)
-				curr_dp += 2*c_pi;
+		//上采样到16倍
 
-			dp_iqbuf[output_sps][0] = rpf[0];
-			dp_iqbuf[output_sps][1] = rpf[1];
-
-			double angfinal = atan2(rpf[1],rpf[0]);
-
-			dp_bitbuf[output_sps] = get_sym8psk(angfinal);
-			++output_sps;
-		}
-		//错开最佳点进行判断
-		if (last_synclk + WINSZ < clk && ((clk+2) % 4)==bestOff)
+		static double fir_up[2][25] = {{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}};
+		static unsigned long long up_clock = 0;
+		//1:4 pre-shaping filter
+		//x4
+		for (int sym = 0; sym< 4;++sym)
 		{
-			last_synclk = clk;
-			int maxo = 0;
-			double maxv = clk_offset[0];
-			if (clk_offset[1] > maxv) { maxo = 1; maxv = clk_offset[1];}
-			if (clk_offset[2] > maxv) { maxo = 2; maxv = clk_offset[2];}
-			if (clk_offset[3] > maxv) { maxo = 3; maxv = clk_offset[3];}
-			//qDebug() << maxo;
-			bestOff = maxo;
-
-			if (clk % (WINSZ * 10)==0)
-				fprintf(stderr,"BestOffset=%d,clk = %lld, pyherr = %lf, large ang = %lld\n",bestOff,clk,phy,bad_angle);
-
-			clk_offset[0] = 0;
-			clk_offset[1] = 0;
-			clk_offset[2] = 0;
-			clk_offset[3] = 0;
+			fir_up[0][up_clock % 25] = sym==0?(dcremoved[0]):0;
+			fir_up[1][up_clock % 25] = sym==0?(dcremoved[1]):0;
+			double fval[2] = {0,0};
+			for (int f=0;f<25;++f)
+			{
+				fval[0] += fir_up[0][(up_clock + 1 + f) % 25] * fir_rcos_q25[f];
+				fval[1] += fir_up[1][(up_clock + 1 + f) % 25] * fir_rcos_q25[f];
+			}
+			++up_clock;
+			cache_win[up_clock%WINSZx4][0] = fval[0];
+			cache_win[up_clock%WINSZx4][1] = fval[1];
+
+			//3. 最佳采样位置跟踪
+			const int curr_offv = up_clock % 16;
+			const double curr_r =  sqrt(fval[0]*fval[0] + fval[1] * fval[1]);
+			if (clk_offsetLast[curr_offv] >= 0)
+				clk_offset[curr_offv] += fabs(curr_r -clk_offsetLast[curr_offv] );
+			clk_offsetLast[curr_offv] = curr_r;
+			double phy = 0;
+			if (curr_offv == bestOff)
+			{
+				//旋转
+				static const double beautiful_rt225[2]{cos(c_pi/8),sin(c_pi/8)};
+				double rt1[2]{cos(-curr_dp),sin(-curr_dp)};
+				double rt2[2]{cos(-curr_dp-c_pi/4),sin(-curr_dp-c_pi/4)};
+				double rt3[2]{cos(-curr_dp-c_pi/2),sin(-curr_dp-c_pi/2)};
+				double rt4[2]{cos(-curr_dp-3*c_pi/4),sin(-curr_dp-3*c_pi/4)};
+
+				const double out[2]{cache_win[(up_clock+16)%WINSZx4][0],cache_win[(up_clock+16)%WINSZx4][1]};
+				double rp1[2]{
+					out[0] * rt1[0] - out[1] * rt1[1],
+					out[0] * rt1[1] + out[1] * rt1[0]
+				};
+				double rp2[2]{
+					out[0] * rt2[0] - out[1] * rt2[1],
+					out[0] * rt2[1] + out[1] * rt2[0]
+				};
+				double rp3[2]{
+					out[0] * rt3[0] - out[1] * rt3[1],
+					out[0] * rt3[1] + out[1] * rt3[0]
+				};
+				double rp4[2]{
+					out[0] * rt4[0] - out[1] * rt4[1],
+					out[0] * rt4[1] + out[1] * rt4[0]
+				};
+				double rpf[2]{
+					rp1[0] * beautiful_rt225[0] - rp1[1] * beautiful_rt225[1],
+					rp1[0] * beautiful_rt225[1] + rp1[1] * beautiful_rt225[0]
+				};
+				//测角
+				double phy1 = atan_r(rp1[1],rp1[0]);
+				double phy2 = atan_r(rp2[1],rp2[0]);
+				double phy3 = atan_r(rp3[1],rp3[0]);
+				double phy4 = atan_r(rp4[1],rp4[0]);
+				phy = phy1;
+				if (fabs(phy2) < fabs(phy))
+					phy = phy2;
+				if (fabs(phy3) < fabs(phy))
+					phy = phy3;
+				if (fabs(phy4) < fabs(phy))
+					phy = phy4;
+				//调整比例以调整跟踪的步进。
+				curr_dp += phy/2;
+				while (curr_dp>2*c_pi)
+					curr_dp -= 2*c_pi;
+				while (curr_dp<-2*c_pi)
+					curr_dp += 2*c_pi;
+
+				dp_iqbuf[output_sps][0] = rpf[0];
+				dp_iqbuf[output_sps][1] = rpf[1];
+
+				double angfinal = atan2(rpf[1],rpf[0]);
+
+				dp_bitbuf[output_sps] = get_sym8psk(angfinal);
+				++output_sps;
+			}
+			//错开最佳点进行判断
+			if (last_synclk + WINSZx4 < up_clock && ((up_clock+7) % 16)==bestOff)
+			{
+				last_synclk = up_clock;
+				int mino = 0;
+				double minv = clk_offset[0];
+				for (int j=0;j<16;++j)
+				{
+					if (clk_offset[j] < minv)
+					{
+						mino = j;
+						minv = clk_offset[j];
+					}
+					clk_offset[j] = 0;
+					clk_offsetLast[j] = -1;
+				}
+				//qDebug() << maxo;
+				bestOff = mino;
+				//fprintf(stderr,"BestOffset=%d,clk = %lld, pyherr = %lf, large ang = %lld\n",bestOff,up_clock,phy,bad_angle);
+			}
 		}
 		++clk;
 	}