fft.c

FFT的实现在LPC2378上。首先采用了拉个朗日插值算法对采集的电网波形频率跟踪

//
// Re1 = ReArray[indexA], Re2 = ReArray[indexB]
// Im1 = ImArray[indexA], Im2 = ImArray[indexB]
// x = the angle: 2*pi*(sin_index/NUM_FFT), in radians. The necessary values
// are stored in code space in the SinTable[] array.
//
//
// Key Points for using this FFT routine:
//
// 1) It expects REAL data (in ReArray[]), in 2’s complement, 16-bit binary
// format and assumes a value of 0 for all imaginary locations
// (in ImArray[]).
//
// 2) It expects the REAL input data to be sorted in bit-reversed index order.
//
// 3) SIN and COS values are retrieved and calculated from a table consisting
// of 1/4 of a period of a SIN function.
//
// 4) It is optimized to use integer math only (no floating-point operations),
// and for storage space. The input, all intermediate stages, and the
// output of the FFT are stored as 16-bit INTEGER values. This limits the
// precision of the routine. When using input data of less than 16-bits,
// the best results are produced by left-justifying the data prior to
// windowing and performing the FFT.
//
// 5) The algorithm is a Radix-2 type, meaning that the number of samples must
// be 2^N, where N is an integer. The minimum number of samples to process
// is 4. The constant NUM_FFT contains the number of samples to process.
//
//
void Int_FFT(INT16 ReArray[], INT16 ImArray[])
{
	#if (NUM_FFT >= 512)
	WORD sin_index, g_cnt, s_cnt; // Keeps track of the proper index
	WORD indexA, indexB; // locations for each calculation
	#endif
	#if (NUM_FFT <= 256)
	unsigned char sin_index, g_cnt, s_cnt; // Keeps track of the proper index
	unsigned char indexA, indexB; // locations for each calculation
	#endif
	WORD group = NUM_FFT/4, stage = 2;
	long CosVal, SinVal;
	long TempImA, TempImB, TempReA, TempReB, TempReA2, TempReB2;
	IBALONG ReTwid, ImTwid, TempL;
	// FIRST STAGE - optimized for REAL input data only. This will set all
	// Imaginary locations to zero.
	//
	// Shortcuts have been taken to remove unnecessary multiplications during this
	// stage. The angle “x” is 0 radians for all calculations at this point, so
	// the SIN value is equal to 0.0 and the COS value is equal to 1.0.
	// Additionally, all Imaginary locations are assumed to be ‘0’ in this stage of
	// the algorithm, and are set to ‘0’.
	indexA = 0;
	for (g_cnt = 0; g_cnt < NUM_FFT/2; g_cnt++)
	{
		indexB = indexA + 1;
		TempReA = ReArray[indexA];
		TempReB = ReArray[indexB];
		// Calculate new value for ReArray[indexA]
		TempL.l = (long)TempReA + TempReB;
		if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
			TempReA2 = (TempL.l >> 1) + 1;
		else TempReA2 = TempL.l >> 1;
		// Calculate new value for ReArray[indexB]
		TempL.l = (long)TempReA - TempReB;
		if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
			ReArray[indexB] = (TempL.l >> 1) + 1;
		else ReArray[indexB] = TempL.l >> 1;
		ReArray[indexA] = TempReA2;
		ImArray[indexA] = 0; // set Imaginary locations to ‘0’
		ImArray[indexB] = 0;
		indexA = indexB + 1;
		//
	}
	// END OF FIRST STAGE
	while (stage <= NUM_FFT/2)
	{
		indexA = 0;
		sin_index = 0;
		for (g_cnt = 0; g_cnt < group; g_cnt++)
		{
			for (s_cnt = 0; s_cnt < stage; s_cnt++)
			{
				indexB = indexA + stage;
				TempReA = ReArray[indexA];
				TempReB = ReArray[indexB];
				TempImA = ImArray[indexA];
				TempImB = ImArray[indexB];
				// The following first checks for the special cases when the angle “x” is
				// equal to either 0 or pi/2 radians. In these cases, unnecessary
				// multiplications have been removed to improve the processing speed.
				if (sin_index == 0) // corresponds to “x” = 0 radians
				{
					// Calculate new value for ReArray[indexA]
					TempL.l = (long)TempReA + TempReB;
					if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
						 TempReA2 = (TempL.l >> 1) + 1;
					else TempReA2 = TempL.l >> 1;
					// Calculate new value for ReArray[indexB]
					TempL.l = (long)TempReA - TempReB;
					if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
						 TempReB2 = (TempL.l >> 1) + 1;
					else TempReB2 = TempL.l >> 1;
					// Calculate new value for ImArray[indexB]
					TempL.l = (long)TempImA - TempImB;
					if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
						 TempImB = (TempL.l >> 1) + 1;
					else TempImB = TempL.l >> 1;
					// Calculate new value for ImArray[indexA]
					TempL.l = (long)TempImA + TempImB;
					if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
						 TempImA = (TempL.l >> 1) + 1;
					else TempImA = TempL.l >> 1;
				}
				else if (sin_index == NUM_FFT/4) // corresponds to “x” = pi/2 radians
				{
					// Calculate new value for ReArray[indexB]
					TempL.l = (long)TempReA - TempImB;
					if ((TempL.l < 0)&&(0x01 & TempL.b[0]))//
						 TempReB2 = (TempL.l >> 1) + 1;
					else TempReB2 = TempL.l >> 1;
					// Calculate new value for ReArray[indexA]
					TempL.l = (long)TempReA + TempImB;
					if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
						 TempReA2 = (TempL.l >> 1) + 1;
					else TempReA2 = TempL.l >> 1;
					// Calculate new value for ImArray[indexB]
					TempL.l = (long)TempImA + TempReB;
					if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
						 TempImB = (TempL.l >> 1) + 1;
					else TempImB = TempL.l >> 1;
					// Calculate new value for ImArray[indexA]
					TempL.l = (long)TempImA - TempReB;
					if ((TempL.l < 0)&&(0x01 & TempL.b[0]))
						 TempImA = (TempL.l >> 1) + 1;
					else TempImA = TempL.l >> 1;
				}
				else
				{
					// If no multiplication shortcuts can be taken, the SIN and COS
					// values for the Butterfly calculation are fetched from the
					// SinTable[] array.
					if (sin_index > NUM_FFT/4)
					{
						SinVal = SinTable[(NUM_FFT/2) - sin_index];
						CosVal = -SinTable[sin_index - (NUM_FFT/4)];
					}
					else
					{
						SinVal = SinTable[sin_index];
						CosVal = SinTable[(NUM_FFT/4) - sin_index];
					}
					// The SIN and COS values are used here to calculate part of the
					// Butterfly equation
					ReTwid.l = ((long)TempReB * CosVal) +
					((long)TempImB * SinVal);
					ImTwid.l = ((long)TempImB * CosVal) -
					((long)TempReB * SinVal);
					// Using the values calculated above, the new variables
					// are computed
					// Calculate new value for ReArray[indexA]
					TempL.i[0] = 0;    //mod zhw 2007-4-26  TempL.i[1] = 0;  
	                TempL.i[1] = TempReA; //TempL.i[0] = TempReA;
	 				TempL.l = TempL.l >> 1;
					ReTwid.l += TempL.l;
					
					if ((ReTwid.l < 0)&&(ReTwid.i[0])) //mod zhw 2007-4-26  if ((ReTwid.l < 0)&&(ReTwid.i[1]))
						 TempReA2 = ReTwid.i[1] + 1;  //mod zhw 2007-4-26  TempReA2 = ReTwid.i[0] + 1;
					else TempReA2 = ReTwid.i[1];  //mod zhw 2007-4-26  TempReA2 = ReTwid.i[0];
					// Calculate new value for ReArray[indexB]
					TempL.l = TempL.l << 1;
					TempL.l -= ReTwid.l;
					if ((TempL.l < 0)&&(TempL.i[0]))  //mod zhw 2007-4-26  if ((TempL.l < 0)&&(TempL.i[1]))
						 TempReB2 = TempL.i[1] + 1;   //mod zhw 2007-4-26  TempReB2 = TempL.i[0] + 1;
					else TempReB2 = TempL.i[1];  //mod zhw 2007-4-26  TempReB2 = TempL.i[0];
					// Calculate new value for ImArray[indexA]
					TempL.i[0] = 0;  //  mod zhw 2007-4-26  TempL.i[0] = 0;
					TempL.i[1] = TempImA; //mod zhw 2007-4-26  TempL.i[0] = TempImA;
					
					TempL.l = TempL.l >> 1;
					ImTwid.l += TempL.l;
					if ((ImTwid.l < 0)&&(ImTwid.i[0]))  //mod zhw 2007-4-26  	if ((ImTwid.l < 0)&&(ImTwid.i[1]))
						 TempImA = ImTwid.i[1] + 1;  // mod zhw 2007-4-26  TempImA = ImTwid.i[0] + 1;
					else TempImA = ImTwid.i[1];  //mod zhw 2007-4-26  TempImA = ImTwid.i[0];
					// Calculate new value for ImArray[indexB]
					TempL.l = TempL.l << 1;
					TempL.l -= ImTwid.l;
					if ((TempL.l < 0)&&(TempL.i[0]))  //if ((TempL.l < 0)&&(TempL.i[1]))
						 TempImB = TempL.i[1] + 1;  //TempImB = TempL.i[0] + 1;
					else TempImB = TempL.i[1]; // TempImB = TempL.i[0];
				}
				ReArray[indexA] = TempReA2;
				ReArray[indexB] = TempReB2;
				ImArray[indexA] = TempImA;
				ImArray[indexB] = TempImB;
				indexA++;
				sin_index += group;
			} // END of stage FOR loop (s_cnt)
			indexA = indexB + 1;
			sin_index = 0;
		} // END of group FOR loop (g_cnt)
		group /= 2;
		stage *= 2;
	} // END of While loop
	
} // END Int_FFT