Can't seem to calibrate MPU to save my life

numendues_02 · January 3, 2024, 7:59pm

I am working on a drone using only Arduino and Arduino components after some researching these are all the parts i am using in this project

Arduino Nano (copy)
X2-l293D motor driver
NRF+NRF adapter
MPU6050

everything in my project seems to work just fine and so does the MPU however no matter what i do i just can't seem to get proper values IE - yaw pitch roll are always insane values and when i try to calculate any angle (x,y) it gives me insane values in the thousands here is my code

#include <Wire.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <MPU6050.h>
#include <PID_v1.h>

RF24 radio(8, 9); // CE, CSN pins

MPU6050 mpu;


enum MotorState {
  OFF,
  ON,
  WAIT_OFF
};

MotorState motorState = OFF;
unsigned long startTime = 0;
const unsigned long motorDuration = 2000; // 4 seconds


// Create a struct to hold the received data
struct DataPacket {
  int X;
  int Y;
  int switchState;
  int switchState2;
};

const byte address[6] = "00001";

int enA = 2;
int in1 = 7;
int in2 = A7;
int enB = 3;
int in3 = A1;
int in4 = A0;
int enC = 4;
int in2_1 = 6;
int in2_2 = A6;
int enD = 5;
int in2_3 = A3;
int in2_4 = A2;

int16_t Acc_rawX, Acc_rawY, Acc_rawZ, Gyr_rawX, Gyr_rawY, Gyr_rawZ;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];

float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180 / 3.141592654;

float PID, pwm1, pwm2, pwm3, pwm4, error, previous_error;
float pid_p = 0;
float pid_i = 0;
float pid_d = 0;

/////////////////PID CONSTANTS/////////////////
double kp = 3.00; //3.55
double ki = 0.003; //0.003
double kd = 2.0; //2.05
///////////////////////////////////////////////

double throttle = 150; //initial value of throttle to the motors
float desired_angle = 0; //This is the angle in which we want the balance to stay steady

void setup() {
  Wire.begin(); //begin the wire communication
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(600);
  
  radio.begin();
  radio.openReadingPipe(1, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();

  time = millis(); //Start counting time in milliseconds
  
  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enC, OUTPUT);
  pinMode(in2_1, OUTPUT);
  pinMode(in2_2, OUTPUT);
  pinMode(enD, OUTPUT);
  pinMode(in2_3, OUTPUT);
  pinMode(in2_4, OUTPUT);

  analogWrite(enA, 0);
  analogWrite(enB, 0);
  analogWrite(enC, 0);
  analogWrite(enD, 0);

  Serial.println("Calibrating MPU6050. Keep the device stationary...");
  delay(5000); // Adjust as needed

  calibrateMPU6050();
}

  void calibrateMPU6050() {
  const int numSamples = 500;
  int16_t accX_offset = 0, accY_offset = 0, accZ_offset = 0;
  int16_t gyroX_offset = 0, gyroY_offset = 0, gyroZ_offset = 0;

  // Collect samples for calibration
  for (int i = 0; i < numSamples; i++) {
    int16_t accX, accY, accZ, gyroX, gyroY, gyroZ;
    mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);

    accX_offset += accX;
    accY_offset += accY;
    accZ_offset += accZ;
    gyroX_offset += gyroX;
    gyroY_offset += gyroY;
    gyroZ_offset += gyroZ;

    delay(5); // Adjust as needed based on your application
  }

  // Calculate average offsets
  accX_offset /= numSamples;
  accY_offset /= numSamples;
  accZ_offset /= numSamples;
  gyroX_offset /= numSamples;
  gyroY_offset /= numSamples;
  gyroZ_offset /= numSamples;

  // Set the offsets
  Acc_rawX -= accX_offset;
  Acc_rawY -= accY_offset;
  Acc_rawZ -= accZ_offset;
  Gyr_rawX -= gyroX_offset;
  Gyr_rawY -= gyroY_offset;
  Gyr_rawZ -= gyroZ_offset;
}


