nihe.c

完成两路带有三差信号的拟合输出

#include"F206_Addr.h"
#include"FLOAT.h" 

       
#include"ioports.h"
#include<math.h>
#include<stdio.h>
#include<stdlib.h>

/*#define IOSR   0xFFF6h*/  
/*#define IOSR_VAL 0x0004h*/ 
/*#define ASPCR 0xFFF5h*/
/*#define ASPCR_UAL 0xE003h*/

/*ioport unsigned port0000; */
float c[5][32], d[32][1], dd[5][1],a2[32],b2[32],x[5];
float a,g,p,q,cc,dd2,tt;
int i,j,k,m,sp;
 
float aa[5][5];
float ddd[5][1];
float a1[32]={2.4300,2.4299,2.4298,2.4295,2.4290,2.4285,2.4278,2.4270,2.4261,2.4251,
               2.4239,2.4227,2.4213,2.4197,2.4181,2.4164,2.4145,2.4125,2.4104,2.4081,
               2.4058,2.4033,2.4007,2.3980,2.3952,2.3922,2.3892,2.3860,2.3827,2.3793,
               2.3757,2.3721};
       
float b1[32]={1.6000,1.6086,1.6172,1.6258,1.6342,1.6427,1.6510,1.6593,1.6676,
      1.6758,1.6840,1.6920,1.7001,1.7080,1.7160,1.7238,1.7316,1.7393,
      1.7470,1.7546,1.7621,1.7695,1.7769,1.7843,1.7915,1.7987,1.8058,
      1.8129,1.8199,1.8268,1.8336,1.8404}; 

/*interrupt void read_AD(void)
{
 for(sp=0;sp<32;sp++)
 {while((IOSR&0x4)==4)
  a1[sp]=port0000;
  while((IOSR&0x4)==0) {};
   } 
}   */
main()
{
 while(1) 
 {
  asm(" setc intm "); 
  
     /*volatile int ASPCR=(volatile int )0xFFF5h;*/
    /**(volatile int *)ASPCR=0xE003h; */                         
   /*  *(volatile int *)IOSR=0xFFF6h;             /* *(volatile int*)*/

/* for(sp=0;sp<32;sp++)
 {while((IOSR&0x4h)==4)
  a1[sp]=port0000;
  while((IOSR&0x4)==0) {};
   } */
  	 
   
 /*  while(1)
		
	a1[0]=ADPORT;
	while(1)
		{   
		
	for(sp=1;sp<32;sp++)
	{
		
		a1[sp]=ADPORT; }  }  */
 /* a1[32]={2.4300,2.4299,2.4298,2.4295,2.4290,2.4285,2.4278,2.4270,2.4261,2.4251,
               2.4239,2.4227,2.4213,2.4197,2.4181,2.4164,2.4145,2.4125,2.4104,2.4081,
               2.4058,2.4033,2.4007,2.3980,2.3952,2.3922,2.3892,2.3860,2.3827,2.3793,
               2.3757,2.3721};
  b1[32]={1.6000,1.6086,1.6172,1.6258,1.6342,1.6427,1.6510,1.6593,1.6676,
      1.6758,1.6840,1.6920,1.7001,1.7080,1.7160,1.7238,1.7316,1.7393,
      1.7470,1.7546,1.7621,1.7695,1.7769,1.7843,1.7915,1.7987,1.8058,
      1.8129,1.8199,1.8268,1.8336,1.8404};  */       
 for (i=0;i<=31;i++)
  { c[0][i]=b1[i]*b1[i];
    c[1][i]=a1[i]*b1[i];
    c[2][i]=a1[i];
    c[3][i]=b1[i];
    c[4][i]=1;
   }

 for(i=0;i<32;i++)
  {d[i][1]=a1[i]*a1[i];
    }


 for(j=0;j<5;j++)
  {for(k=0;k<5;k++)
   {for(m=0;m<32;m++)
    {cc=c[j][m]*c[k][m];
      aa[j][k]=aa[j][k]+cc;}
      }
   }

 for(i=0;i<5;i++)
 {for(j=0;j<32;j++)
  {dd2=c[i][j]*dd[j][1];
   ddd[i][1]=ddd[i][1]+dd2;
    }
   }
/*高斯迭代求解*/

for(i=0;i<5;i++)
{if(aa[i][i]==0)
   aa[i][i]=0.001;
}
for(k=0;k<4;k++)
{for(i=k+1;i<5;i++)
  {aa[i][k]=-aa[i][k]/aa[k][k];
   ddd[i][1]=ddd[i][1]+aa[i][k]*ddd[k][1];
   for(j=k+1;j<5;j++)
    {aa[i][j]=aa[i][j]+aa[i][k]*aa[k][j];
      }
   }
}

x[4]=ddd[4][1]/aa[4][4];
for(k=3;k>=0;k--)
{tt=0;
  for(j=k+1;j<5;j++)
   {tt=aa[k][j]*x[j]+tt;
     }
 x[k]=(ddd[k][1]-tt)/aa[k][k];
}




a=asin(-x[1]/sqrt(-4*x[0]));

g=sqrt(-x[0]);
p=(2*x[0]*x[2]-x[3]*x[1])/(x[1]*x[1]+4*x[0]);
q=(-2*x[3]-x[2]*x[1])/(x[1]*x[1]+4*x[0]);




for(i=0;i<32;i++)
{a2[i]=a1[i]-p;
}
for(i=0;i<32;i++)
{b2[i]=(a2[i]*sin(a)+g*(b1[i]-q))/cos(a);
}

}

}      









