#include <Wire.h>
#include <MPU9250.h>
#include <math.h>
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float temperature;
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
bool set_gyro_angles = false;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
long loop_timer;
int temp;
int dt = 800; //800 seconds
int lastCal = 0;
void setup() {
Wire.begin();
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
// setting the accelerometer full scale range to +/-8G
//IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G);
// setting the gyroscope full scale range to +/-500 deg/s
//IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);
// setting DLPF bandwidth to 20 Hz
//IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
// setting SRD to 19 for a 50 Hz update rate
//IMU.setSrd(19);
if (IMU.calibrateAccel() <= 0) {
Serial.print("accelCal = ");
Serial.println(IMU.calibrateAccel());
} else if (IMU.calibrateAccel() > 0){
Serial.print("OK accelCal = ");
Serial.println(IMU.calibrateAccel());
}
if (IMU.calibrateGyro() <= 0) {
Serial.print("gyroCal = ");
Serial.println(IMU.calibrateGyro());
} else if (IMU.calibrateGyro() > 0){
Serial.print("OK gyroCal = ");
Serial.println(IMU.calibrateGyro());
}
if (IMU.calibrateMag() <= 0) {
Serial.print("magCal = ");
Serial.println(IMU.calibrateMag());
} else if (IMU.calibrateMag() > 0) {
Serial.print("OK magCal = ");
Serial.println(IMU.calibrateMag());
}
}
void loop() {
// read the sensor
IMU.readSensor();
IMU.calibrateAccel();
IMU.calibrateGyro(); //The 3 Types of Calibrations Inputs I found on Github for BolderFlight
IMU.calibrateMag();
temperature = IMU.getTemperature_C(),6;
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Read the raw acc and gyro data from the MPU-6050 for 1000 times
IMU.readSensor();
gyro_x_cal += IMU.getGyroX_rads(),6; //Add the gyro x offset to the gyro_x_cal variable
gyro_y_cal += IMU.getGyroY_rads(),6; //Add the gyro y offset to the gyro_y_cal variable
gyro_z_cal += IMU.getGyroZ_rads(),6; //Add the gyro z offset to the gyro_z_cal variable
delay(3);
}
// divide by 1000 to get avarage offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
loop_timer = micros(); //Reset the loop timer
//Subtract the offset values from the raw gyro values
gyro_x -= IMU.getGyroX_rads(),6;
gyro_y -= IMU.getGyroY_rads(),6;
gyro_z -= IMU.getGyroZ_rads(),6;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
angle_pitch += (IMU.getGyroX_rads(),6) * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += (IMU.getGyroY_rads(),6) * 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_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
Serial.println(angle_pitch);
Serial.println(angle_roll);
//Accelerometer angle calculations
acc_total_vector = sqrt((IMU.getAccelX_mss(),6*IMU.getAccelX_mss(),6)+(IMU.getAccelY_mss(),6*IMU.getAccelY_mss(),6)+(IMU.getAccelZ_mss(),6*IMU.getAccelZ_mss(),6)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin(((float)IMU.getAccelY_mss(),6)/acc_total_vector)* 57.296; //Calculate the pitch angle
angle_roll_acc = asin(((float)IMU.getAccelX_mss(),6)/acc_total_vector)* -57.296; //Calculate the roll angle
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9 + angle_pitch_acc * 0.1; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9 + angle_roll_acc * 0.1; //Correct the drift of the gyro roll angle with the accelerometer roll angle
}
else{ //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
Serial.print(" | Angle Pitch = "); Serial.println(angle_pitch_output);
Serial.print(" | Angle Roll = "); Serial.println(angle_roll_output);
while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
loop_timer = micros();//Reset the loop timer
//do {
// display the data
Serial.print(IMU.getAccelX_mss(),6);
Serial.print(",");
Serial.print(IMU.getAccelY_mss(),6);
Serial.print(",");
Serial.print(IMU.getAccelZ_mss(),6);
Serial.print(",");
Serial.print(IMU.getGyroX_rads(),6);
Serial.print(",");
Serial.print(IMU.getGyroY_rads(),6);
Serial.print(",");
Serial.print(IMU.getGyroZ_rads(),6);
Serial.print(",");
Serial.print(IMU.getMagX_uT(),6);
Serial.print(",");
Serial.print(IMU.getMagY_uT(),6);
Serial.print(",");
Serial.print(IMU.getMagZ_uT(),6);
Serial.print(",");
Serial.print("Temperature = ");
Serial.println(temperature);
//Serial.print(
//} while (IMU.calibrateAccel() >= 1, IMU.calibrateGyro() >= 1, IMU.calibrateMag() >= 1);
//Remember to fix the lns for the functions you use.
//Serial.print(theta);
//Serial.print(",");
//Serial.println(phi);
delay(20);
}
I'm using a Eleggo Uno R3, and a Makerhawk MPU-9250 IMU.
I can't tell what the problem is with the code, or if it's a problem with IMU itself. Unfortunatly there have been issues getting the manufacturers code.
The pitch and roll go to 90 degrees and -90 degrees after starting from zero, and they stay there. I move the IMU around and tilt it, and they are always the same. I took the functions from other work on GitHub, so they must work. The accel, gyro, mag and temperature values change and seem to be working. Other than that is makes no sense that the pitch and roll are always these values even when lying flat.
Any help or suggestions would be greatly appreciated. Thank you.