void loop() {
  if (radio.available()) {
    // Read the received struct
    DataPacket data;
    radio.read(&data, sizeof(data));

    // Display received data
    Serial.print("Received - Switch: ");
    Serial.println(data.switchState);
    Serial.print("Received - Switch2: ");
    Serial.println(data.switchState2);
    Serial.print("Received - X-axis: ");
    Serial.println(data.X);
    Serial.print("Received - Y-axis: ");
    Serial.println(data.Y);

    if (data.switchState == 0) {
      motorState = ON;
      startTime = millis(); // Record the start time
    } else if (data.switchState2 == 0) {
      motorState = OFF; // Turn off motors if switchState is 1
    }
  }

    // IMU and PID code
  processIMU(); // Read and filter IMU data
  computePID(); // Calculate PID control
  updateMotors(); // Apply PID control to motors

  // Manage motor states
  switch (motorState) {
    case ON:
      directionControl(); // Adjust motor speed without PID
      break;

    case OFF:
      // Turn off motors
      analogWrite(enA, 0);
      analogWrite(enB, 0);
      analogWrite(enC, 0);
      analogWrite(enD, 0);
      break;
  }
}

void directionControl() {
  // Your motor driver control code here...
  // Use the angles obtained from MPU6050 to adjust motor speed with PID
  // AnalogWrite values can be adjusted based on the PID output
  analogWrite(enA, 200);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  
  analogWrite(enB, 200);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);

  analogWrite(enC, 200);
  digitalWrite(in2_1, HIGH);
  digitalWrite(in2_2, LOW);

  analogWrite(enD, 200);
  digitalWrite(in2_3, HIGH);
  digitalWrite(in2_4, LOW);
}


void updateMotors() {
  // Your motor driver control code here...
  // Use pwm1, pwm2, pwm3, pwm4 to control motor speed
  // Map these values to your specific motor driver setup
  // ...

  analogWrite(enA, pwm1);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);

  analogWrite(enB, pwm2);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);

  analogWrite(enC, pwm3);
  digitalWrite(in2_1, HIGH);
  digitalWrite(in2_2, LOW);

  analogWrite(enD, pwm4);
  digitalWrite(in2_3, HIGH);
  digitalWrite(in2_4, LOW);
}



void processIMU() {
  // Read raw values from MPU6050
  int16_t accX, accY, accZ, gyroX, gyroY, gyroZ;
  mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);

  // Check if denominator is not close to zero
  const float epsilon = 0.0001;
  if (fabs(accZ) > epsilon) {
    // Convert raw values to angles
    float roll = atan2(accY, accZ) * RAD_TO_DEG;
    float pitch = atan2(-accX, sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

    // Apply complementary filter with angle normalization:
    float dt = (millis() - timePrev) / 1000.0;
    Total_angle[0] = fmod(0.99 * (Total_angle[0] + gyroX * dt) + 0.01 * roll, 360.0);
    Total_angle[1] = fmod(0.99 * (Total_angle[1] + gyroY * dt) + 0.01 * pitch, 360.0);

    // Print total angles
    Serial.print("Total Angle X: "); Serial.println(Total_angle[0]);
    Serial.print("Total Angle Y: "); Serial.println(Total_angle[1]);

    // Update previous time for the next iteration
    timePrev = millis();
  }
  else {
    Serial.println("Acc Z close to zero. Skipping angle calculation.");
  }
}



void computePID() {
  // Calculate error between desired angle and measured angle
  float errorX = Total_angle[0] - desired_angle;
  float errorY = Total_angle[1] - desired_angle;

  // Proportional term
  pid_p = kp * errorX;

  // Integral term (optional, depending on your system)
  if (-3 < errorX && errorX < 3) {
    pid_i += ki * errorX;

    // Saturate the integral term to prevent it from growing too large
    pid_i = constrain(pid_i, -100, 100);  // Adjust the saturation limits as needed
  }

  // Derivative term
  pid_d = kd * (errorX - previous_error) / elapsedTime;

  // Calculate PID output
  PID = pid_p + pid_i + pid_d;

  // Update previous error for the next iteration
  previous_error = errorX;

  // Debug prints
  Serial.print("PID: "); Serial.println(PID);

  // Calculate PWM values
  pwm1 = throttle + PID;
  pwm2 = throttle - PID;
  pwm3 = throttle + PID;
  pwm4 = throttle - PID;

  // Debug prints
  Serial.print("PWM1: "); Serial.println(pwm1);
  Serial.print("PWM2: "); Serial.println(pwm2);
  Serial.print("PWM3: "); Serial.println(pwm3);
  Serial.print("PWM4: "); Serial.println(pwm4);

  // Ensure PWM values are within the acceptable range
  pwm1 = constrain(pwm1, 150, 255);
  pwm2 = constrain(pwm2, 150, 255);
  pwm3 = constrain(pwm3, 150, 255);
  pwm4 = constrain(pwm4, 150, 255);
}

