/****************************************************************************** 版权所有： 文件名称： fourier.c 文件版本： 01.01 创建作者: sunxi 创建日期： 2008-07-09 功能说明： 傅立叶函数，每个周波采样32点。 其它说明： 1.进行一次计算需要4.47us（ram）。 2.新写法在采样数据4byte对齐的时候需要3.038us（ram），2byte对齐的时候需要3.775us（ram)。 在flash上运行时需要的时间分别为4.438us和5.275us.(52259上） 3.在MCF54418上，只使用acc0，需要1.24us；使用ACC0、ACC1，需要0.781us。 修改记录： */ /*------------------------------- 头文件 -------------------------------------- */ #ifndef WIN32 //VC6.0 中定义的宏 #include "bspconfig.h" #include "rt.h" #include "fourier.h" #else #define CFG_BSP_DEBUG 1 #endif #define sin(x) (x) // sunxi 20220408 由于找不到编译器的数学库，暂时定义一个宏代替。 /*------------------------------- 宏定义 -------------------------------------- */ #if CFG_BSP_DEBUG #define _FOURIER_DEBUG #endif /*------------------------------ 全局变量 ------------------------------------- */ //用int定义代替short定义,确保这个数组4bytes对齐，以保证函数执行效率。 const int g_fourier_coefficient[3][2][16]= { //基波 { { //实部系数?(2/32)*cos(k*1*2*pi/32)*65536 ( 4096<<16)|( 4017&0xffff), ( 3784<<16)|( 3406&0xffff), ( 2896<<16)|( 2276&0xffff), ( 1567<<16)|( 799&0xffff), ( 0<<16)|( -799&0xffff), ((u16)(-1567)<<16)|(-2276&0xffff), ((u16)(-2896)<<16)|(-3406&0xffff), ((u16)(-3784)<<16)|(-4017&0xffff), ((u16)(-4096)<<16)|(-4017&0xffff), ((u16)(-3784)<<16)|(-3406&0xffff), ((u16)(-2896)<<16)|(-2276&0xffff), ((u16)(-1567)<<16)|( -799&0xffff), ( 0<<16)|( 799&0xffff), ( 1567<<16)|( 2276&0xffff), ( 2896<<16)|( 3406&0xffff), ( 3784<<16)|( 4017&0xffff), }, { //虚部系数?-(2/32)*sin(k*1*2*pi/32)*65536 ( 0<<16)|( (u16)(-799)&0xffff), ((u16)(-1567)<<16)|(-2276&0xffff), ((u16)(-2896)<<16)|(-3406&0xffff), ((u16)(-3784)<<16)|(-4017&0xffff), ((u16)(-4096)<<16)|(-4017&0xffff), ((u16)(-3784)<<16)|(-3406&0xffff), ((u16)(-2896)<<16)|(-2276&0xffff), ((u16)(-1567)<<16)|( -799&0xffff), ( 0<<16)|( 799&0xffff), ( 1567<<16)|( 2276&0xffff), ( 2896<<16)|( 3406&0xffff), ( 3784<<16)|( 4017&0xffff), ( 4096<<16)|( 4017&0xffff), ( 3784<<16)|( 3406&0xffff), ( 2896<<16)|( 2276&0xffff), ( 1567<<16)|( 799&0xffff), }, }, //二次谐波 { { //实部系数?(2/32)*cos(k*2*2*pi/32)*65536 ( 4096<<16)|( 3784&0xffff), ( 2896<<16)|( 1567&0xffff), ( 0<<16)|(-1567&0xffff), ((u16)(-2896)<<16)|(-3784&0xffff), ((u16)(-4096)<<16)|(-3784&0xffff), ((u16)(-2896)<<16)|(-1567&0xffff), ( 0<<16)|( 1567&0xffff), ( 2896<<16)|( 3784&0xffff), ( 4096<<16)|( 3784&0xffff), ( 2896<<16)|( 1567&0xffff), ( 0<<16)|(-1567&0xffff), ((u16)(-2896)<<16)|(-3784&0xffff), ((u16)(-4096)<<16)|(-3784&0xffff), ((u16)(-2896)<<16)|(-1567&0xffff), ( 0<<16)|( 1567&0xffff), ( 2896<<16)|( 3784&0xffff), }, { //虚部系数?-(2/32)*sin(k*2*2*pi/32)*65536 ( 0<<16)|(-1567&0xffff), ((u16)(-2896)<<16)|(-3784&0xffff), ((u16)(-4096)<<16)|(-3784&0xffff), ((u16)(-2896)<<16)|(-1567&0xffff), ( 0<<16)|( 1567&0xffff), ( 2896<<16)|( 3784&0xffff), ( 4096<<16)|( 3784&0xffff), ( 2896<<16)|( 1567&0xffff), ( 0<<16)|(-1567&0xffff), ((u16)(-2896)<<16)|(-3784&0xffff), ((u16)(-4096)<<16)|(-3784&0xffff), ((u16)(-2896)<<16)|(-1567&0xffff), ( 0<<16)|( 1567&0xffff), ( 2896<<16)|( 3784&0xffff), ( 4096<<16)|( 3784&0xffff), ( 2896<<16)|( 1567&0xffff), }, }, //五次谐波 { { //实部系数?(2/32)*cos(k*5*2*pi/32)*65536 ( 4096<<16)|( 2276&0xffff), ((u16)(-1567)<<16)|(-4017&0xffff), ((u16)(-2896)<<16)|( 799&0xffff), ( 3784<<16)|( 3406&0xffff), ( 0<<16)|(-3406&0xffff), ((u16)(-3784)<<16)|( -799&0xffff), ( 2896<<16)|( 4017&0xffff), ( 1567<<16)|(-2276&0xffff), ((u16)(-4096)<<16)|(-2276&0xffff), ( 1567<<16)|( 4017&0xffff), ( 2896<<16)|( -799&0xffff), ((u16)(-3784)<<16)|(-3406&0xffff), ( 0<<16)|( 3406&0xffff), ( 3784<<16)|( 799&0xffff), ((u16)(-2896)<<16)|(-4017&0xffff), ((u16)(-1567)<<16)|( 2276&0xffff), }, { //虚部系数?-(2/32)*sin(k*5*2*pi/32)*65536 ( 0<<16)|(-3406&0xffff), ((u16)(-3784)<<16)|( -799&0xffff), ( 2896<<16)|( 4017&0xffff), ( 1567<<16)|(-2276&0xffff), ((u16)(-4096)<<16)|(-2276&0xffff), ( 1567<<16)|( 4017&0xffff), ( 2896<<16)|( -799&0xffff), ((u16)(-3784)<<16)|(-3406&0xffff), ( 0<<16)|( 3406&0xffff), ( 3784<<16)|( 799&0xffff), ((u16)(-2896)<<16)|(-4017&0xffff), ((u16)(-1567)<<16)|( 2276&0xffff), ( 4096<<16)|( 2276&0xffff), ((u16)(-1567)<<16)|(-4017&0xffff), ((u16)(-2896)<<16)|( 799&0xffff), ( 3784<<16)|( 3406&0xffff), }, }, }; /*------------------------------ 外部函数 ------------------------------------- */ /****************************************************************************** 函数名称： fourier32 函数版本： 01.01 创建作者: sunxi 创建日期： 2008-06-26 函数说明： 32点傅立叶算法。此函数中使用的ACC寄存器在中断处理时不会进行压栈处理， 所以整个系统中，不能在不同的中断级别调用这个函数。 注意：d0～d2、a0～a1是scratch register，不需要保存。 d0: 采样点的值 d1: 实部系数值 d2: 实部系数值 a0: 采样点首地址。 a1: 实部首地址。 a2: 虚部首地址。 a3: 返回复数对的地址 参数说明： dots： 采样点数据指针，包含32点的采样数据。在本函数中对采样数据的 地址进行了循环buf的处理，具体做法是将地址和MAC中的MASK 寄存器相与。所以用户必须注意采样点buf的首地址(不是dots 的地址）必须是采样点buf长度（以byte为单位）的2倍。 real_image_pair：函数返回的实部/虚部对。 harmonic: 需要计算的谐波次数，用定义好的宏表示。 返回值： 无 修改记录： */ #if 1 void fourier32(short *dots,short * real_image_pair,unsigned int harmonic) { long a,b,i; short *coefficient; a = 0; b = 0; coefficient = (short *)((char *) harmonic); for(i=0; i<32; i++,dots++) { a += (*dots) * coefficient[i]; b += (*dots) * coefficient[i+32]; } real_image_pair[0] = (short)(a >> 16); real_image_pair[1] = (short)(b >> 16); //已综合考虑正数和负数的情况。 if(a&0x8000) { real_image_pair[0] += 1; } if(b&0x8000) { real_image_pair[1] += 1; } } #else void fourier32(short *dots,struct ri_s16 * ri,unsigned int harmonic) { } #endif void fourier32_half(short *dots,struct ri_s16 * ri,unsigned int harmonic) { } /*------------------------------ 内部函数 ------------------------------------- */ /*------------------------------ 测试函数 ------------------------------------- */ #ifdef _FOURIER_DEBUG //由于系数高低byte的不同，目前这个函数在VC下已不能得到正确结果。 static void fourier32_c(short *dots,short * real_image_pair,unsigned int harmonic) { long a,b,i; short *coefficient; a = 0; b = 0; coefficient = (short *)(harmonic); for(i=0; i<32; i++,dots++) { a += (*dots) * coefficient[i]; b += (*dots) * coefficient[i+32]; } real_image_pair[0] = (short)(a >> 16); real_image_pair[1] = (short)(b >> 16); //已综合考虑正数和负数的情况。 if(a&0x8000) { real_image_pair[0] += 1; } if(b&0x8000) { real_image_pair[1] += 1; } } #ifndef WIN32 #include "ustimer.h" #include "rt.h" #include "bsp.h" #if 1 /* fourier32_c:real=-51,image=-86,us=3934! fourier32(1):real=-51,image=-86,m=100,p=-120.6846,us=427! fourier32(2):real=0,image=0,m=0,p=90.0000,us=427! fourier32(5):real=-234,image=442,m=500,p=117.8720,us=427! fourier32_half(1):real=-51,image=-86,m=100,p=-120.6846,us=330! fourier32_half(2):real=-197,image=12,m=197,p=180.0000,us=330! fourier32_half(5):real=-234,image=442,m=500,p=117.8720,us=330! */ const short g_fourier_dots[] = { -285,-530,-333,149,503,432,13,-365, -356,43,484,579,244,-226,-418,-172, 285,530,333,-149,-503,-432,-13,365, 356,-43,-484,-579,-244,226,418,172, }; #else //amplitude=1111.000000,phase=11.000000,count=32 //RESULT(0):real=212,image=-1091 //RESULT(0):amplitude=1111.406766,phase=79.003488 //RESULT(1):real=832,image=-2060 //RESULT(1):amplitude=2221.671443,phase=68.007031 //RESULT(2):real=4551,image=-3186 //RESULT(2):amplitude=5555.375505,phase=34.994555 const short g_fourier_dots[] = { 5594,7155,3860,-837,-2490,373,4942,6684, 3445,-2542,-6542,-5446,-474,3930,4038,107, -3930,-4041,230,5281,6610,2797,-3206,-6646, -5109,-572,2452,1002,-3646,-7100,-5774,-145, }; #endif #ifndef __KERNEL__ #define PI 3.1415926535897932384626433832795 int fourier_dots(double amp, double phase, int count, short *dots); int fourier_dots(double amp, double phase, int count, short *dots) { int i; double temp; for(i=0;i= 0) { temp += 0.5; } else { temp -= 0.5; } dots[i] = (short)(temp); #if 0 temp = amp*2*sin(phase*2 + i*2*(2*PI)/count); if(temp >= 0) { temp += 0.5; } else { temp -= 0.5; } dots[i] += (short)(temp); #endif temp = amp*5*sin(phase*5 + i*5*(2*PI)/count); if(temp >= 0) { temp += 0.5; } else { temp -= 0.5; } dots[i] += (short)(temp); } return 0; } #endif void fourier_test_get_dots(short * dots); void fourier_test_get_dots(short * dots) { #if __KERNEL__ //在Linux中使用以前的数据，因为没有sin函数供调用。 memcpy(dots,g_fourier_dots,sizeof(g_fourier_dots)); #else // 如果使用半波算法，必须使用生成的数据，不包含二次谐波，否则算不准。 fourier_dots(100.0,100.0,32,(short *)dots); #endif return; } uint32_t _AmXY(long nRe,long nIm); long _AngPQ(long P,long Q); int fourier_test(void) { int i,j; uint32_t us0,us;//,flags; long phase; uint32_t magnitude; struct ri_s16 ri = { 0,0 }; int harmonic[3][2] = { {1,0}, {2,0}, {5,0}, }; harmonic[0][1] = FOURIER_HARMONIC_1; harmonic[1][1] = FOURIER_HARMONIC_2; harmonic[2][1] = FOURIER_HARMONIC_5; //fourier32_c rt_irq_save(flags); fourier_test_get_dots((short *)g_adc_dots); // 打印采样点 rt_printf("const short g_fourier_dots[] =\r\n{\t"); for(i=0;i<32;i++) { if(i%8 == 0) rt_printf("\r\n\t"); rt_printf("%d,",((short*)g_adc_dots)[i]); } rt_printf("\r\n};\r\n"); us0 = ustimer_get_origin(); for(i=0;i<1000;i++) { fourier32_c((short *)g_adc_dots,(s16 *)&ri,FOURIER_HARMONIC_1); } us = ustimer_get_duration(us0); rt_irq_restore(flags); rt_printf("fourier32_c:real=%d,image=%d,us=%lu!\r\n",\ ri.r,ri.i,us); //fourier32 #if 1 for(j=0;j<128;j++) { rt_irq_save(flags); fourier_test_get_dots((short *)g_adc_dots); fourier_test_get_dots((short *)g_adc_dots + 32); fourier_test_get_dots((short *)g_adc_dots + 64); fourier_test_get_dots((short *)g_adc_dots + 96); us0 = ustimer_get_origin(); for(i=0;i<1;i++) { fourier32((short *)g_adc_dots +j,(short *)&ri,(unsigned int)harmonic[0][1]); } us = ustimer_get_duration(us0); rt_irq_restore(flags); magnitude = _AmXY(ri.r,ri.i); phase = _AngPQ(ri.r,ri.i); rt_printf("fourier32(%d):real=%d,image=%d,m=%lu,p=%s,us=%lu!\r\n",\ j,ri.r,ri.i,magnitude,q16_to_str(phase),us); } ustimer_delay(USTIMER_SEC*3); // 半波傅氏测试 //fourier32_half for(j=0;j<128;j++) { rt_irq_save(flags); fourier_test_get_dots((short *)g_adc_dots); fourier_test_get_dots((short *)g_adc_dots + 32); fourier_test_get_dots((short *)g_adc_dots + 64); fourier_test_get_dots((short *)g_adc_dots + 96); us0 = ustimer_get_origin(); for(i=0;i<1;i++) { fourier32_half((short *)g_adc_dots +j,&ri,(unsigned int)harmonic[0][1]); } us = ustimer_get_duration(us0); rt_irq_restore(flags); magnitude = _AmXY(ri.r,ri.i); phase = _AngPQ(ri.r,ri.i); rt_printf("fourier32_half(%d):real=%d,image=%d,m=%lu,p=%s,us=%lu!\r\n",\ j,ri.r,ri.i,magnitude,q16_to_str(phase),us); } #else for(j=0;j<3;j++) { rt_irq_save(flags); fourier_test_get_dots((short *)g_adc_dots); us0 = ustimer_get_origin(); for(i=0;i<1000;i++) { fourier32((short *)g_adc_dots,real_image_pair,(unsigned int)harmonic[j][1]); } us = ustimer_get_duration(us0); rt_irq_restore(flags); magnitude = _AmXY(real_image_pair[0],real_image_pair[1]); phase = _AngPQ(real_image_pair[0],real_image_pair[1]); rt_printf("fourier32(%d):real=%d,image=%d,m=%lu,p=%s,us=%lu!\r\n",\ harmonic[j][0],real_image_pair[0],real_image_pair[1],magnitude,q16_to_str(phase),us); } // 半波傅氏测试 //fourier32_half for(j=0;j<3;j++) { rt_irq_save(flags); fourier_test_get_dots((short *)g_adc_dots); us0 = ustimer_get_origin(); for(i=0;i<1000;i++) { fourier32_half((short *)g_adc_dots,real_image_pair,(unsigned int)harmonic[j][1]); } us = ustimer_get_duration(us0); rt_irq_restore(flags); magnitude = _AmXY(real_image_pair[0],real_image_pair[1]); phase = _AngPQ(real_image_pair[0],real_image_pair[1]); rt_printf("fourier32_half(%d):real=%d,image=%d,m=%lu,p=%s,us=%lu!\r\n",\ harmonic[j][0],real_image_pair[0],real_image_pair[1],magnitude,q16_to_str(phase),us); } #endif rt_printf("\r\n"); // ustimer_delay(5*USTIMER_SEC); return 0; } #else // ***************以下程序在VC中调用，不能在嵌入式环境中使用*********************** #include "stdio.h" #include "math.h" #define PI 3.1415926535897932384626433832795 #define AMPLITUDE 1111.0 #define PHASE 11.0 #define COUNT 32 double g_fourier_real32_double[3][32]; double g_fourier_image32_double[3][32]; short g_fourier_coefficient_short[3][2][32]; int aaaa[] = { (int)(2.25*65536),(int)(-2.25*65536),(int)(2.75*65536),(int)(-2.75*65536), }; int fourier_dots(double amp, double phase, int count, short *dots) { int i; double temp; for(i=0;i= 0) { temp += 0.5; } else { temp -= 0.5; } dots[i] = (short)(temp); temp = amp*2*sin(phase*2 + i*2*(2*PI)/count); if(temp >= 0) { temp += 0.5; } else { temp -= 0.5; } dots[i] += (short)(temp); temp = amp*5*sin(phase*5 + i*5*(2*PI)/count); if(temp >= 0) { temp += 0.5; } else { temp -= 0.5; } dots[i] += (short)(temp); } return 0; } int fourier_dots_double(double amp, double phase, int count, double *dots) { int i; for(i=0;i=0 ? d + 0.5 : d - 0.5; g_fourier_coefficient_short[0][0][i] = (short)d; d = -sin(11.25*i*1*PI/180)/16*65536; d = d>=0 ? d + 0.5 : d - 0.5; g_fourier_coefficient_short[0][1][i] = (short)d; d= cos(11.25*i*2*PI/180)/16*65536; d = d>=0 ? d + 0.5 : d - 0.5; g_fourier_coefficient_short[1][0][i] = (short)d; d = -sin(11.25*i*2*PI/180)/16*65536; d = d>=0 ? d + 0.5 : d - 0.5; g_fourier_coefficient_short[1][1][i] = (short)d; d = cos(11.25*i*5*PI/180)/16*65536; d = d>=0 ? d + 0.5 : d - 0.5; g_fourier_coefficient_short[2][0][i] = (short)d; d= -sin(11.25*i*5*PI/180)/16*65536; d = d>=0 ? d + 0.5 : d - 0.5; g_fourier_coefficient_short[2][1][i] = (short)d; } } void fourier1_sample32_double(double *p,double * real_image_pair,int harmonic) { double a,b; long i; a = 0; b = 0; for(i=0; i<32; i++,p++) { a += (*p) * g_fourier_real32_double[harmonic][i]; b += (*p) * g_fourier_image32_double[harmonic][i]; } real_image_pair[0] = (double)a/65536; real_image_pair[1] = (double)b/65536; return; } void print_array(FILE *file,short * p) { int i,j; for(i=0; i<4; i++) { fprintf(file,"\t"); for(j=0;j<4; j++) { fprintf(file,"\t(% 5d<<16)|(% 5d&0xffff),",*p,*(p+1)); // fprintf(file,"\t% 5d,% 5d,",*p,*(p+1)); p += 2; } fprintf(file,"\n"); } } short g_test_dots[32]= { //15152, 13584, 12032, 10632, 9336, 8296, 7536, 7040, 6856, 7008, 7440, 8160, 9144, 10352, 11792, 13288, 14880, 16472, 18040, 19440, 20720, 21736, 22512, 23008, 23200, 23064, 22648, 21880, 20888, 19664, 18264, 16752 22032, 22704, 23112, 23168, 22936, 22448, 21640, 20512, 19240, 17816, 16248, 14656, 13080, 11568, 10160, 9016, 8040, 7344, 6952, 6880, 7120, 7648, 8440, 9464, 10800, 12232, 13792, 15376, 16992, 18480, 19848, 21088 }; int fourier_test_short(void) { short dots[32]; short real_image_pair[2]; double amp,phase,amp_percent,phase_percent; FILE *file; int i; fourier_coefficient_short(); // fourier_dots(AMPLITUDE,PHASE*(PI/180),32,dots); for(i=0;i<32;i++) { dots[i] = g_test_dots[i]; } file = fopen("f:/fourierdata.c","w"); fprintf(file,"//amplitude=%f,phase=%f,count=%d\n",AMPLITUDE,PHASE,COUNT); for(i=0;i<3;i++) { fourier32_c(dots,real_image_pair,i*sizeof(g_fourier_coefficient[0])); amp = sqrt((double)(real_image_pair[0]*real_image_pair[0] + real_image_pair[1]*real_image_pair[1])); amp_percent = (amp - AMPLITUDE)/AMPLITUDE; phase = atan((double)real_image_pair[1]/real_image_pair[0])*360/(2*PI); phase_percent = (phase - PHASE)/PHASE; fprintf(file,"//RESULT(%d):real=%d,image=%d\n",i,real_image_pair[0],real_image_pair[1]); fprintf(file,"//RESULT(%d):amplitude=%f,phase=%f\n",i,amp,phase); } //输出点 fprintf(file,"const short g_fourier_dots[] =\n{\n"); for(i=0;i