Questions About Self leveling arduino flight controllers.

after doing some work, i got my arduino quadcopter to fly in rate mode but im having a little trouble with angle mode. Mainly because i have a few questions that i haven't been able to find the answer to online.

  1. When you create two inner PID loops, how fast do each one has to be? right now my rate controller runs once every 4ms but i dont know how fast should the Angle Pid run.

  2. Should the angle PID has its own separate PID gains or should both the rate loop and the angle loop use the same terms?

3.to be clear, the corrections that the angle loop produces should be used as a setpoint and create an error for the rate loop to correct, right? so its more or less like this :

{ //loop once every 4 ms// this would be gyro loop
  
         {  // loop once every ??ms // this would be angle loop
        
             angle error=pilotInput-currentAngle
              .
              .//rest of pid
              .
               Pitch_Input=......
               Roll_Input=......
               yaw_Input=......
           }

gyroError = Pitch_Input-CurrentGyro
.
.
.//rest of gyro pid.
}

so generally, this would be how you can get the error to flow from the Angle loop to the gyro loop right?

Hi vortix2950,

Congratulations on getting your quadcopter flying in rate mode by the way.

  1. When you create two inner PID loops, how fast do each one has to be? right now my rate controller runs once every 4ms but i dont know how fast should the Angle Pid run.

As it's possible to sample the gyroscope and accelerometer at the same rate, (up to 1000 times per second using the MPU6050 with filtering), just run both the outer angle and inner rate PID loops as fast as your microcontroller allows. 4ms is about right for rate mode using an Uno/Nano/Pro Micro with floating point PIDs.

  1. Should the angle PID has its own separate PID gains or should both the rate loop and the angle loop use the same terms?

The angle PIDs for roll and pitch should have their own gain values. I only use the P gain term for these outer control loops. For a PID controller using angle in degrees, I set the P term to a value of 3.0.

3.to be clear, the corrections that the angle loop produces should be used as a setpoint and create an error for the rate loop to correct, right?

Yes that is correct. The key point is that a PID control loops' output doesn't have an units as such, it's just a magnitude. In this case, the outputs of the outer angle PIDs control the inner rate PIDs.

Angle Estimation using the Complementary Filter

As you mention you need two outer PID control loops for roll and pitch. In auto-level mode the pilot input for the roll and pitch channels are converted to angle, say ±45° instead and act as the "setpoint". The "input" to these control loops is the estimated roll and pitch angle. The output from these outer control loops is used to drive the corresponding inner rate loop. The yaw axis remains unchanged from rate mode and as before is driven by the pilot "setpoint" and gyrscope "input" in degrees per second using a single rate PID control loop.

Angle estimation opens up a how whole new world of mathematics, sensor fusion and filters techniques, such a Direction Cosine Matrices (DCM), Quaternions and Kalman filtering. Fortunately however, it's possible to produce a good angle estimation using some simpler techniques.

It's possible to calculate the roll and pitch angle by integrating the gyroscope output, but while gyroscopes are stable over the short term, they tend to drift over the long term. It's also possible calculate the roll and pitch angle by employing an accelerometer to measure gravity and using a bit of trigonometry, but while the accelerometer is noisy over the short term, it does however remain stable over the long term. In short both sensors are able to measure angle, but their measurement characteristics are somewhat different.

The solution is to combine or fuse the sensors using a complementary filter, to take advantage of their beneficial characteristics. The filter estimates the angle by summing the previous angle with a percentage of the integrated gyroscope and accelerometer outputs:

angle estimation = previous angle + 0.999 * gyroscope output * dt + 0.001 * accelerometer output

It's worth reading these articles to get a feel for how it works:

Also, there's the additional complication of yaw coupling. In reality roll, pitch and yaw are not independent variables. DCM and quaternion mathematics with advanced filtering accounts for this, but simply using roll, pitch and yaw Euler angles with a complementary filter does not. For example imagine your quadcopter is hovering level, if you pitch up the nose by 45° and then yaw to the left 90°, you're now actually in the roll position at 45°. The issue is that during the yaw motion the gyroscope cannot sense the change in roll and pitch. The accelerometer does sense this change, but as complementary filter is only taking a tiny 0.1% (0.001) fraction of its output, it takes too long for the filter to react. Therefore it's necessary to cross couple roll and pitch motion when yawing, this is detailed in the article here:

MartinL:
Hi vortix2950,

Congratulations on getting your quadcopter flying in rate mode by the way.

As it's possible to sample the gyroscope and accelerometer at the same rate, (up to 1000 times per second using the MPU6050 with filtering), just run both the outer angle and inner rate PID loops as fast as your microcontroller allows. 4ms is about right for rate mode using an Uno/Nano/Pro Micro with floating point PIDs.

The angle PIDs for roll and pitch should have their own gain values. I only use the P gain term for these outer control loops. For a PID controller using angle in degrees, I set the P term to a value of 3.0.

Yes that is correct. The key point is that a PID control loops' output doesn't have an units as such, it's just a magnitude. In this case, the outputs of the outer angle PIDs control the inner rate PIDs.