I have tried a lot of things and i have ran into many issues while making this project mainly (NRF) but after solving that i just can't seem to be able to fix this so if anyone has any advice or can help i would appreciate it immensely

p.s this is how the chip looks like irl (its very messy and i am not proud but as a first time project ever i am happy that it works except the MPU ).

jremington · January 3, 2024, 8:06pm

Calibrating the accelerometer is not really necessary, and the approach you have posted is completely wrong.

Try this simple code to measure pitch and roll tilt angles, and report back.

// minimal MPU-6050 tilt and roll (sjr). Works with MPU-9250 too.
// works perfectly with GY-521, pitch and roll signs agree with arrows on sensor module 7/2019
//
// Tested with 3.3V eBay Pro Mini with no external pullups on I2C bus (worked with internal pullups)
// Add 4.7K pullup resistors to 3.3V if required. A4 = SDA, A5 = SCL

#include<Wire.h>
const int MPU_addr1 = 0x68;
float xa, ya, za, roll, pitch;

void setup() {

  Wire.begin();                                      //begin the wire communication
  Wire.beginTransmission(MPU_addr1);                 //begin, send the slave adress (in this case 68)
  Wire.write(0x6B);                                  //make the reset (place a 0 into the 6B register)
  Wire.write(0);
  Wire.endTransmission(true);                        //end the transmission
  Serial.begin(9600);
}

void loop() {

  Wire.beginTransmission(MPU_addr1);
  Wire.write(0x3B);  //send starting register address, accelerometer high byte
  Wire.endTransmission(false); //restart for read
  Wire.requestFrom(MPU_addr1, 6); //get six bytes accelerometer data
  int t = Wire.read();
  xa = (t << 8) | Wire.read();
  t = Wire.read();
  ya = (t << 8) | Wire.read();
  t = Wire.read();
  za = (t << 8) | Wire.read();
// formula from https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
  roll = atan2(ya , za) * 180.0 / PI;
  pitch = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI; //account for roll already applied

  Serial.print("roll = ");
  Serial.print(roll,1);
  Serial.print(", pitch = ");
  Serial.println(pitch,1);
  delay(400);
}

If that works, accuracy of the tilt and roll calculation can be slightly improved by following the six point calibration procedure described in this tutorial: Accelerometer Calibration II: Simple Methods | Chionotech

numendues_02 · January 3, 2024, 8:06pm

Thank you so much (btw your awesome) i will use the code and get back to you

numendues_02 · January 3, 2024, 8:10pm

I tested it out and this is the output

23:08:36.909 -> roll = -1.1, pitch = -15.4

23:08:37.298 -> roll = 1.0, pitch = -5.9

23:08:37.700 -> roll = 0.0, pitch = -5.5

23:08:38.108 -> roll = 0.5, pitch = -5.7

23:08:38.497 -> roll = 0.7, pitch = -5.4

23:08:38.888 -> roll = 3.6, pitch = 1.7

it seems a lot more stable what do you think ? sorry just saw the article attached i will read it and update the post with the updated code

again thank you so so so much

jremington · January 3, 2024, 8:12pm

Do the roll and pitch angles agree with how you are holding the sensor?

Roll and pitch should both be about zero if the sensor board is level, with Z pointing up.

numendues_02 · January 3, 2024, 8:16pm

this is how its set up

and this is the output

23:15:30.144 -> roll = 0.2, pitch = -5.8

23:15:30.532 -> roll = 0.6, pitch = -6.0

23:15:30.952 -> roll = 0.8, pitch = -6.0

23:15:31.354 -> roll = 0.9, pitch = -5.7

23:15:31.756 -> roll = 0.9, pitch = -6.0

23:15:32.134 -> roll = 1.0, pitch = -5.9

23:15:32.536 -> roll = 0.8, pitch = -5.7

23:15:32.957 -> roll = 1.0, pitch = -5.9

23:15:33.344 -> roll = 1.4, pitch = -6.0

the roll is close to zero which is giving me so much hope and the pitch is still off but i have read a lot that you have to do a lot of calibration for the pitch i am hoping this is an ok response since i am very new to this

again thank you for your time !!

jremington · January 3, 2024, 9:14pm

Have you carefully measured the pitch, using a level?

With the sensor fixed to a big board like that, I can suspect that it is difficult to determine an angular error of just a few degrees.

And, you will have difficulty calibrating the sensor, following the six-point approach linked at the end of post #2.