/**************************************************************
* 本程序根据Heydemann算法对带有三差的数据进行补偿             *
* AD转换由外部信号开启，dsp根据IO2口的电平变化判断是否接收数据*
* 对每采集的32个数据进行拟合，纠正三差误差，输出32个数据      *
***************************************************************/


#include"F206_Addr.h"               /*定义寄存器地址：GREG为0x0005，IOSR为portFFF6，ASPCR为portFFF5，ADPORT为port0000*/
#include"FLOAT.h" 
#include"ioports.h"                 /*定义了访问IO口的两个宏和相关函数，inport和outport宏是访问I/O口最方便的方法*/
#include<math.h>                    /*定义了三角，指数，双曲等数学函数，三角函数以弧度作为单位。*/
#include<stdio.h>
#include<stdlib.h>                  /*说明了多个函数，宏，和数据类型*/

/*.....................................定义变量...................................*/

float c[5][32], d[32][1], dd[5][1],a2[32],b2[32],x[5];   /* 定义算法中的变量，类型为浮点型*/
float a,g,p,q,cc,dd2,tt;
int i,j,k,m,sp;
 
float aa[5][5];                      /*存放矩阵c和c的转秩的乘积，为5×5的矩阵*/
float ddd[5][1];                     /*存放矩阵c和y相乘的结果，为5×1的矩阵*/
float a1[32];                        /*存放AD转换后输入的32个数*/
float b1[32];                        /*存放AD转换后输入的32个数*/

/*.....................................主函数......................................*/   

main()                                          
{
 while(1)                            /*循环条件为一，是程序循环运行*/
 {
  asm("setc intm");                  /*屏蔽中断*/
     
  /* *(volatile int *)ASPCR=0xE003h; */   /*ASPCR复位值为0x0000h，此时IO2配置为输入，可不必设置*/                       

/*...................................数据输入并按算法赋值.................................*/
   
  for(sp=0;sp<32;sp++)
    {while((IOSR&0x0004h)==4)         /*如果IO2为高电平，进行ad数据采集*/
       a1[sp]=ADPORT;
     while((IOSR&0x0004)==0) {};      /*如果IO2为低电平，不操作*/
    }

  for (i=0;i<=31;i++)                 /* 赋值，如式2-16*/
    { c[0][i]=b1[i]*b1[i];
      c[1][i]=a1[i]*b1[i];
      c[2][i]=a1[i];
      c[3][i]=b1[i];
      c[4][i]=1;
    }

  for(i=0;i<32;i++)                   /* 赋值，如式2-16*/
    {d[i][1]=a1[i]*a1[i];
    }

  for(j=0;j<5;j++)                    /*求c和c转秩的乘积，得到5*5矩阵,存放到aa[5][5]中*/
    {for(k=0;k<5;k++)
      {for(m=0;m<32;m++)
        {cc=c[j][m]*c[k][m];
         aa[j][k]=aa[j][k]+cc;
        }
      }
    }

  for(i=0;i<5;i++)                    /*求c和y的积，得到5*1矩阵，存放到ddd[5][1]*/
    {for(j=0;j<32;j++)
      {dd2=c[i][j]*dd[j][1];
       ddd[i][1]=ddd[i][1]+dd2;
      }
    }

/*.........................................高斯迭代求解...................................*/

  for(i=0;i<5;i++)
    {if(aa[i][i]==0)                  /*排除对角线上数据为零的情况*/
     aa[i][i]=0.001;
    }

  for(k=0;k<4;k++)                    /*进行高斯迭代，化为上三角行列式*/
    {for(i=k+1;i<5;i++)               /*从第一行开始，将第一列的2--5元素消去，然后从第二行开始，消去第二列的3--5*/                  {aa[i][k]=-aa[i][k]/aa[k][k];   /*依此类推，化为上三角行列式*/
       ddd[i][1]=ddd[i][1]+aa[i][k]*ddd[k][1];
       for(j=k+1;j<5;j++)
        {aa[i][j]=aa[i][j]+aa[i][k]*aa[k][j];
        }
      }
    }

  x[4]=ddd[4][1]/aa[4][4];            /*求出上三角行列式最后一行的变量，即求出x[4]*/
  for(k=3;k>=0;k--)                   /*反向迭代求出x[0],x[1],x[2],x[3]*/
    {tt=0;
     for(j=k+1;j<5;j++)
      {tt=aa[k][j]*x[j]+tt;
      }
     x[k]=(ddd[k][1]-tt)/aa[k][k];
    }

/*.......................................按照Heydemann算法求出纠正值a，g，p，q............................*/

  a=arcsin(-x[1]/sqrt(-4*x[0]));             
  g=sqrt(-x[0]);
  p=(2*x[0]*x[2]-x[3]*x[1])/(x[1]*x[1]+4*x[0]);
  q=(-2*x[3]-x[2]*x[1])/(x[1]*x[1]+4*x[0]);

/*...............................对输入的32个数据进行误差补偿，存放到数组a2[32],b2[32]中......................*/

  for(i=0;i<32;i++)                    
    {a2[i]=a1[i]-p;
    }
  for(i=0;i<32;i++)
    {b2[i]=(a2[i]*sin(a)+g*(b1[i]-q))/cos(a);
    }
  }
}