MPU6050 Drone Wobbling

Community General Discussion
1

Hello,

I'm building a drone right now with an Arduino Mega and I'm using an MPU6050 accelerometer. I'm having an issue where when I turn up the throttle to the drone, instead of flying up, it wobbles and rotates. I haven't managed to get it off the ground. Here is the code that I'm running(I took inspiration from Carbon Aeronautics):

#include <Servo.h>

Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;

// MPU-6050 initialized below
#include <Wire.h>
float RatePitch, RateRoll, RateYaw, RateCalibrationPitch, RateCalibrationRoll, RateCalibrationYaw;
int RateCalibrationNumber;

// Reciever initialization below

#define throttlePin 9
#define rollPin 10
#define pitchPin 11
#define yawPin 12
float ReceiverValue[4]={};

uint32_t LoopTimer;

// All variables required for PID Control Loop initialized below
float DesiredRateRoll, DesiredRatePitch, DesiredRateYaw;
float ErrorRateRoll, ErrorRatePitch, ErrorRateYaw;
float InputRoll, InputThrottle, InputPitch, InputYaw;
float PrevErrorRateRoll, PrevErrorRatePitch, PrevErrorRateYaw;
float PrevItermRateRoll, PrevItermRatePitch, PrevItermRateYaw;
float PIDReturn[] = {0,0,0};

// PID constants defined below
float PRateRoll = 0.6; float PRatePitch = PRateRoll; float PRateYaw = 2;
float IRateRoll = 3.5; float IRatePitch = IRateRoll; float IRateYaw = 12;
float DRateRoll = 0.03; float DRatePitch = DRateRoll; float DRateYaw = 0;

float MotorInput1, MotorInput2, MotorInput3, MotorInput4;

void read_receiver(){
  ReceiverValue[0] = pulseIn(yawPin, HIGH);
  ReceiverValue[1] = pulseIn(pitchPin, HIGH);
  ReceiverValue[2] = pulseIn(throttlePin, HIGH);
  ReceiverValue[3] = pulseIn(rollPin, HIGH);

  /*Serial.print("Yaw: ");
  Serial.print(ReceiverValue[0]);
  Serial.print(" | Pitch: ");
  Serial.print(ReceiverValue[1]);
  Serial.print(" | Roll: ");
  Serial.print(ReceiverValue[2]);
  Serial.print(" | Throttle: ");
  Serial.println(ReceiverValue[3]);
  */
  Serial.print("| MotorInput4 ");
  Serial.print(MotorInput4);
  Serial.print("| MotorInput3 ");
  Serial.print(MotorInput3);
  Serial.print("| MotorInput2 ");
  Serial.print(MotorInput2);
  Serial.print("| MotorInput1 ");
  Serial.println(MotorInput1);

}



void gyro_signals() {
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0x68,6);
  int16_t GyroX = Wire.read() << 8 | Wire.read();
  int16_t GyroY = Wire.read() << 8 | Wire.read();
  int16_t GyroZ = Wire.read() << 8 | Wire.read();
  RateRoll = (float) GyroX / 65.5;
  RatePitch = (float) GyroY / 65.5;
  RateYaw = (float) GyroZ / 65.6;
  // Serial.println(GyroX);
}

void pid_equation(float Error, float P, float I, float D, float PrevError, float PrevIterm){
  float Pterm = P*Error;
  float Iterm = PrevIterm+I*(Error + PrevError)*0.004/2;
  if (Iterm > 400){
    Iterm = 400;
  } else if (Iterm < -400){
    Iterm = -400;
  }
  float Dterm = D*(Error-PrevError)/0.004;
  float PIDOutput = Pterm+Iterm+Dterm;
  if (PIDOutput > 400){
    PIDOutput = 400;
  } else if (PIDOutput < -400){
    PIDOutput = -400;
  }
  PIDReturn[0] = PIDOutput;
  PIDReturn[1] = Error;
  PIDReturn[2] = Iterm;
}