numendues_02 · January 4, 2024, 6:26pm

Hello,

first of all thank you so much for helping me and pointing me in the right direction.
in terms of pitch my mpu is not placed 100% flat so there was no way it would give me a zero reading however i was able to move it and get it to 0.0 and so with that i was able to fix the rest of the code (i think i am very new so don't take my words for it)

here is the updated code for anyone who may read this in the future and has the same issue

#include <Wire.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <MPU6050.h>

RF24 radio(8, 9); // CE, CSN pins


const int MPU_addr1 = 0x68;
float xa, ya, za, roll, pitch;

MPU6050 mpu;


enum MotorState {
  OFF,
  ON,
  WAIT_OFF
};

MotorState motorState = OFF;
unsigned long startTime = 0;
const unsigned long motorDuration = 2000; // 4 seconds
 unsigned long pid_StartingTime = millis();


// Create a struct to hold the received data
struct DataPacket {
  int X;
  int Y;
  int switchState;
  int switchState2;
};

const byte address[6] = "00001";

int enA = 2;
int in1 = 7;
int in2 = A7;
int enB = 3;
int in3 = A1;
int in4 = A0;
int enC = 4;
int in2_1 = 6;
int in2_2 = A6;
int enD = 5;
int in2_3 = A3;
int in2_4 = A2;

float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];

float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180 / 3.141592654;

float PID, pwm1, pwm2, pwm3, pwm4, error, previous_error;
float pid_p = 0;
float pid_i = 0;
float pid_d = 0;

/////////////////PID CONSTANTS/////////////////
double kp = 3.00; //3.55
double ki = 0.003; //0.003
double kd = 2.0; //2.05
///////////////////////////////////////////////

double throttle = 150; //initial value of throttle to the motors
float desired_angle = 0; //This is the angle in which we want the balance to stay steady

void setup() {
  Wire.begin();                                      //begin the wire communication
  Wire.beginTransmission(MPU_addr1);                 //begin, send the slave adress (in this case 68)
  Wire.write(0x6B);                                  //make the reset (place a 0 into the 6B register)
  Wire.write(0);
  Wire.endTransmission(true);  
  Serial.begin(600);
  
  radio.begin();
  radio.openReadingPipe(1, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();

  time = millis(); //Start counting time in milliseconds
  
  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enC, OUTPUT);
  pinMode(in2_1, OUTPUT);
  pinMode(in2_2, OUTPUT);
  pinMode(enD, OUTPUT);
  pinMode(in2_3, OUTPUT);
  pinMode(in2_4, OUTPUT);

  analogWrite(enA, 0);
  analogWrite(enB, 0);
  analogWrite(enC, 0);
  analogWrite(enD, 0);

  Serial.println("Calibrating MPU6050. Keep the device stationary...");
  delay(5000); // Adjust as needed

}



void loop() {
  if (radio.available()) {
    // Read the received struct
    DataPacket data;
    radio.read(&data, sizeof(data));

    // Display received data
    Serial.print("Received - Switch: ");
    Serial.println(data.switchState);
    Serial.print("Received - Switch2: ");
    Serial.println(data.switchState2);
    Serial.print("Received - X-axis: ");
    Serial.println(data.X);
    Serial.print("Received - Y-axis: ");
    Serial.println(data.Y);

    if (data.switchState == 0) {
      motorState = ON;
      startTime = millis(); // Record the start time
    } else if (data.switchState2 == 0) {
      motorState = OFF; // Turn off motors if switchState is 1
    }
  }

    // IMU and PID code
  processIMU(); // Read and filter IMU data
  computePID(); // Calculate PID control
  updateMotors(); // Apply PID control to motors

  // Manage motor states
  switch (motorState) {
    case ON:
      directionControl(); // Adjust motor speed without PID
      break;

    case OFF:
      // Turn off motors
      analogWrite(enA, 0);
      analogWrite(enB, 0);
      analogWrite(enC, 0);
      analogWrite(enD, 0);
      break;
  }
}

void directionControl() {
  // Your motor driver control code here...
  // Use the angles obtained from MPU6050 to adjust motor speed with PID
  // AnalogWrite values can be adjusted based on the PID output
  analogWrite(enA, 200);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  
  analogWrite(enB, 200);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);

  analogWrite(enC, 200);
  digitalWrite(in2_1, HIGH);
  digitalWrite(in2_2, LOW);

  analogWrite(enD, 200);
  digitalWrite(in2_3, HIGH);
  digitalWrite(in2_4, LOW);
}


