智能车制作

标题: 得到了角速度，想化出来角度，但是根据别人的程序改编后有问题 [打印本页]

作者: 黄某 时间: 2015-11-8 21:14
标题: 得到了角速度，想化出来角度，但是根据别人的程序改编后有问题
#include "common.h"
#include "include.h"
#include "MPU6050.h"
#include "MK60_i2c.h"
#include "math.h"
#include "MK60_lptmr.h"

long LastTime=0,NowTime,TimeSpan;//用来对角速度积分

//====定义了陀螺仪的偏差===========
#define Gx_offset -1.196
#define Gy_offset -0.1157
#define Gz_offset 0.9125
#define pi 3.141592653
//====================
//MPU6050 accelgyro;

int16_t ax,ay,az;
int16_t gx,gy,gz;//存处原始数据
float aax,aay,aaz,ggx,ggy,ggz;//存储量化后的数据
float Ax,Ay,Az;//单位 g(9.8m/s^2)
float Gx,Gy,Gz;//单位°/s

float Angle_accX,Angle_accY,Angle_accZ;//存储加速度计算出的角度

int main(void)
{
  //时钟启动
  lptmr_time_start_ms();
/*陀螺仪传感器初始化*/
  MPU6050_init();

while(1)
{

ax=MPU6050_GetDoubleData(MPU6050_ACCEL_XOUT);
ay=MPU6050_GetDoubleData(MPU6050_ACCEL_YOUT);
az=MPU6050_GetDoubleData(MPU6050_ACCEL_ZOUT);
gx=MPU6050_GetDoubleData(MPU6050_GYRO_XOUT);
gy=MPU6050_GetDoubleData(MPU6050_GYRO_YOUT);
gz=MPU6050_GetDoubleData(MPU6050_GYRO_ZOUT);

//======对加速度进行量化，得出单位为g的加速度值-2g量程
Ax=ax/16384.00;
Ay=ay/16384.00;
Az=az/16384.00;
//=====是用加速度计算三个轴和水平面坐标系之间的夹角
Angle_accX= atan(Ax / Az)*180/pi;    //加速度仪，反正切获得弧度值，乘以180度除以π
Angle_accY= atan(Ay / Az)*180/ pi; //获得角度值，乘以-1得正
//==========以下三行是对角速度做量化-2000°量程==========
ggx=gx/16.38;
ggy=gy/16.38;
ggz=gz/16.38;

  //===============以下是对角度进行积分处理================
  NowTime=lptmr_time_get_ms();//获取当前程序运行的毫秒数
  TimeSpan=NowTime-LastTime;//积分时间这样算不是很严谨
  //下面三行是通过对角速度积分实现各个轴的角度测量，当然假设各轴的起始角度都是0
  Gx=Gx+(ggx-Gx_offset)*TimeSpan/1000;
  Gy=Gy+(ggy-Gy_offset)*TimeSpan/1000;
  Gz=Gz+(ggz-Gz_offset)*TimeSpan/1000;

  LastTime=NowTime;

  //==============================
printf("\r\n*****GYRO_accX=********%f\r\n",Gx);
printf("\r\n*****GYRO_accY=********%f\r\n",Gy);
printf("\r\n*****GYRO_accZ=********%f\r\n",Gz);

DELAY_MS(500);
}

}

这个程序有问题，但我不知道有哪些错误！  只知道一个地方
ggx=gx/16.38;
ggy=gy/16.38;
ggz=gz/16.38;  int 型和float型相除，这有问题  这个程序是一个大神用在32上的，我改成了K60的没变多少！

欢迎光临智能车制作 (http://111.231.132.190/)