void reset_pid(){
  PrevErrorRateRoll = 0; PrevErrorRatePitch = 0; PrevErrorRateYaw = 0;
  PrevItermRateRoll = 0; PrevItermRatePitch = 0; PrevItermRateYaw = 0;
}

void setup() {
  motor1.attach(5);
  motor2.attach(6);
  motor3.attach(7);
  motor4.attach(8);
  Serial.begin(9600);
  pinMode(throttlePin, INPUT);
  pinMode(yawPin, INPUT);
  pinMode(pitchPin, INPUT);
  pinMode(rollPin, INPUT);

  // initializing communication with the gyroscope below
  Wire.setClock(400000);
  Wire.begin();
  delay(250);
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  // calibrating gyroscope

  for (RateCalibrationNumber = 0; RateCalibrationNumber < 2000; RateCalibrationNumber++){
    gyro_signals();
    RateCalibrationRoll += RateRoll;
    RateCalibrationPitch += RatePitch;
    RateCalibrationYaw += RateYaw;
    delay(1);
  }
  RateCalibrationRoll /= 2000;
  RateCalibrationPitch /= 2000;
  RateCalibrationYaw /= 2000;



  // checking to see if throttle stick is in lowest position below

  
  //while (ReceiverValue[2] < 1050){
  read_receiver();
  delay(4);
  //}
  
}

void loop() {
  // Changing the Rate variables from gyroscope to calibrate
  
  gyro_signals();
  RateRoll -= RateCalibrationRoll;
  RatePitch -= RateCalibrationPitch;
  RateYaw -= RateCalibrationYaw;

  read_receiver();
  // Mapping desired variables to reciever values below
  DesiredRateRoll = 0.15 * (ReceiverValue[0] - 1500); DesiredRatePitch = 0.15 * (ReceiverValue[1] - 1500);
  InputThrottle = ReceiverValue[2];
  DesiredRateYaw = 0.15 * (ReceiverValue[3] - 1500);

  // Calculating errors for the PID calculations below
  ErrorRateRoll = DesiredRateRoll - RateRoll;
  ErrorRatePitch = DesiredRatePitch - RatePitch;
  ErrorRateYaw = DesiredRateYaw - RateYaw;

  pid_equation(ErrorRateRoll, PRateRoll, IRateRoll, DRateRoll, PrevErrorRateRoll, PrevItermRateRoll);
  InputRoll = PIDReturn[0];
  PrevErrorRateRoll = PIDReturn[1];
  PrevItermRateRoll = PIDReturn[2];
  pid_equation(ErrorRatePitch, PRatePitch, IRatePitch, DRatePitch, PrevErrorRatePitch, PrevItermRatePitch);
  InputPitch = PIDReturn[0];
  PrevErrorRatePitch = PIDReturn[1];
  PrevItermRatePitch = PIDReturn[2];
  pid_equation(ErrorRateYaw, PRateYaw, IRateYaw, DRateYaw, PrevErrorRateYaw, PrevItermRateYaw);
  InputYaw = PIDReturn[0];
  PrevErrorRateYaw = PIDReturn[1];
  PrevItermRateYaw = PIDReturn[2];

  // Limiting throttle to 80 percent below because maximum throttle is 2000
  if (InputThrottle > 1800){
    InputThrottle = 1800;
  }

  // Calculating motor inputs below
  MotorInput1 = 1.024 * ( InputThrottle - InputRoll - InputPitch - InputYaw);
  MotorInput2 = 1.024 * (InputThrottle - InputRoll + InputPitch + InputYaw);
  MotorInput3 = 1.024 * (InputThrottle + InputRoll + InputPitch - InputYaw);
  MotorInput4 = 1.024 * (InputThrottle + InputRoll - InputPitch + InputYaw);

  // Limiting maximum power going to motors below
  if (MotorInput1 > 2000){
    MotorInput1 = 1999;
  }
  if (MotorInput2 > 2000){
    MotorInput2 = 1999;
  }
  if (MotorInput3 > 2000){
    MotorInput3 = 1999;
  }
  if (MotorInput4 > 2000){
    MotorInput4 = 1999;
  }

  // Setting motor to 18 percent to stop drone from crashing if the throttle stick is pushed down too low 
  int ThrottleIdle = 1180;
  if (MotorInput1 < ThrottleIdle) {
    MotorInput1 = ThrottleIdle;
  }
  if (MotorInput2 < ThrottleIdle) {
    MotorInput2 = ThrottleIdle;
  }
  if (MotorInput3 < ThrottleIdle) {
    MotorInput3 = ThrottleIdle;
  }
  if (MotorInput4 < ThrottleIdle) {
    MotorInput4 = ThrottleIdle;
  }

  // if the throttle stick is set to its lowest position, the motors will turn off
  int ThrottleCutOff = 1000;
  if (ReceiverValue[2]<1050){
    MotorInput1 = ThrottleCutOff;
    MotorInput2 = ThrottleCutOff;
    MotorInput3 = ThrottleCutOff;
    MotorInput4 = ThrottleCutOff;
    reset_pid();
  }

  //writing to the motors below
  motor1.writeMicroseconds(MotorInput1);
  motor2.writeMicroseconds(MotorInput2);
  motor3.writeMicroseconds(MotorInput3);
  motor4.writeMicroseconds(MotorInput4);

}

