Hey everyone,
I recently purchased two modules, the GY-87 and the GY-521, both of which utilize the MPU6050. I have the following code, which I borrowed from Joop Brokking and made some slight adjustments to:
#include <Wire.h>
//Manual accelerometer calibration values for IMU angles:
int16_t manual_acc_pitch_cal_value = 0;
int16_t manual_acc_roll_cal_value = 0;
//Manual gyro calibration values.
//Set the use_manual_calibration variable to true to use the manual calibration variables.
uint8_t use_manual_calibration = false;
int16_t manual_gyro_pitch_cal_value = 0;
int16_t manual_gyro_roll_cal_value = 0;
int16_t manual_gyro_yaw_cal_value = 0;
float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll;
int16_t acc_axis[4], gyro_axis[4], temperature;
int32_t gyro_axis_cal[4], acc_axis_cal[4];
int32_t cal_int;
int16_t loop_counter;
uint32_t loop_timer;
//The I2C address of the MPU-6050 is 0x68 in hexadecimal form.
uint8_t gyro_address = 0x68;
TwoWire HWire(2, I2C_FAST_MODE);
void setup() {
Serial.begin(57600); //Set the serial output to 57600 kbps.
delay(100); //Give the serial port some time to start to prevent data loss.
HWire.begin(); //Start the I2C as master
HWire.beginTransmission(gyro_address); //Start communication with the MPU-6050.
HWire.write(0x6B); //We want to write to the PWR_MGMT_1 register (6B hex).
HWire.write(0x00); //Set the register bits as 00000000 to activate the gyro.
HWire.endTransmission(); //End the transmission with the gyro.
HWire.beginTransmission(gyro_address); //Start communication with the MPU-6050.
HWire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex).
HWire.write(0x08); //Set the register bits as 00001000 (500dps full scale).
HWire.endTransmission(); //End the transmission with the gyro.
HWire.beginTransmission(gyro_address); //Start communication with the MPU-6050.
HWire.write(0x1C); //We want to write to the ACCEL_CONFIG register (1A hex).
HWire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range).
HWire.endTransmission(); //End the transmission with the gyro.
HWire.beginTransmission(gyro_address); //Start communication with the MPU-6050.
HWire.write(0x1A); //We want to write to the CONFIG register (1A hex).
HWire.write(0x03); //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz).
HWire.endTransmission(); //End the transmission with the gyro.
}
void loop() {
// put your main code here, to run repeatedly:
check_imu_angles();
}
void gyro_signalen(void) {
//Read the MPU-6050 data.
HWire.beginTransmission(gyro_address); //Start communication with the gyro.
HWire.write(0x3B); //Start reading @ register 43h and auto increment with every read.
HWire.endTransmission(); //End the transmission.
HWire.requestFrom(gyro_address, 14); //Request 14 bytes from the MPU 6050.
acc_axis[1] = HWire.read() << 8 | HWire.read(); //Add the low and high byte to the acc_x variable.
acc_axis[2] = HWire.read() << 8 | HWire.read(); //Add the low and high byte to the acc_y variable.
acc_axis[3] = HWire.read() << 8 | HWire.read(); //Add the low and high byte to the acc_z variable.
temperature = HWire.read() << 8 | HWire.read(); //Add the low and high byte to the temperature variable.
gyro_axis[1] = HWire.read() << 8 | HWire.read(); //Read high and low part of the angular data.
gyro_axis[2] = HWire.read() << 8 | HWire.read(); //Read high and low part of the angular data.
gyro_axis[3] = HWire.read() << 8 | HWire.read(); //Read high and low part of the angular data.
gyro_axis[2] *= -1; //Invert gyro so that nose up gives positive value.
gyro_axis[3] *= -1; //Invert gyro so that nose right gives positive value.
acc_axis[1] -= manual_acc_pitch_cal_value; //Subtact the manual accelerometer pitch calibration value.
acc_axis[2] -= manual_acc_roll_cal_value; //Subtact the manual accelerometer roll calibration value.
gyro_axis[1] -= manual_gyro_roll_cal_value; //Subtact the manual gyro roll calibration value.
gyro_axis[2] -= manual_gyro_pitch_cal_value; //Subtact the manual gyro pitch calibration value.
gyro_axis[3] -= manual_gyro_yaw_cal_value; //Subtact the manual gyro yaw calibration value.
}
void check_imu_angles(void) {
uint8_t first_angle = 0;
loop_counter = 0;
first_angle = false;
if (use_manual_calibration)cal_int = 2000; //If manual calibration is used.
else {
cal_int = 0; //If manual calibration is not used.
manual_gyro_pitch_cal_value = 0; //Set the manual pitch calibration variable to 0.
manual_gyro_roll_cal_value = 0; //Set the manual roll calibration variable to 0.
manual_gyro_yaw_cal_value = 0; //Set the manual yaw calibration variable to 0.
}
while (1) {
loop_timer = micros() + 4000; //Set the loop_timer variable to the current micros() value + 4000.
if (cal_int == 0) { //If manual calibration is not used.
gyro_axis_cal[1] = 0; //Reset calibration variables for next calibration.
gyro_axis_cal[2] = 0; //Reset calibration variables for next calibration.
gyro_axis_cal[3] = 0; //Reset calibration variables for next calibration.
Serial.print("Calibrating the gyro");
//Let's take multiple gyro data samples so we can determine the average gyro offset (calibration).
for (cal_int = 0; cal_int < 2000 ; cal_int ++) { //Take 2000 readings for calibration.
gyro_signalen(); //Read the gyro output.
gyro_axis_cal[1] += gyro_axis[1]; //Ad roll value to gyro_roll_cal.
gyro_axis_cal[2] += gyro_axis[2]; //Ad pitch value to gyro_pitch_cal.
gyro_axis_cal[3] += gyro_axis[3]; //Ad yaw value to gyro_yaw_cal.
delay(4); //Small delay to simulate a 250Hz loop during calibration.
}
Serial.println(".");
//Now that we have 2000 measures, we need to devide by 2000 to get the average gyro offset.
gyro_axis_cal[1] /= 2000; //Divide the roll total by 2000.
gyro_axis_cal[2] /= 2000; //Divide the pitch total by 2000.
gyro_axis_cal[3] /= 2000; //Divide the yaw total by 2000.
manual_gyro_pitch_cal_value = gyro_axis_cal[2]; //Set the manual pitch calibration variable to the detected value.
manual_gyro_roll_cal_value = gyro_axis_cal[1]; //Set the manual roll calibration variable to the detected value.
manual_gyro_yaw_cal_value = gyro_axis_cal[3]; //Set the manual yaw calibration variable to the detected value.
}
gyro_signalen(); //Let's get the current gyro data.
//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_axis[2] * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable.
angle_roll += gyro_axis[1] * 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
angle_pitch -= angle_roll * sin(gyro_axis[3] * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel.
angle_roll += angle_pitch * sin(gyro_axis[3] * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel.
//Accelerometer angle calculations
if (acc_axis[1] > 4096)acc_axis[1] = 4096; //Limit the maximum accelerometer value.
if (acc_axis[1] < -4096)acc_axis[1] = -4096; //Limit the maximum accelerometer value.
if (acc_axis[2] > 4096)acc_axis[2] = 4096; //Limit the maximum accelerometer value.
if (acc_axis[2] < -4096)acc_axis[2] = -4096; //Limit the maximum accelerometer value.
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_axis[1] / 4096) * 57.296; //Calculate the pitch angle.
angle_roll_acc = asin((float)acc_axis[2] / 4096) * 57.296; //Calculate the roll angle.
if (!first_angle) { //When this is the first time.
angle_pitch = angle_pitch_acc; //Set the pitch angle to the accelerometer angle.
angle_roll = angle_roll_acc; //Set the roll angle to the accelerometer angle.
first_angle = true;
}
else { //When this is not the first time.
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle.
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle.
}
//We can't print all the data at once. This takes to long and the angular readings will be off.
if (loop_counter == 0)Serial.print("Pitch: ");
if (loop_counter == 1)Serial.print(angle_pitch , 1);
if (loop_counter == 2)Serial.print(" Roll: ");
if (loop_counter == 3)Serial.print(angle_roll , 1);
if (loop_counter == 4)Serial.print(" Yaw: ");
if (loop_counter == 5)Serial.print(gyro_axis[3] / 65.5 , 0);
if (loop_counter == 6)Serial.print(" Temp: ");
if (loop_counter == 7)Serial.println(temperature / 340.0 + 35.0 , 1);
loop_counter ++;
if (loop_counter == 60)loop_counter = 0;
while (loop_timer > micros());
}
loop_counter = 0; //Reset the loop counter variable to 0.
}
I am using an STM32F103C8T6 with these modules, and I'm encountering a problem. With the GY-521, the code seems to work correctly: the angles do not drift. For example, if I hold the module at a 45-degree angle, it shows that angle accurately. However, with the GY-87, the angle drifts. It starts decreasing, and when the module is leveled again, it shows about a 10-degree roll.
Has anyone experienced the same problem and could explain what the issue might be?
P.S. I tried different GY-521 and GY-87 modules and even tried with an Arduino UNO, but the problem remains the same.
Thanks in advance!
2 posts - 2 participants