恩智浦直立车源代码速度闭环

#include “include.h“
#include  “CollectDate.h“

int nGravityMax=2856;            
int nGravityMin=900;

unsigned int ATD_Data_Buf[21][2]={0}; 
unsigned int ATD_Data_mid[3]={0};
unsigned int GRAVITY_dataAR1_data; 
int VOLTAGE_GRAVITY;
int nGravityOffset;
int VOLTAGE_GYRO;
int g_nGyroscopeOffset;
extern float g_fGravityAngleg_fYGravityAngleg_fCarSpeedAngle=0;
extern float g_fGyroAngleIntegralg_fSpeedControlIntegralg_fGyroAngleSpeedg_fGyroAngle; 
float fDeltaValue;

float angleangle_dot;//互补滤波
float bias_cf;
float dt=0.005;
extern float CarAngle;

float  kai kai1;

void Read_AD（void）    //读取陀螺仪和加速度计AD值
{ 
  unsigned char i;
  for（i=0;i<=20;i++） 
  { 
    ATD_Data_Buf[i][0]=ad_once（ADC0SE8 ADC_12bit）;    //B0 陀螺仪通道  AR1 
    ATD_Data_Buf[i][1]=ad_once（ADC0SE9 ADC_12bit）;    //B1 Z轴加速度计
  }
}

void Get_AD_mid（void）      //中值滤波
{ 
  unsigned char ijkmin;
  unsigned int t;
 
  for（j=0;j<2;j++） 
  {    
    for（i=0;i<20;i++） 
    {
      min=i;   
      for（k=i+1;k<21;k++） 
      {
        if（ATD_Data_Buf[k][j]        {
          min=k;
        }
      }
      if（min!=i）
      {
        t=ATD_Data_Buf[i][j];
        ATD_Data_Buf[i][j]=ATD_Data_Buf[min][j];
        ATD_Data_Buf[min][j]=t;
      }
    }
    
    ATD_Data_mid[j]=ATD_Data_Buf[10][j]; 
  }     
}  


void AVE（void）
{
  int SUM_AR1=0SUM_GRAZ=0;
  u8 i;
  for（i=0;i<10;i++）
  {
    Read_AD（）;
    Get_AD_mid（）;
    SUM_AR1+=ATD_Data_mid[0];   //陀螺仪值
    SUM_GRAZ+=ATD_Data_mid[1];  //加速度值
  }    
    AR1_data=SUM_AR1/10;    //陀螺仪值
    GRAVITY_data=SUM_GRAZ/10;       //加速度值
    //printf（“2=%d
“GRAVITY_data）; 
}

/***********互补滤波****************/

void complement_filter（float angle_m_cffloat gyro_m_cf）
{
    bias_cf*=0.001;			//陀螺仪零飘低通滤波；500次均值；0.998 
    bias_cf+=gyro_m_cf*0.09;		   //0.002
    angle_dot=gyro_m_cf-bias_cf;		   
    angle=（angle+angle_dot*dt）*0.95+angle_m_cf*0.04;	
    //加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；0.90 0.05   
    CarAngle=angle;
}

