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.

Drone build completed

Author: Jonah Siekmann

It’s been a while since we updated this blog on the drone build progress, but today the last part came in and the drone is officially flying! We still have to mount the RFID/GPS stack, so it’s not autonomous, but it’s flying very smoothly.

For the drone, we used:

CC3D Flight Controller

750kv 28-30 motors

30A SimonK ESCs

11″ propellors

5000mAh 4s 25c battery

Flysky T6 transmitter/receiver

S500 frame

Here is a short video showing part of its maiden flight:

Also, you may recall in the last drone update, we documented an issue where the Arduino Uno microcontroller would shut off mid-flight – this was because of a faulty UBEC that wasn’t supplying a steady 5v, forcing the Arduino’s internal regulator to work overtime and thus overheating. Replacing the UBEC solved the issue. 

Now, we just have to program the autonomous navigation portion of the drone and mount the Arduino/RFID/GPS stack. 

Asynchronously Reading a Sonar Distance Sensor

Author: Jonah Siekmann

To get the quadcopter to fly at a set altitude above crops, we need to use a distance sensor (or more likely, a combination of distance sensors). We’re going to use a sonar distance sensor for at least part of the altitude calculation. However, the speed of sound is pretty slow. In the time it takes the sensor to send a ping and receive the echo, dozens of milliseconds could have passed. So we need to read it asynchronously. This turned out to be a little trickier than I first thought, so I made an instructable on it. You can read it here:

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

Building a Drone

Author: Jonah Siekmann

For this project, we’ll need to construct a drone that will carry the RFID reader around a crop field. It’ll need to remember the locations of every new RFID ‘Dogbones’ moisture sensor it encounters, and store the GPS coordinates so that it can return to the sensor later. We’re not sure yet how much weight it’ll need to carry, which leaves a lot of the the specs of the drone up in the air. We’ve come up with a general parts list, however, which includes the following:

CC3D Flight Controller

750KV Motor (x4)

30A ESC (x4)

4000mah 3s battery

12×4.5″ propellors

650mm frame

Arduino Uno

GPS module

RC Transmitter/Receiver

Our idea for flying the drone autonomously involves hooking up the Arduino to the CC3D flight controller’s RC receiver pins, and simulating the 1000-2000us pulses that the flight controller would expect from a receiver with the Arduino. This way, the CC3D flight controller does all the low-level attitude hold and hovering work, while the Arduino is free to read the GPS sensor and control the position of the quad by pretending to be an RC receiver.