根据malc《【翻译】加速度计和陀螺仪指南 》写的mpu6050程序·
本帖最后由 cyq456 于 2013-8-24 21:02 编辑看了这贴http://www.geek-workshop.com/thread-1695-1-1.html都有几天了，有所领悟，所以根据http://starlino.com/imu_kalman_arduino.html这贴写了一个mpu6050可以用的程序。
————————————————分割线—————————————————————————————————
其中RwEst 为处理后x轴的加速度， RwEst 为处理后y轴的加速度 ， RwEst 为处理后z轴的加速度。
处理后的加速度可以直接用于求角度。
想求角度可以参考这贴http://www.geek-workshop.com/for ... 328&page=1#pid27876
代码：
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
#define PI 3.14159265358979f
#define INPUT_COUNT 5
#define Gz_offset 0.30
#define Gy_offset 1.09
#define Gx_offset -1.94
int16_t ax, ay, az;
int16_t gx, gy, gz;
int an;
char firstSample;
float wGyro;
float RwEst;
unsigned long lastMicros;
unsigned long interval;
float RwAcc;
float RwGyro;
float Awz;
bool blinkState = false;
void setup()
{
Serial.begin(38400);
Wire.begin();
accelgyro.initialize();
wGyro=10;
firstSample=1;
}
void loop()
{
getEstimatedInclination();
Serial.print(interval);
Serial.print(",");
Serial.print(RwAcc);//Inclination X axis (as measured by accelerometer)
Serial.print(",");
Serial.print(RwEst);//Inclination X axis (estimated / filtered)
Serial.print(",");
Serial.print(RwAcc);//Inclination Y axis (as measured by accelerometer)
Serial.print(",");
Serial.print(RwEst);//Inclination Y axis (estimated / filtered)
Serial.print(",");
Serial.print(RwAcc);//Inclination Z axis (as measured by accelerometer)
Serial.print(",");
Serial.print(RwEst);//Inclination Z axis (estimated / filtered)
Serial.println("");
blinkState = !blinkState;
}
void getEstimatedInclination()
{
static int i,w;
static float tmpf,tmpf2;
static unsigned long newMicros;
static char signRzGyro;
newMicros=millis();
Get_an();
interval=newMicros-lastMicros;
lastMicros = newMicros;
for(w=0;w<=2;w++) RwAcc=getInput(w);
normalize3DVector(RwAcc);
if (firstSample)
{
for(w=0;w<=2;w++) RwEst=RwAcc; //initialize with accelerometer readings
}
else
{
if(abs(RwEst)<0.1)
{
for(w=0;w<=2;w++) RwGyro=RwEst;
}
else
{
for(w=0;w<=1;w++)
{
tmpf=getInput(3+w);
tmpf *= interval / 1,000.0f;
Awz = atan2(RwEst,RwEst) * 180 /PI;
Awz += tmpf;
}
signRzGyro=(cos(Awz*PI/180)>=0)?1:-1;
for(w=0;w<=1;w++)
{
RwGyro = sin(Awz*PI/180);
RwGyro/=sqrt(1+squared(cos(Awz*PI/180))*squared(tan(Awz*PI/180)) );
}
RwGyro=signRzGyro*sqrt(1-squared(RwGyro)-squared(RwGyro));
}
for(w=0;w<=2;w++) RwEst = (RwAcc + wGyro* RwGyro)/(1 + wGyro);
normalize3DVector(RwEst);
}
firstSample = 0;
}
float squared(float x)
{
return x*x;
}
float getInput(char i)
{
static float tmpf;
if(i<=2) tmpf=an/16384.0;
else if(i==3) tmpf=an/131.0-Gx_offset;
else if(i==4) tmpf=an/131.0-Gy_offset;
else tmpf=an/131.0-Gz_offset;
return tmpf;
}
void normalize3DVector(float* vector)
{
static float R;
R = sqrt(vector*vector+vector*vector+vector*vector);
vector /= R;
vector /= R;
vector /= R;
}
void Get_an()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
an=ax;
an=ay;
an=az;
an=gx;
an=gy;
an=gz;
}
调试效果怎样？？？ 学慧放弃 发表于 2013-8-25 09:59
调试效果怎样？？？
比没处理的数据稳定多了 cyq456 发表于 2013-8-25 23:12 static/image/common/back.gif
比没处理的数据稳定多了
滤波效果怎样？ cyq456 发表于 2013-8-25 23:12 static/image/common/back.gif
比没处理的数据稳定多了
滤波效果怎样？ 请问这个可以用在51单片机上吗？:) 想問#define Gz_offset 0.30
#define Gy_offset 1.09
#define Gx_offset -1.94
GX GY GZ這三個值是如何得出來的
页:
[1]