Right now, I'm just wondering if there is an issue within the code or if it is probably a hardware issue. When I run the code, the motor outputs seem to look correct(within a range between 1000 and 2000).

2

Please explain the theory behind the code, especially how your use of measured rotation rates is supposed to control drone 3D orientation.

Where did all the "magic numbers" in the code come from, for example, the PID constants?

What have you done to debug the problem?

1 Like
3

So there are the Pitch, Yaw and Roll rates as well as the Desired Roll, Pitch and Yaw rates (proportional to the difference between 1500(midpoint of the joystick) and the receiver inputs).

It calculates the desired rotation rates with inputs from a radio remote. Then desired rates of rotation for the drone in each axis are created.

PID is used to control the drone's orientation based on the difference between the desired rates and the measured rotation rates from the gyroscope.

The P term is the initial response to the error(Rate roll, pitch yaw and desired roll, pitch and yaw).
The I term is the cumulative error in case a steady-state error happens.
The D term accounts for potential overshoots and oscillations.

As for the "magic numbers," I attempted to tune the PID and ended up accidentally breaking a few sets of propellers. I just tried to reset it to its initial values I saw on a GitHub page. I think there still is some PID tuning to be done, but the values haven't been derived mathematically if that was what you were asking.

When tuning the PID, I accidentally increased the I constant a bit too much, which made the drone oscillate and crash. Right now, I think there may be a problem with the drift in the MPU6050, but I'm not sure exactly what the problem is.

So far to debug the problem, I have run the motors to see if they respond to the receiver inputs on the remote. The drone responds correctly to changes in the stick position mapped to the pitch and roll. I tested it out and the drone leans and moves towards the desired directions based on the stick positions.

I think that the main issue is that it can't get off the ground due to drift. Or it could just be a fine-tuning issue.

4

Thanks for writing all that, but it does not explain how rotation rates can be used to stabilize orientation. The rotation rates are the derivative (rate of change) of the orientation angles and give no information on the absolute orientation.

Also, as I'm sure you are aware, gyros drift, and the offset is temperature dependent, so measured rates alone can never be enough to stabilize absolute orientation for more than a few dozen seconds.

There are established, recommended procedures for PID tuning, starting with all PID constants set to zero. Google "PID tuning" for tutorials.

5

In rate mode, commands from the pilot are used to set the rates at which the quadcopter pitches, rolls and yaws.

The PID loop compares the commands to the facts and adjusts the motor speeds to attain the desired rate of rotation about each of the three axes.

If you could grab a quadcopter in rate mode and force it into a new orientation, it would then stay at that orientation. Much like you could muscle a gyroscope - you would feel it trying to keep you from doing, but if so re-oriented, it would not try to get back to where it was.

