Cascaded PID controller for a Quadrotor in C++.

First, we will implement the body rate and roll / pitch control.
For the simulation, you will use Scenario 2. In this scenario, we will see a quad above the origin.
It is created with a small initial rotation speed about its roll axis. Our controller will need to stabilize the rotational motion and bring the vehicle back to level attitude.

To accomplish this, we will:

1. Implement body rate control

• implement the code in the function GenerateMotorCommands()
• implement the code in the function BodyRateControl()
• Tune kpPQR in QuadControlParams.txt to get the vehicle to stop spinning quickly but not overshoot

successful, we should see the rotation of the vehicle about roll (omega.x) get controlled to 0 while other rates remain zero. Note that the vehicle will keep flying off quite quickly, since the angle is not yet being controlled back to 0. Also note that some overshoot will happen due to motor dynamics!.

2. Implement roll / pitch control We won't be worrying about yaw just yet.

• implement the code in the function RollPitchControl()
• Tune kpBank in QuadControlParams.txt to minimize settling time but avoid too much overshoot

successful, we should now see the quad level itself (as shown below), though it’ll still be flying away slowly since we’re not controlling velocity/position! You should also see the vehicle angle (Roll) get controlled to 0.

Next, we will implement the position, altitude and yaw control for your quad. For the simulation, we will use Scenario 3. This will create 2 identical quads, one offset from its target point (but initialized with yaw = 0) and second offset from target point but yaw = 45 degrees.

• implement the code in the function LateralPositionControl()
• implement the code in the function AltitudeControl()
• tune parameters kpPosZ and kpPosZ
• tune parameters kpVelXY and kpVelZ

successful, the quads should be going to their destination points and tracking error should be going down (as shown below). However, one quad remains rotated in yaw.

• implement the code in the function YawControl()
• tune parameters kpYaw and the 3rd (z) component of kpPQR

Tune position control for settling time. Don’t try to tune yaw control too tightly, as yaw control requires a lot of control authority from a quadcopter and can really affect other degrees of freedom. This is why you often see quadcopters with tilted motors, better yaw authority!

Hint: For a second order system, such as the one for this quadcopter, the velocity gain (kpVelXY and kpVelZ) should be at least ~3-4 times greater than the respective position gain (kpPosXY and kpPosZ).

In this part, we will explore some of the non-idealities and robustness of a controller. For this simulation, we will use Scenario 4. This is a configuration with 3 quads that are all are trying to move one meter forward. However, this time, these quads are all a bit different:

• The green quad has its center of mass shifted back
• The orange vehicle is an ideal quad
• The red vehicle is heavier than usual

1. Run controller & parameter set from Step 3. Do all the quads seem to be moving OK? If not, try to tweak the controller parameters to work for all 3 (tip: relax the controller).

2. Edit AltitudeControl() to add basic integral control to help with the different-mass vehicle.

3. Tune the integral control, and other control parameters until all the quads successfully move properly. Your drones' motion should look like this:

The specific performance metrics are as follows:

• scenario 2

  • roll should less than 0.025 radian of nominal for 0.75 seconds (3/4 of the duration of the loop)
  • roll rate should less than 2.5 radian/sec for 0.75 seconds

• scenario 3

  • X position of both drones should be within 0.1 meters of the target for at least 1.25 seconds
  • Quad2 yaw should be within 0.1 of the target for at least 1 second

• scenario 4

  • position error for all 3 quads should be less than 0.1 meters for at least 1.5 seconds

• scenario 5

  • position error of the quad should be less than 0.25 meters for at least 3 seconds