Developing an altitude hold algorithm for a quadcopter

Author: Jonah Siekmann

For the past few months, the drone portion of the RFID moisture project has been focused on the altitude hold algorithm of the drone flight controller as a sort of first step towards autonomy.


Currently, the user is responsible for throttle, pitch, roll and yaw – in theory, altitude hold would remove the need for the user to moderate throttle while GPS navigation would remove the need for the other three. The user would just need to flip a switch on the remote control, and the drone would use a sonar sensor to determine the altitude from the ground. The sonar is mounted on a gimbal so that regardless of the drone’s orientation, it is always pointing at the ground.

Materials & Description

We constructed a gimbal from two digital servos and used an MPU 6050 to get positional data. We also used an HC-SR04 sonar sensor and arduino to process the data. Initially, the sonar sensor was located directly underneath two propellors, which resulted in propwash interfering with the sensor receiving pings. This meant the flight controller could go for seconds without a single reading indicating its position relative to the ground – an extremely unsafe predicament. It could have crashed or flown dozens of meters into the air in the meantime.

 Some online research revealed that the best place for a sonar sensor was in the exact center of the frame. So the drone’s battery, Arduino, and gimbal were removed and reassembled in a spot that allowed the gimbal to be in the center. The result looks like this:

This allowed for much more consistent readings. Afterward, the problem arose of using a PID controller that assumed a constant time interval to control drone altitude. Normally, a PID controller looks something like this.

  integral += error; //integrate errors by adding them together
  derivative = (error - prevError); //get derivative by finding difference
  prevError = error;
  output = KP * error + KI * integral + KD * derivative; //multiply by gains

And this worked fine for previous flight controllers that I’d written. However, this is because they used a constant loop time of exactly 4ms – meaning that the flight controller would poll sensors for new data exactly every 4ms.

However, in the case of a sonar sensor, the time it takes to get a reading depends on how far away an object is. If a drone is hovering 20 feet in the air, it can take around 40ms to get a ping back from the ground – an extremely long amount of time in computational terms. So instead, an interrupt subroutine was used as described in this instructable:

However, this meant that the time in between readings is not constant and can vary hugely. For a PID controller, this is a massive problem – say two readings are taken, the first at 3m and the second at 2m. If there is a one second difference between those readings, then they can be taken to mean that the drone is dropping at 1m/s and needs to be corrected immediately. If there is a ten second difference, it is only dropping at 0.1m/s and the correction should be a lot less forceful. However, the PID controller as above would consider both of these cases to be identical and react inappropriately.

Essentially, it’s necessary to take time into account when reading data from a sensor that does not provide readings at a constant rate. The resulting PID subroutine looks like this:

  int dt = micros() - timeOfLastExecution;
  timeOfLastExecution = micros();

  integral += error * dt; //multiply error by time interval since last reading
  derivative = (error - prevError)/dt; //divide rise over run - dY/dt
  prevError = error;
  output = KP * error + KI * integral + KD * derivative; //multiply by gains


This steps described in this post allowed the altitude hold algorithm to be realized. Below is a video of a rough working version of the altitude hold algorithm – the PID values are not yet tuned. The throttle was not touched for the entirety of the following video after about 0:07 seconds – the only axes controlled were pitch and roll.

Getting in the air

Author: Jonah Siekmann

A few weeks ago, I started putting together various different quadcopter builds and comparing cost, efficiency, and lifting power. I’ve come up with a few builds that I think are pretty solid:

I also had two drones laying around in my dorm that I’d built the previous year, and I figured I could use those to do some tests on how long they could stay in the air with a payload of a given weight, since that would give us concrete data on what specs might or might not work.

However, to do that I needed to rewrite the flight controller I’d written previously, because the old version used rate mode – which means that the quad didn’t know which way was up, and it was up to the user to make sure it stayed pointing in the right direction. To carry a payload (and eventually navigate autonomously) I needed to make the quadcopter autolevel – meaning the flight controller needs to know which way is up, and can stay pointing up (instead of say, pitched 30 degrees forward). 

So for the last few days, I’ve been putting together a new flight controller paired with an IMU (instead of a standalone gyroscope) and trying to get that working. I had an issue that kept me grounded for about half a week where the MPU6050 (the IMU I’m using) wouldn’t start up unless the Arduino Uno was plugged in through USB – I’m still not sure what exactly was causing that, but I wired it up to a UBEC and it worked fine.

Today, I flew the drone with a payload roughly the same weight as the RFID reader we’ll be using – about 750 grams. The drone flew surprisingly well with the extra weight, although it was still very difficult to manage – the PID values are tuned for a drone about 750 grams lighter. Here’s a picture of me trying my best to keep it in one spot (hence the facial expression):

However, I encountered an issue where it seems like the Arduino Uno shuts off randomly, while leaving the motors spinning – usually resulting in the drone flying into a wall or nearby bush. This isn’t a very desirable outcome when carrying a sensor package worth almost $500, so I need to figure out what’s causing that before we do any testing with it. I have a feeling it’s due to a crappy solder connection on my part, so over spring break I think I’ll just rebuild the whole thing and see if it keeps happening.

-Jonah Siekmann, URSA researcher