Developing an altitude hold algorithm for a quadcopter

Posted on by

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.

Abstract

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:

https://www.instructables.com/id/Asynchronously-Reading-the-HC-SR04-Sensor/

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

Results

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.