智能车制作

标题: 求助加速度计跟陀螺仪的调试。 [打印本页]

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

程序是这样的：
#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 时间: 2015-3-22 14:22
你把x轴缩小一下

作者: 喜欢阳光的猪 时间: 2015-3-26 23:35

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

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

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

作者: 喜欢阳光的猪 时间: 2015-3-30 11:28

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

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

作者: wy941016 时间: 2015-3-30 14:49
哦哦，我多看看吧，关键不太理解。。谢谢啦

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

作者: 喜欢阳光的猪 时间: 2015-4-8 21:33

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

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

欢迎光临智能车制作 (http://dns.znczz.com/)