Hey guys, I am trying to read the angle values from the mpu6050
Progress: I can read the accelerometer value and convert it to angles and works fine.
I can read the gyroscope angles and convert them o angles.
Problem: I know that the value of the gyroscope drifts, but the rate at which mine drifts i am not satisfied (aprx. 1deg per sec drifting). And applying this to a drone dosen't seem to be a good idea.
My desire: Is there any way the drift can be made better? I will be using this to make a quadcopter.
my Code;
NOTE: I am using a timer interrupt so that i read the gyroscope value 250 times a second.
//libraries
#include <SoftwareSerial.h>
#include<Wire.h>
#include <TimerOne.h>
#include <Servo.h>//Using servo library to control ESC
//----------------------------
double pitch_output;
double roll_output;
double a_roll;
double a_pitch;
double a_yaw;
double g_pitch;
double g_roll;
double pitch_rate;
double roll_rate;
//mpu6050-------------------
const int MPU_addr = 0x68;
double AcX, AcY, AcZ, GyX, GyY, GyZ, Tmp;
int minVal = 265;
int maxVal = 402;
void setup() {
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Timer1.initialize(4000);
Timer1.attachInterrupt(g_calculate);
}
void loop() {
decoding_input();
mpu6050_reading();
correction();
Serial.println(pitch_output);
delay(10);
}
void g_calculate() {
//Gyroscope angle calculation
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
g_pitch += GyX * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
g_roll += GyY * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
g_pitch += g_roll * sin(GyZ * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
g_roll -= g_pitch * sin(GyZ * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
}
void correction() {
//----------------------------------------------------------------
roll_rate = GyY / 250;
pitch_rate = GyX / 250;
//---------------converting x and y angles from 0-360 to (-180)-(180)----------------
if (a_roll > 180)
a_roll = a_roll - 360;
else if (a_roll > 0)
a_roll = a_roll;
if (a_pitch > 180)
a_pitch = a_pitch - 360;
else if (a_pitch > 0)
a_pitch = a_pitch;
//------------------------------------------------
g_pitch = g_pitch * 0.9996 + a_pitch * 0.0004; //adjusting the gyro output with the accelerometr to prevent drift
g_roll = g_roll * 0.9996 + a_roll * 0.0004;
//--------------------------------------------------------------------
pitch_output = g_pitch * 0.9 + a_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
roll_output = g_roll * 0.9 + a_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
//--------------------------------------------------------------------
}
void mpu6050_reading() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
//Accelerometer calculation
int xAng = map(AcX, minVal, maxVal, -90, 90);
int yAng = map(AcY, minVal, maxVal, -90, 90);
int zAng = map(AcZ, minVal, maxVal, -90, 90);
a_pitch = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI);
a_roll = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI);
a_yaw = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
}
For undertanding
a_pitch/roll/yaw- values of the acceleromer
g_pitch/roll/yaw- values of the gyroscope
pitch/roll_output- the final calculated values of pitch and output
