Drone doesn't Stabilize with MPU6050

Hello, I recently created my own Arduino drone and remote, and I am having problems with stabilizing it for takeoff. I tried using the PID formula in correspondence with the MPU6050 library by Jeff Rowberg, and my drone takes off in a similar, reckless fashion as it did without it. I haven't implemented any of my remote's pitch, yaw, and roll controls yet as I'm first trying to get the thing to take off with only a throttle input from the remote.

Is there anything wrong with my code that is preventing from my drone from properly stabilizing itself, and if so what can I do to fix it?

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


struct dataPackage
{
  byte throttleYValue;
  byte throttleState;

  byte yawXValue;

  byte rollXValue;
  byte pitchYValue;

  byte throttlePeakValue;
  byte headingValue;
};
dataPackage data;

void resetData()
{
  data.throttleYValue = 0;
  data.yawXValue = 121;

  data.throttleState = false;

  data.rollXValue = 124;
  data.pitchYValue = 127;

  data.throttlePeakValue = 0;
  data.headingValue = 0;
}

unsigned long lastReceiveTime = 0;
unsigned long currentTime = 0;

RF24 radio(8, 10); // CE, CSN

MPU6050 mpu;
Servo motor1, motor2, motor3, motor4;

// PID Constants
float Kp = 1.5;
float Ki = 0.2;
float Kd = 0.1;

// Target angles
float targetRoll = 0.0;
float targetPitch = 0.0;

// PID Variables
float previousRollError = 0.0;
float previousPitchError = 0.0;
float integralRollError = 0.0;
float integralPitchError = 0.0;

void setup() {

  motor1.attach(5, 1000, 2000);
  motor2.attach(3, 1000, 2000);
  motor3.attach(9, 1000, 2000);
  motor4.attach(6, 1000, 2000);
  
  const byte address[6] = "00001";
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setAutoAck(false);
  radio.setDataRate(RF24_250KBPS);
  radio.setPALevel(RF24_PA_MAX);
  radio.startListening();
  
  Wire.begin();
  mpu.initialize();
  calibrateMPU6050();
  delay(1000);
  
  
}

void loop() {
   currentTime = millis();
  if (currentTime - lastReceiveTime > 1000)
    resetData();

  if (radio.available())
  {
    radio.read(&data, sizeof(dataPackage));
    /*Serial.println("1|X" + String(data.yawXValue) + ":Y" + String(data.throttleYValue) + ":P" + String(data.throttlePeakValue));
    Serial.println("2|X" + String(data.rollXValue) + ":Y" + String(data.pitchYValue) + ":H" + String(data.headingValue));
    Serial.println("throttleState:" + String(data.throttleState) + '\n');*/

    lastReceiveTime = millis();
  }

  if (data.throttleState == true)
  {
     if(data.throttleYValue <= 124){
      motor1.writeMicroseconds(1065);
      motor2.writeMicroseconds(1065);
      motor3.writeMicroseconds(1065);
      motor4.writeMicroseconds(1065); 
    }
    else{
      float roll = getRoll();
      float pitch = getPitch();
    
      float rollError = targetRoll - roll;
      float pitchError = targetPitch - pitch;
    
      float rollCorrection = calculatePID(rollError, previousRollError, integralRollError);
      float pitchCorrection = calculatePID(pitchError, previousPitchError, integralPitchError);
      
      int throttleMax = map(data.throttlePeakValue, 0, 255, 1500, 2000);
      int desiredSpeed =  map(data.throttleYValue, 124, 255, 1000, throttleMax);
      
      // Calculate motor speeds
      int motorSpeed1 = desiredSpeed + rollCorrection + pitchCorrection;
      int motorSpeed2 = desiredSpeed - rollCorrection + pitchCorrection;
      int motorSpeed3 = desiredSpeed - rollCorrection - pitchCorrection;
      int motorSpeed4 = desiredSpeed + rollCorrection - pitchCorrection;
    
      // Set motor speeds
      motor1.writeMicroseconds(motorSpeed1);
      motor2.writeMicroseconds(motorSpeed2);
      motor3.writeMicroseconds(motorSpeed3);
      motor4.writeMicroseconds(motorSpeed4);
    
      // Update PID variables
      previousRollError = rollError;
      previousPitchError = pitchError;
      integralRollError += rollError;
      integralPitchError += pitchError;
    }
  }
  else{
      motor1.writeMicroseconds(0);
      motor2.writeMicroseconds(0);
      motor3.writeMicroseconds(0);
      motor4.writeMicroseconds(0); 
  }
}

void calibrateMPU6050() {
  int calibrateIterations = 2000;
  float rollTotal = 0.0;
  float pitchTotal = 0.0;

  for (int i = 0; i < calibrateIterations; i++) {
    float roll = getRoll();
    float pitch = getPitch();

    rollTotal += roll;
    pitchTotal += pitch;

    delay(3);
  }

  targetRoll = rollTotal / calibrateIterations;
  targetPitch = pitchTotal / calibrateIterations;
}

float getRoll() {
  int16_t accX = mpu.getAccelerationX();
  int16_t accY = mpu.getAccelerationY();
  int16_t accZ = mpu.getAccelerationZ();

  float roll = atan2(accY, accZ) * RAD_TO_DEG;

  return roll;
}

float getPitch() {
  int16_t accX = mpu.getAccelerationX();
  int16_t accY = mpu.getAccelerationY();
  int16_t accZ = mpu.getAccelerationZ();

  float pitch = atan2(accX, accZ) * RAD_TO_DEG;

  return pitch;
}

float calculatePID(float error, float previousError, float integralError) {
  float proportionalTerm = Kp * error;
  float derivativeTerm = Kd * (error - previousError);
  float integralTerm = Ki * integralError;

  float output = proportionalTerm + derivativeTerm + integralTerm;

  return output;
}

Thanks for taking the time to read my question, and I appreciate any feedback and responses.

A good place to start with the debugging is to put in print statements to see whether the measured 3D orientation (pitch and roll) angles have any connection with reality, and are stable for fixed orientations.

If so, then make sure that PID-ordered corrections to the prop speeds have the correct signs and reasonable magnitudes, etc.

Finally, the PID constants must be individually tuned for every new application.

Note that the MPU-6050 cannot measure yaw, as there is no external reference.

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