/*********************Get_ATD_mid**************************/
void AngleCalculate（void） 
{   
    VOLTAGE_GRAVITY=GRAVITY_Receive（）;
    //printf（“%d
“VOLTAGE_GRAVITY1）;   
    //加速度传感器角度输出        
    g_fGravityAngle=（float）（VOLTAGE_GRAVITY-1838）*GRAVITY_ANGLE_RATIO;
    //陀螺仪角速度
    VOLTAGE_GYRO=AR1_Receive（）;
    //VOLTAGE_GYR1=ad_ave（ADC0SE12 ADC_12bit20）;
    //printf（“g_nGyroscopeOffset=%d
“g_nGyroscopeOffset）;
    g_fGyroAngleSpeed=（float）（（VOLTAGE_GYRO-2852）*0.45）;//0.123陀螺仪比例参数021    
    //g_fGyroAngle=g_fGyroAngleSpeed*0.005;   //求陀螺仪比例系数
    g_fGyroAngle=g_fGyroAngleIntegral;
    fDeltaValue = （g_fGravityAngle - g_fGyroAngle）/0.7;                  
    //g_fGyroAngleIntegral+=（（g_fGravityAngle-g_fGyroAngleIntegral）*（1.00+Scan_Key0（））+g_fGyroAngleSpeed）*（0.21+Scan_Key0（））;
    g_fGyroAngleIntegral+=（g_fGyroAngleSpeed+fDeltaValue）*0.005; 
    //complement_filter（g_fGyroAngleIntegralg_f

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2016-03-15 22:02  寒假调直立 - 16-03-15速度闭环
     目录           0  2016-03-15 22:02  寒假调直立 - 16-03-15速度闭环uild
     目录           0  2016-03-15 22:02  寒假调直立 - 16-03-15速度闭环uildconfig files
     文件        2030  2015-03-30 11:51  寒假调直立 - 16-03-15速度闭环uildconfig files128KB_Pflash.icf
     文件        2206  2010-11-12 03:41  寒假调直立 - 16-03-15速度闭环uildconfig files128KB_Pflash_128KB_Dflash.icf
     文件        2035  2012-02-28 15:33  寒假调直立 - 16-03-15速度闭环uildconfig files128KB_Ram.icf
     文件        2030  2010-10-28 03:30  寒假调直立 - 16-03-15速度闭环uildconfig files256KB_Pflash.icf
     文件        2214  2012-03-01 20:12  寒假调直立 - 16-03-15速度闭环uildconfig files256KB_Pflash_256KB_Dflash.icf
     文件        2024  2010-10-28 03:30  寒假调直立 - 16-03-15速度闭环uildconfig files32KB_Ram.icf
     文件        2041  2012-03-01 18:46  寒假调直立 - 16-03-15速度闭环uildconfig files512KB_Pflash.icf
     文件        2217  2010-11-12 03:41  寒假调直立 - 16-03-15速度闭环uildconfig files64KB_Pflash_64KB_Dflash.icf
     文件        2024  2010-10-28 03:30  寒假调直立 - 16-03-15速度闭环uildconfig files64KB_Ram.icf
     文件        3463  2012-03-08 21:04  寒假调直立 - 16-03-15速度闭环uildconfig filesfire_RAM_K60N512.icf
     文件        3441  2012-03-08 21:04  寒假调直立 - 16-03-15速度闭环uildconfig filesfire_ROM_K60N512.icf
     文件         669  2012-03-10 23:38  寒假调直立 - 16-03-15速度闭环uildiar.h
     目录           0  2016-03-15 22:02  寒假调直立 - 16-03-15速度闭环uildled_demo
     目录           0  2016-03-15 22:02  寒假调直立 - 16-03-15速度闭环uildled_demoDebug
     目录           0  2016-03-15 22:02  寒假调直立 - 16-03-15速度闭环uildled_demoDebugExe
     文件       45603  2016-03-12 20:56  寒假调直立 - 16-03-15速度闭环uildled_demoDebugExe.hex
     文件       21994  2015-03-28 10:54  寒假调直立 - 16-03-15速度闭环uildled_demoDebugExefire_gpio_demo.hex
     文件      284536  2016-03-12 20:56  寒假调直立 - 16-03-15速度闭环uildled_demoDebugExefire_gpio_demo.out
     文件       16243  2016-03-12 20:57  寒假调直立 - 16-03-15速度闭环uildled_demoDebugExefire_gpio_demo.sim
     目录           0  2016-03-15 22:02  寒假调直立 - 16-03-15速度闭环uildled_demoDebugList
     文件       34922  2016-03-12 20:56  寒假调直立 - 16-03-15速度闭环uildled_demoDebugListfire_gpio_demo.map
     目录           0  2016-03-15 22:02  寒假调直立 - 16-03-15速度闭环uildled_demoDebugObj
     文件       27156  2015-07-20 11:56  寒假调直立 - 16-03-15速度闭环uildled_demoDebugObjadc.o
     文件       11376  2015-07-20 11:56  寒假调直立 - 16-03-15速度闭环uildled_demoDebugObjalloc.o
     文件       16528  2015-07-20 11:56  寒假调直立 - 16-03-15速度闭环uildled_demoDebugObjarm_cm4.o
     文件        8728  2015-08-12 20:35  寒假调直立 - 16-03-15速度闭环uildled_demoDebugObjassert.o
     文件       37728  2016-03-12 20:56  寒假调直立 - 16-03-15速度闭环uildled_demoDebugObjCollectDate.o
     文件       35772  2016-03-12 20:56  寒假调直立 - 16-03-15速度闭环uildled_demoDebugObjControl.o
............此处省略298个文件信息