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);
} 本帖最後由 超新手 於 2016-3-29 08:34 編輯
這是 Filter, 所以會有些延遲, 不算"不正常"
如果覺得輸出不穩或太慢, 你可以調整一下 Complementary Filter 的參數看看
iy 的 0.94 和 0.06 , 這兩個數字只要小於 1, 而且加起來等於 1 即可
你可以大略小調一下, 再看看是不是你要的
只是.... Gyro_x = gx/131.00+6; 為什麼要加 6? 前輩你好
關於Filter前面的參數我已經有調整過,但pitch部分還是會有值突然加大的情況 你好~可以跟你請教你的程式碼的iz計算方式嗎??
頁:
[1]