求助加速度计跟陀螺仪的调试。

喜欢阳光的猪

前几天调试加速度计跟陀螺仪模块，选用的清华直立方案，对陀螺仪的输出换算成角度积分，然后在Visualscope上面调试，但是输出的波形是这样的。表示很不解？我看到其他队的都是很连续的波形，算法没有问题的话，那一定在程序上了。希望哪位大神指点指点，感激不尽。（图里面Ch1是角速度，Ch2是加速度计的角度，Ch3是处理后的最终的角度）


程序是这样的：
#include "main.h"
#define acce_zero 1310
#define gyro_zero 1500
//#define   LED1       PORTK_PK7
//#define   LED1_DIR   DDRK_DDRK7
unsigned char i=0,j=0,Timerflag1ms=0;
unsigned int ad_acceler=0,ad_gyro=0,av_ad_acceler=0,av_ad_gyro=0;
float gyro_angle_intergral=0,accer_angle=0,gyro_speed=0;
static float acceler_rate=0.125,gyro_rate=0.235;
void PLL_32M(void)                      //BUS CLOCK = 32MHZ, PLL CLOCK = 64MHZ
{
    CLKSEL &= 0x7f;                     //set OSCCLK as sysclk
    PLLCTL &= 0x8F;                     //Disable PLL circuit   
    CRGINT &= 0xDF;
   
    SYNR = 0x43;                             
    REFDV = 0x81;                       //PLLCLK=2×OSCCLK×(SYNR+1)/(REFDV+1)＝64MHz ,fbus=32M
   
    PLLCTL = PLLCTL | 0x70;             //Enable PLL circuit
    asm NOP;
    asm NOP;
    while(!(CRGFLG&0x08));              //PLLCLK is Locked already
    CLKSEL |= 0x80;                     //set PLLCLK as sysclk
}
/****************************初始化设备**************************************/
void DeviceInit()
{
  
PLL_32M() ;
UART_Init();
}
void PIT_inits(void)                    //定时间断初始化函数 1ms定时间断设置
{
  PITCFLMT_PITE=0;                      //关PIT
  PITCE_PCE0=1;                         //定时器通道0使能
  PITMTLD0=32-1;                        //8位定时器初值设定,32分频，在32MHzBusClock下，为1MHz。即1us
  PITLD0=1000-1;                        //16位定时器初值设定。1000*1us=1ms
  PITINTE_PINTE0=1;                     //定时器间断通道0间断使能
  PITCFLMT_PITE=1;                      //使能PIT
}
/*************************************************************/
/*                         延时函数                          */
/*************************************************************/
void AD_GYRO_ACC_Init()
{
ATD0DIEN=0x00;//禁止数字输入
ATD0CTL0=0x02;//从0通道开始采集到2通道返回
ATD0CTL1=0x40;//无外部触发，12位，不放电
ATD0CTL2=0x40;//快速清零，禁止外部触发，禁止比较中断
ATD0CTL3=0x98;//右对齐，转换序列3，非FIFO模式
ATD0CTL4=0x01;//采样时间4个AD周期
ATD0CTL5=0x30;//禁止特殊通道，AD连续转换，通道0开始
}
void AD_measure()
{
   for(i=0;i<16;i++)
   {
      while(!ATD0STAT2_CCF2);
      ad_acceler+=ATD0DR2;
      while(!ATD0STAT2_CCF1);
      ad_gyro+=ATD0DR1;
   }
   av_ad_acceler= ad_acceler>>4;
   av_ad_gyro= ad_gyro>>4;
}   
/*void AD_measure()
{
   while(!ATD0STAT2_CCF2);
   ad_acceler+=ATD0DR2;
   while(!ATD0STAT2_CCF1);
   ad_gyro+=ATD0DR1;
} */
void caculate()
{
   float deltaangle;
   gyro_speed=gyro_rate*(av_ad_acceler-gyro_zero);
   accer_angle=acceler_rate*(av_ad_gyro-acce_zero);
   deltaangle=(accer_angle-gyro_angle_intergral)*200;
   gyro_angle_intergral+=(gyro_angle_intergral+ gyro_speed)/200;
         
}
void main(void)
{
  
  DisableInterrupts;                    
  DeviceInit();
  PIT_inits();
  AD_GYRO_ACC_Init();
  EnableInterrupts;
  uart_putstr("Usart Is Working!");
//  LED1_DIR = 1;                         //PK2口设置为输出
// LED1 = 0;                             //初始化输出低电平
  for(;;)
  {  
    caculate();
    OutData[0] = gyro_speed*1000;
    OutData[1] = accer_angle*1000;
    OutData[2] = gyro_angle_intergral;
    OutData[3] = 0;
      
    OutPut_Data();               
  }
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 PIT0(void)  //1ms
{
  //中断执行程序
   PITTF_PTF0 = 1;   //清中断标志位   
   AD_measure();               
}

▓追梦﹏forever

你把x轴缩小一下

喜欢阳光的猪

▓追梦﹏forever 发表于 2015-3-22 14:22
你把x轴缩小一下

3q3q,解决了呢，滤波的算法出问题了还有一点程序上的语法问题，总算弄出来了。

wy941016

我用的128芯片，一直测不到输出波形，请问是用串口调试的嘛？

喜欢阳光的猪

wy941016 发表于 2015-3-30 11:14
我用的128芯片，一直测不到输出波形，请问是用串口调试的嘛？

嗯，sci通信，发送到这个上位机。看到很多这种帖子，Visualscope的。

wy941016

哦哦，我多看看吧，关键不太理解。。 谢谢啦

彼时年少

请问一下gyro_rate这个参数代表什么。。表示自己手上也有程序，但看不懂。。

喜欢阳光的猪

彼时年少 发表于 2015-4-6 13:07
请问一下gyro_rate这个参数代表什么。。表示自己手上也有程序，但看不懂。。

就是从ad采集过来的值，还要把它转换会实际的模拟值，这个可以根据加速度计的精度跟ad的位数来算，或者通过实验的方式来测（把它认为是线性的情况）。