Flying in rate mode is not the usual choice for cinematography or toys or beginning fliers. Most racers and freestyle pilots use rate mode. It does mean you have to actually fly the thing.

Contrast this to angle mode, where the quadcopter takes on an angle correlated to the stick position. Centering the sticks will return the quadcopter to level with the thrust vector pointing up.

Even in angle mode, yaw is still a rate-orientated command input. You do not aim a quadcopter with a fixed input, say a compass heading. Rather you command the quadcopter to rotate about its Z axis at a rate.

a7

1 Like
6

Sounds good, but what stabilizes that new orientation, if the gyro is the only input?

What is wrong with the OP's code?

7

I can't explain it any more. The gyros and PID loop work to keep the quadcopter from not rotating about any axis. Therefore the quadcopter should not change its orientation.

If some external force makes the quadcopter start rotating about any axis without control input commanding it to do, the quadcopter makes that stop. But does not make it go back to where it was orientation-wise.

Probably why you have to fly it in rate mode, that is to say the human becomes part of the larger feedback loop that maintains some absolute orientation if that is what is desired.

At first blush it sounds almost impossible, but in reality it is easier to fly in rate mode for some of the things that ppl use quadcopters to get up to.

As for why the @2anishr's quadcopter is misbehaving, I have no idea if there is a problem in the code, but I can say that tuning a PID loop for a quadcopter is as much art as it is science, and not at all easy.

Yes. Not even fine. Tuning full stop. There are a crap-ton of "how to tune" videos and tutorials, I suggest to you that some time spent googling may give you a recipe that would lead to success.

You can save crashing and destroying quads by constraining the quadcopter. It isn't too hard and if you google as much as I think you should, you will see clever people suspendering their aircraft variously and getting a basic rate-mode (gyro only) tuning in place and functioning well enough so you can fly with less possibility of cratching.

It's hard enough when one is using real flight control software and the ability to "black box" record a flight and review it. Here FWIW is a still shot from a tuning session:

blackBox
blackBox1813×1413 289 KB


We see the stick input asking for the quad to rotate in one direction, stop and rotate in the other direction. In the air, this might be pitching forward to upside down, pausing and then pitching back to level.

You can see the rear motors gear up to make the quad start rotating forward (pitching), then a burst of front motors to stop that rotation. On the other input pulse, the front motors do the same job of starting, then the rear motors stop the rotation. In between starting and stopping, the motors are controlled to leave the imposed rate unchanged.

The line "Gyro vs rcCommand" shows a classic response to two quick flicks on the elevator. Here it looks a bit underdamped and makes a little shiver as it hunts to bring the rotation rate to the commanded 0. On the other hand, the response at the leading edge of the flicks is smart and snappy, and gets right to rotating as commanding without delay, overshooting or oscillating.

In 2023, Betaflight out of the box is good enough that only those with time and a high need for getting the best possible tune even bother trying to mess with the defaults.

So @2anishr you are doing the hardest part of rolling your own software... I admire your motivation and hope you can get a flyable tune going.

If you just want to fly, consider the work you have done as very much fun and educational, and switch to Betaflight on one of the many very good and quite inexpensive FC boards available.

a7

8

Can you point me to code that successfully does that? I'm interested in the theory behind the operation!

The rate gyros provide no information about absolute orientation, so I suspect that there is a separate control loop that includes other IMU inputs, such as the accelerometer.

9

Only needed for angle mode flying, where the control input is not the rate at which the quadcopter is rotating, but the angles with respect to the world the operator wants to make quadcopter take on.

Betaflight is mature flight control software. In rate mode, the accelerometer is not used. In angle mode, a second PID loop exploits the rate PID loop, and works it to keep the quadcopter at the desired angle.

Silverware is a simpler but comprehensive flight control program. This guy forked from the inventor and has done quite a bit of good work enhancing it. But you can find the PID stuff easily enough:


A rate mode flyer asks the quad to pitch forward for a few fractions of a second, then returns elevator input to zero. The quad is now flying forward, and the pilot increases throttle to account for the fact that the thrust vector is no longer entirely available for maintaining altitude.