void updateMotors() {
  // Your motor driver control code here...
  // Use pwm1, pwm2, pwm3, pwm4 to control motor speed
  // Map these values to your specific motor driver setup
  // ...

  analogWrite(enA, pwm1);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);

  analogWrite(enB, pwm2);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);

  analogWrite(enC, pwm3);
  digitalWrite(in2_1, HIGH);
  digitalWrite(in2_2, LOW);

  analogWrite(enD, pwm4);
  digitalWrite(in2_3, HIGH);
  digitalWrite(in2_4, LOW);
}



void processIMU() {
    // Read raw values from MPU6050
     Wire.beginTransmission(MPU_addr1);
    Wire.write(0x3B);  //send starting register address, accelerometer high byte
    Wire.endTransmission(false); //restart for read
    Wire.requestFrom(MPU_addr1, 6); //get six bytes accelerometer data
    int t = Wire.read();
    xa = (t << 8) | Wire.read();
    t = Wire.read();
    ya = (t << 8) | Wire.read();
    t = Wire.read();
    za = (t << 8) | Wire.read();

    roll = atan2(ya , za) * 180.0 / PI;
    pitch = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI; //account for roll already applied
      Serial.print("roll = ");
      Serial.print(roll,1);
      Serial.print(", pitch = ");
      Serial.println(pitch,1);
    // Apply complementary filter with angle normalization:
    float dt = (millis() - timePrev) / 1000.0;
    Total_angle[0] = fmod(0.99 * (Total_angle[0] + roll * dt) + 0.01 * roll, 360.0);
    Total_angle[1] = fmod(0.99 * (Total_angle[1] + pitch * dt) + 0.01 * pitch, 360.0);

    // Print total angles
    Serial.print("Total Angle X: "); Serial.println(Total_angle[0]);
    Serial.print("Total Angle Y: "); Serial.println(Total_angle[1]);

    // Update previous time for the next iteration
    timePrev = millis();
  
 
}



void computePID() {
  // Calculate error between desired angle and measured angle
  float errorX = pitch - desired_angle;
 // float errorY = roll - desired_angle;
  unsigned long CurrentTime = millis();
  unsigned long elapsedTime = CurrentTime - pid_StartingTime;

  // Proportional term
  pid_p = kp * errorX;

  // Integral term (optional, depending on your system)
  if (-3 < errorX && errorX < 3) {
    pid_i += ki * errorX;

    // Saturate the integral term to prevent it from growing too large
    pid_i = constrain(pid_i, -100, 100);  // Adjust the saturation limits as needed
  }
    Serial.println(errorX);
    Serial.println(previous_error);
    Serial.println(elapsedTime);

  // Derivative term
  pid_d = kd * (errorX - previous_error) / elapsedTime;
  Serial.println(pid_p);
  Serial.println(pid_i);
  Serial.println(pid_d);
  // Calculate PID output
  PID = pid_p + pid_i + pid_d;

  // Update previous error for the next iteration
  previous_error = errorX;

  // Debug prints
  Serial.print("PID: "); Serial.println(PID);

  // Calculate PWM values
  pwm1 = throttle + PID;
  pwm2 = throttle - PID;
  pwm3 = throttle + PID;
  pwm4 = throttle - PID;

  // Debug prints
  Serial.print("PWM1: "); Serial.println(pwm1);
  Serial.print("PWM2: "); Serial.println(pwm2);
  Serial.print("PWM3: "); Serial.println(pwm3);
  Serial.print("PWM4: "); Serial.println(pwm4);

  // Ensure PWM values are within the acceptable range
  pwm1 = constrain(pwm1, 150, 255);
  pwm2 = constrain(pwm2, 150, 255);
  pwm3 = constrain(pwm3, 150, 255);
  pwm4 = constrain(pwm4, 150, 255);
}

again thank you so much jremington

sincerely,
Ayham.

system · July 2, 2024, 6:27pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
MPU-6050 angle readings for drone Sensors	2	1332	August 3, 2021
Mpu6050 and arduino nano on I2c Networking, Protocols, and Devices	4	1230	May 6, 2021
[Solved]Problem reading MPU-6050 pitch and yaw Sensors	8	3206	May 6, 2021
MPU6050 Pitch angle Sensors	9	1186	March 12, 2024
Drone doesn't Stabilize with MPU6050 Sensors	2	481	January 4, 2024

Can't seem to calibrate MPU to save my life

Related Topics