Angle Estimation using the Complementary Filter

As you mention you need two outer PID control loops for roll and pitch. In auto-level mode the pilot input for the roll and pitch channels are converted to angle, say ±45° instead and act as the "setpoint". The "input" to these control loops is the estimated roll and pitch angle. The output from these outer control loops is used to drive the corresponding inner rate loop. The yaw axis remains unchanged from rate mode and as before is driven by the pilot "setpoint" and gyrscope "input" in degrees per second using a single rate PID control loop.

Angle estimation opens up a how whole new world of mathematics, sensor fusion and filters techniques, such a Direction Cosine Matrices (DCM), Quaternions and Kalman filtering. Fortunately however, it's possible to produce a good angle estimation using some simpler techniques.

It's possible to calculate the roll and pitch angle by integrating the gyroscope output, but while gyroscopes are stable over the short term, they tend to drift over the long term. It's also possible calculate the roll and pitch angle by employing an accelerometer to measure gravity and using a bit of trigonometry, but while the accelerometer is noisy over the short term, it does however remain stable over the long term. In short both sensors are able to measure angle, but their measurement characteristics are somewhat different.

The solution is to combine or fuse the sensors using a complementary filter, to take advantage of their beneficial characteristics. The filter estimates the angle by summing the previous angle with a percentage of the integrated gyroscope and accelerometer outputs:

angle estimation = previous angle + 0.999 * gyroscope output * dt + 0.001 * accelerometer output

It's worth reading these articles to get a feel for how it works:

http://www.geekmomprojects.com/gyroscopes-and-accelerometers-on-a-chip/

Shane Colton: KK2.0 Firmware Mod: Unleash the Complementary Filter

Also, there's the additional complication of yaw coupling. In reality roll, pitch and yaw are not independent variables. DCM and quaternion mathematics with advanced filtering accounts for this, but simply using roll, pitch and yaw Euler angles with a complementary filter does not. For example imagine your quadcopter is hovering level, if you pitch up the nose by 45° and then yaw to the left 90°, you're now actually in the roll position at 45°. The issue is that during the yaw motion the gyroscope cannot sense the change in roll and pitch. The accelerometer does sense this change, but as complementary filter is only taking a tiny 0.1% (0.001) fraction of its output, it takes too long for the filter to react. Therefore it's necessary to cross couple roll and pitch motion when yawing, this is detailed in the article here:

Shane Colton: Fun with the Complementary Filter / MultiWii

thank you, i also have a problem picking values for the Max/min for the Corrections after the PID. right now they are set at 200 but that is arbitrary. i dont know if it should be high or lower or how i determine that.

thank you, i also have a problem picking values for the Max/min for the Corrections after the PID. right now they are set at 200 but that is arbitrary. i dont know if it should be high or lower or how i determine that.

Good question, it's the little details like this that are often the most difficult. I pondered over this issue as well.

I decided to also arbitrarily set the PID control loops' min/max limit (after summing the P, I and D terms) to ±1000 for roll/pitch and ±500 for yaw. I also constrained the PWM output to the RC receiver throttle min/max limits as well, just before ouputting the signal to the motors. A standard RC receiver's throttle limits are usually around 1100us and 1900us and the ESCs are normally calibrated to these limits.

This gives the PID control loops free rein to act over the 1000us to 2000us (or 1100us to 1900us using a standard RC receiver) about the throttle value. In practice it turns out that the limits are not too much of an issue for quadcopters, as they are able to react quickly to the pilot's commands, thereby preventing the PID error (setpoint - input) from becoming too large. Limiting the the PID output is probably more appropriate for aircraft controlling servos, such as the yaw servo on a tricopter, where the min/max output limits prevent the servo from turning too far.

There's also the question of limiting the integral windup, or in other words limiting the I term of a PID control loops. Again this isn't too much of an issue with quadcopters during flight (for the same reason as above), but can be a problem when the aircraft is on the ground and unable to react. The traditional way of preventing this is to have a throttle cutoff point at say 10%, below which the PID control loops are deactivated and the control loop I terms are reset to 0. I arbitrarily set my I term limits to around ±500 for pitch/roll and ±100 for yaw.

MartinL:
Good question, it's the little details like this that are often the most difficult. I pondered over this issue as well.

I decided to also arbitrarily set the PID control loops' min/max limit (after summing the P, I and D terms) to ±1000 for roll/pitch and ±500 for yaw. I also constrained the PWM output to the RC receiver throttle min/max limits as well, just before ouputting the signal to the motors. A standard RC receiver's throttle limits are usually around 1100us and 1900us and the ESCs are normally calibrated to these limits.

This gives the PID control loops free rein to act over the 1000us to 2000us (or 1100us to 1900us using a standard RC receiver) about the throttle value. In practice it turns out that the limits are not too much of an issue for quadcopters, as they are able to react quickly to the pilot's commands, thereby preventing the PID error (setpoint - input) from becoming too large. Limiting the the PID output is probably more appropriate for aircraft controlling servos, such as the yaw servo on a tricopter, where the min/max output limits prevent the servo from turning too far.

