MPU6050 角度問題請教
各位前輩好小弟目前使用mpu6050 做四軸飛行器
角度值雖然得到了,但遇到了一些問題想請教
算出來的pitch值 (往角度小的方向轉動) ,快速的小小轉動一下mpu6050
發現它的數值pitch會瞬間先變大,再變回當下正確的角度值
有疑惑的是,roll的數值卻是正常的....不會有值突然增大的現象
拜託前輩們及神人們指點!!
附上程式碼
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
long LastTime, NowTime, TimeSpan;
float Ax,Ay,Az;
float X,Y,Z; // angle from acclerometer
float Gyro_x, Gyro_y,Gyro_z; // angle accleration from gyro sensor
float ix,iy,iz;
float Roll,Pitch,Yaw;
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
void setup()
{
Wire.begin();
Serial.begin(9600);
accelgyro.initialize();
}
void loop()
{
angle();
}
void angle(){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Ax = ax/16384.00;
Ay = ay/16384.00;
Az = az/16384.00;
X = atan(Ax/sqrt(Az*Az+Ay*Ay))*180/PI; //angle from accelermeter
Y = atan(Ay/sqrt(Az*Az+Ax*Ax))*180/PI;
Z = atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/PI;
Gyro_x = gx/131.00+6; // get angle acceleration
Gyro_y = gy/131.00;
Gyro_z = gz/131.00;
NowTime = millis(); //star to count time
TimeSpan = NowTime - LastTime; //count time pass
ix = 0.95*(ix + Gyro_x*TimeSpan / 1000) + 0.05*Y; // position count by Complementary Filter
iy = 0.94*(iy + Gyro_y*TimeSpan / 1000) + 0.06*X;
LastTime = NowTime;
Roll = ix
Pitch = iy;
Serial.print("Roll : ");
Serial.print(ix);
Serial.print("Pitch : ");
Serial.println(iy);
} 请问你是怎么连线的？谢谢
页:
[1]