More pitch, more throttle, faster forward flight. In steady fast forward flight, the rudder, aileron and elevator input is zero.

To stop, a similar brief request to pitch back brings the thrust vector perpendicular to the ground. The pilot reduces throttle so the aircraft does not gain altitude. The aircraft hovers, or nearly so. Actually hovering is surprising hard - these things want to go and are much easier to control when moving.

Angle mode, with the accelerometer informing an outer PID loop , handles rate mode control and the operator (not flyer or pilot!) enjoys the ease of pushing forward on the stick to a degree and while held there seeing forward motion. Returning the stick to zero returns the angle of the quadcopter to level and forward motion stops. The software may do the throttle compensation automatically.

Rate mode pilots find it is a lot of work having to stay on the stick alla time to induce a steady state of motion be it forward and back or left to right.

I do not pretend to understand any better, and I may have simplified or got some details very wrong. I can say, however, that flying rate mode does not use the accelerator, and for certain kinds of flying is the mode of choice.

HTH

a7

10

Thanks very much for taking the time to post all that information.

The following makes perfect sense, if the outer loop integrates the gyro rates to get the 3D orientation angles (which appears to be missing from the OP's code). I'll take a look at the code that works.

11

Happy to help. The first time I saw someone flying rate mode, also known as "acro" I though he was doing something impossible.

With rate mode, all kinds of maneuvers become possible.

Missing perhaps by design. If the intent is to fly rate mode, the code may be sufficient. On the other hand, many rate mode pilots have a switch on the remote control to go to angle mode, which can be helpful when things get beyond the pilot's abilities. Switching on angle mode and keeping the throttle up returns the quadcopter to level and maybe rising.

In LOS line of sight flying, it is nearly impossible to recover from a loss of orientation if the quadcopter is at a distance. In angle mode, there is an easily performed set of steps guaranteed to leave the quadcopter flying towards the pilot.

a7

12

I doubt it. If you look through the OP's code, some parts make no sense. I'm not surprised that it doesn't work, which is why I asked for a statement of the theory behind it.

1 Like
13

What seems to be missing?

14

I'm just trying to fly rate mode right now. I may implement angle mode later on.

15

OK I have taken a high level flight about your code, and I have also looked at Carbon Aeronautics rate mode flight control code.

To motivate my reading of your code, I looked first to see if I could determine your loop frequency.

Missing or I didn't see was the thing that dictates that in CA's code, viz:

  while (micros() - LoopTimer < 4000);
  LoopTimer=micros();

There may be other places where you improvised without sufficient understanding of the original. This is not just a waste of time, but dangerous.

You may want to get familiar enough with CA's code so that you could write for just one axis and play with that on a mechanically constrained quadcopter.

This

  Serial.begin(9600);

wasn't even fast at the end of the 20th century, and any printing you do, especially in the absence of any loop time control, will cause timing issues and a world of pain.

In your circumstances, it is probably best to slavishly duplicate the entirety of a project that has produced known good results.

Then tuning may end up being the only problem you'd have to work on.

In what ways do your changes reflect your dissatisfaction with the original code you have heavily borrowed from?

a7

16

Your explanation of how and why the code should work.
Your description of what you have done to debug the problem.

17

I was getting another cold one out of the cooler here under the umbrella when it occurred to me to see if you were trying to use an UNO or other slow processor.

The code you are working was written for a Teensy. Comparing the specifications of the two makes me think there is no way the code will ever perform satisfactorily on a Mega.

If you need to use a slow processor like a Mega, find code that anyone ever got anything off the ground with that.

I don't say it does not exist, just that other than broad brush structurally and theoretically, the details will need to be quite different.

a7

18

OK, I found the original code and some explanation of the theory.

The original code assumes a loop time of 0.004 seconds, but does not measure or enforce that value. That timing would be incorrect for a different, slower processor, so any calculation assuming that loop time (as done in the OP's code) will give incorrect results.

The original code, with all of its magic numbers and hidden assumptions, is a poor example to imitate.

1 Like
19

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