There's also the question of limiting the integral windup, or in other words limiting the I term of a PID control loops. Again this isn't too much of an issue with quadcopters during flight (for the same reason as above), but can be a problem when the aircraft is on the ground and unable to react. The traditional way of preventing this is to have a throttle cutoff point at say 10%, below which the PID control loops are deactivated and the control loop I terms are reset to 0. I arbitrarily set my I term limits to around ±500 for pitch/roll and ±100 for yaw.

Alright thank you, The Rate controller seem to work but the Angle seems to have issues. One issue is that for an unknown reason, if i run the Angle loop, the Pitch input switches values so i have to multiply it by -1. Second problem is that no matter what Pid i choose, its never seems to stabilize. its often oscillating like the P is to high but the P is something like 0.000004. i tried quite a few values and the closest i ever got seems to behave like the Rate controller which defeats the point of using it.

float Porpotional_gain=0.092;    //best value so far //0..095  // .092
float Integral_gain=0.17;     //best value so far//0.16399  // .168
float Derivative_gain=0.63;   //best value so far  //0.606   //63


float Porpotional_gain_A=0.007;
float Integral_gain_A=0.0001; 
float Derivative_gain_A=0.00005;  



 if(MessTime3-HoldTime3>interval3){// 40ms

      HoldTime3=MessTime3;
    Error_Pitch_A=Desired_Pitch-((ypr[1]*(180/M_PI)));  // Angles from the MPU using DMP
    Error_Roll_A=Desired_Roll-((ypr[2]*(180/M_PI)));  //Desired are Values from Transmitter.
    Error_Yaw_A=Desired_Yaw-((ypr[0]*(180/M_PI)));
    ;
    
    Integral_Pitch_A+=Integral_gain_A*Error_Pitch_A; 
    Integral_Roll_A+=Integral_gain_A*Error_Roll_A;
    Integral_Yaw_A+=Integral_gain_y*Error_Yaw_A;
    
    Derivative_Roll_A=Derivative_gain_A*((Error_Roll_A-Prev_Error_Roll_A)); 
    Derivative_Pitch_A=Derivative_gain_A*((Error_Pitch_A-Prev_Error_Pitch_A));
    Derivative_Yaw_A=Derivative_gain_y*((Error_Yaw_A-Prev_Error_Yaw_A));
    
    Input_Pitch_A=(Porpotional_gain_A*Error_Pitch_A)+Integral_Pitch_A+Derivative_Pitch_A; 
    Input_Roll_A=(Porpotional_gain_A*Error_Roll_A)+Integral_Roll_A+Derivative_Roll_A;
    Input_Yaw_A=(Porpotional_gain_y*Error_Yaw_A)+Integral_Yaw_A+Derivative_Yaw_A;
 
    Prev_Error_Pitch_A=Error_Pitch_A;
    Prev_Error_Roll_A=Error_Roll_A;
    Prev_Error_Yaw_A=Error_Yaw_A;
      }

    Input_Pitch_A=Input_Pitch_A*-1;
   
    Error_Pitch=Input_Pitch_A-CurrentReading_Pitch; // CurretReading Are From Gyro
    Error_Roll=Input_Roll_A-CurrentReading_Roll;
    Error_Yaw=Input_Yaw_A-CurrentReading_Yaw;
    
    Integral_Pitch+=Integral_gain*Error_Pitch; 
    Integral_Roll+=Integral_gain*Error_Roll;
    Integral_Yaw+=Integral_gain_y*Error_Yaw;
    
    Derivative_Roll=Derivative_gain*((Error_Roll-Prev_Error_Roll)); 
    Derivative_Pitch=Derivative_gain*((Error_Pitch-Prev_Error_Pitch));
    Derivative_Yaw=Derivative_gain_y*((Error_Yaw-Prev_Error_Yaw));
    
    Input_Pitch=(Porpotional_gain*Error_Pitch)+Integral_Pitch+Derivative_Pitch; 
    Input_Roll=(Porpotional_gain*Error_Roll)+Integral_Roll+Derivative_Roll;
    Input_Yaw=(Porpotional_gain_y*Error_Yaw)+Integral_Yaw+Derivative_Yaw;

int limit=200;
if(Input_Pitch>limit){Input_Pitch=limit;}
if(Input_Roll>limit){Input_Roll=limit;}
if(Input_Yaw>limit){Input_Yaw=limit;}
if(Input_Pitch<-1*limit){Input_Pitch=-1*limit;}
if(Input_Roll<-1*limit){Input_Roll=-1*limit;}
if(Input_Yaw<-1*limit){Input_Yaw=-1*limit;}

 
    Motor_1_Speed=(MotorPower_Throttle)+Input_Roll+Input_Pitch-Input_Yaw;//+ 
    Motor_3_Speed=(MotorPower_Throttle)-Input_Roll+Input_Pitch+Input_Yaw;//-
    Motor_2_Speed=(MotorPower_Throttle)-Input_Roll-Input_Pitch-Input_Yaw;//+
    Motor_4_Speed=(MotorPower_Throttle) +Input_Roll-Input_Pitch+Input_Yaw;//-