How I Developed the Scout Flight Controller, Part 7: Full Flight Controller Code

This article is part of a series documenting the development of a custom quadcopter flight controller from scratch. For a list of the other related articles, please refer to the introductory article found here.

Upload the Scout Flight Controller code to your quadcopter microcontroller

Having now covered both the core software concepts that allow a quadcopter to stay airborne and achieve stable flight and the hardware that is required, we are now prepared to take a look at the complete flight controller code. This code stitches together each core concept we discussed earlier.

Full Scout Flight Controller Code

The full code behind the Scout Flight Controller can be found below. I will break down and explain each piece of the code in subsequent sections.

Design Considerations

Before we begin to dissect each piece of the code above, it is important to note several important design considerations I had to keep in mind when developing this software:

This code needs to run quickly — I mentioned in previous articles that the Scout Flight Controller runs the PID adjustment loop at 250 Hz, or 250 times per second. I learned that achieving this speed is a challenge using MicroPython on the inexpensive Raspberry Pi Pico’s CPU hardware. In contrast to the relatively slow MicroPython, if Scout were implemented in C/C++, I’m sure the code would execute far more efficiently, possible 10x+ faster.

To achieve these speeds, you’ll notice that I am not using the basic principles of programming — modularity, reusability, etc. I am keeping my usage of classes, functions, and modules to the bare minimum. This is because usage of these simple Python concepts actually requires a bit of extra processing time that would slow the execution of the code down enough for the 250 Hz adjustment loop to be out of reach. So, while in past articles in this series I shared class definitions for things like reading gyroscope telemetry from the MPU-6050 via I2C, I’m actually not using that class in my code; instead, I am extracting the logic from that class and applying it in a procedural manner in the flight controller code. The same goes for modules; while it would make more sense to store some functionality in different modules for the purpose of organization, this would result in minor performance losses that I simply could not afford. For this reason, the code is (mostly) optimized for performance, not readability, though I did my best to add comments in the code where appropriate and below.

I found that, even with the performance optimizations mentioned above, the code still runs too slowly to achieve the 250 Hz loop target. So, the Scout Flight Controller overclocks the Raspberry Pi Pico’s RP2040 processor from 125 MHz to 250 MHz, double its rated speed. From my experience, this has not caused any adverse effects whatsoever. The power consumption, temperature, reliability, etc., still appear to be normal.

This code is self-contained. Besides the ibus module that is used to read and interpret signals from the onboard FlySky FS-i6 receiver that we discussed in a previous chapter on, this code can run independently of any third party libraries. The base (built-in) modules of the MicroPython implementation is all that is required.

There are two critical safety mechanisms built in. This is code that controls four spinning 8-inch propellers that spin at several thousand RPMs running on an untethered lightweight frame. This is inherently dangerous and poses significant risks to the safety and wellbeing of you and those around you. To prevent a simple mistake in control input from creating a catastrophic accident (like a high-throttle command being sent accidentally), I included two safety mechanisms in this code: firstly, the flight controller will cease to operate if the “flight mode” switch is in the on position as soon as the flight controller gains power and boots up. Secondly, the flight controller will cease to operate if the throttle is in any position other than 0% as soon as the flight mode is switched from standby to flight. I’ll explain these two safety mechanisms in further detail below.

Settings

########################################
###########  SETTINGS  #################
########################################

# Motor GPIO's (not pin number, GPIO number) ###
gpio_motor1 = 2 # front left, clockwise
gpio_motor2 = 28 # front right, counter clockwise
gpio_motor3 = 15 # rear left, counter clockwise
gpio_motor4 = 16 # rear right, clockwise

# i2c pins used for MPU-6050
gpio_i2c_sda = 12
gpio_i2c_scl = 13

# RC-receiver UART
# either a 0 or 1 (Raspberry Pi Pico supports two UART interfaces)
rc_uart = 1

# throttle settings
throttle_idle:float = 0.14 # the minumum throttle needed to apply to the four motors for them all to spin up, but not provide lift (idling on the ground). the only way to find this value is through testing (with props off).
throttle_governor:float = 0.22 # the maximum throttle that can be applied. So, if the pilot is inputting 100% on the controller, it will max out at this. And every value below the pilot's 100% will be scaled linearly within the range of the idle throttle seen above and this governor throttle. If you do not wish to apply a governor (not recommended), set this to None.

# Max attitude rate of change rates (degrees per second)
max_rate_roll:float = 30.0 # roll
max_rate_pitch:float = 30.0 # pitch
max_rate_yaw:float = 50.0 # yaw

# Desired Flight Controller Cycle time
# This is the number of times per second the flight controller will perform an adjustment loop (PID loop)
target_cycle_hz:float = 250.0

# PID Controller values
pid_roll_kp:float = 0.00043714285
pid_roll_ki:float = 0.00255
pid_roll_kd:float = 0.00002571429
pid_pitch_kp:float = pid_roll_kp
pid_pitch_ki:float = pid_roll_ki
pid_pitch_kd:float = pid_roll_kd
pid_yaw_kp:float = 0.001714287
pid_yaw_ki:float = 0.003428571
pid_yaw_kd:float = 0.0

########################################
########################################
########################################

In this first section of code, several programmable setting variables are established. These are values that the subsequent flight controller code will use but that you may want to readily change.

The first portion of this settings section is where we designate the GPIO (“general purpose input/output”) that we will map each motor to. I have the signal wire of each motor directly soldered to pins 2, 28, 15, and 16 sequentially. The specific GPIO pins that are used to communicate with the MPU-6050 via I2C are also noted.

The UART protocol is used to read input commands from the onboard FlySky FS-i6 RC receiver. The Raspberry Pi Pico has two UART interfaces and the interface being used is noted here. I am using 1 because I half the FS-i6 signal wire soldered directly to the GP5 pin of the Raspberry Pi Pico.

Next, the idle and maximum throttle in flight mode is defined. Here, the throttle_idle variable is set to the minimum percentage (14% in this case) that spins each of the four motors but does not produce enough lift for the quadcopter to take off. You will need to find this value through experimentation. The throttle_governor variable is the maximum throttle percentage that should be applied to each of the four motors if the pilot is indicating 100% of the available power via the RC controller. In other words, if the pilot has the throttle stick pushed all the way forward (100% on their controller), the power output to each of the four motors will actually be limited to this governor, of 22% in this case. As mentioned in previous chapters, the motors I used are excessively powerful. To make controlling piloting the quadcopter easier, this governor is implemented.

Next, the maximum roll, pitch, and yaw rates are defined. This is the maximum rate at which the quadcopter can adjust its roll, pitch, or yaw angle. Meaning, in my case, if the pilot pushes the attitude stick all the way forward (indicating a maximum pitch forward), the quadcopter will change its attitude at a rate of 30 degrees per second. The same applies for pitch and yaw with their respective values.

Next, the target cycle time for the internal adjustment loop (“PID loop” as discussed in previous chapters). As mentioned, the Scout Flight Controller targets an adjustment rate of 250 times per second.

Finally, the PID controller parameters are defined. As mentioned previously, each PID controller for each of the three axes requires three variables that control its behavior — a proportional gain, integral gain, and derivative gain. These three variables determine how much influence each controller has on the aggregate output of the PID controller.

**This is not a “one size fits all” approach — the appropriate PID controller gains vary from one quadcopter to another. The values need to be tuned to accommodate the differences in weight, weight distribution, size, motor power, battery power, etc.

For determining the PID values for your quadcopter, I recommend using the proportions of the PID gains I have above to start your tuning process:

1. Take the props off
2. Start by setting each of the PID gains to 50% of the value I have above. Run this in flight mode and ensure the motors do not behave too erratically.
3. Put the props on. Set each PID gain to 25% of the values I have above (proportionally). Run the quadcopter in flight mode and move it around in your hand (be careful of the props!). You should feel the quadcopter fight to stay level.
4. Increase to 50%. Validate.
5. Increase to 75%. Validate.
6. Increase to 100%, or whatever percentage works best for your quadcopter. You may need a PID gain settings well beyond 100% of what I have above, based on the unique attributes of your quadcopter.

When PID tuning, please be extremely careful. Only increase the PID gains gradually and test frequently as you do. Placing only one of the PID gain settings too high can result in catastrophe as the motors spin out of control.

Startup Routine

The full flight controller program, including the PID loop, is enclosed within the run function. The run functions starts with several setups:

1. The Raspberry Pi Pico’s onboard LED is flashed several time to indicate to the user that the microcontroller has been powered on and the Scout Flight Controller is running.
2. The CPU is overclocked to 250 MHz.
3. An instance of the IBus class is created; this will later be used to receive input commands from the pilot.

Safety Mechanism #1: Flight Mode Switch Position

As part of the program boot phase (before the PID loop), a critical safety check is performed:

print("Validating that mode switch is not in flight position...")
for x in range(0, 60):
    rc_data = rc.read()
    if rc_data[5] == 2000: # flight mode on
        FATAL_ERROR("Flight mode detected as on (from RC transmitter) as soon as power was received. As a safety precaution, mode switch needs to be in standby mode when system is powered up.")
    time.sleep(0.025)

The value of the fifth channel from the RC receiver is checked sixty times. If it is noticed that the switch is in the “on” position (value of 2000 as opposed to a value of 1000, indicating an “off” position), the program is aborted. An error message is logged to the device’s flash memory and an infinite loop pulsates the onboard LED in a known pattern indicating this.

This is an important safety mechanism. The serves to prevent the quadcopter from instantly spinning up its motors when the pilot may not be expecting it (the channel 5 switch being in the “on” position indicates the quadcopter should be in flight mode, at the very least idling the propellers).

MPU-6050 Initialization

The MPU-6050 is a critical component of a quadcopter flight controller that must function properly to achieve any semblance of stable, predictable flight. In a previous chapter on the gyroscope, I provided a lightweight class that can be used to interface with an onboard MPU-6050 accelerometer & gyroscope. For the sake of code performance, the Scout Flight Controller does not use this class. It instead communicates with the MPU-6050 procedurally.

The MPU-6050 is set up like so:

# Set up IMU (MPU-6050)
i2c = machine.I2C(0, sda = machine.Pin(gpio_i2c_sda), scl = machine.Pin(gpio_i2c_scl))
mpu6050_address:int = 0x68
i2c.writeto_mem(mpu6050_address, 0x6B, bytes([0x01])) # wake it up
i2c.writeto_mem(mpu6050_address, 0x1A, bytes([0x05])) # set low pass filter to 5 (0-6)
i2c.writeto_mem(mpu6050_address, 0x1B, bytes([0x08])) # set gyro scale to 1 (0-3)

In the above code, the I2C interface is set up that will be used to communicate with the MPU-6050 via the I2C protocol. Three messages are sent to the MPU-6050 to configure settings:

1. 0x01 is written to the 0x6B register, waking the MPU-6050 up from a low-power mode.
2. 0x05 is written to the 0x1A register, turning the MPU-6050's onboard low pass filter to level 5.
3. 0x08 is written to the 0x1B register, setting the gyro scale factor to a level of 1.

Several tests are then conducted to validate that the MPU-6050 responded to the write commands above and is working as expected:

1. One byte is read from register 0x75, the "WHOAMI" register. The MPU-6050 should always return 0x68, or 104, the I2C address that the manufacturer sets. This is a simple way of confirming that bidirectional communication is working.
2. The low pass filter value is validated to be 0x05 (level 5) in the 0x1A register.
3. The gyro scale factor is validated to be 0x05 in the 0x1B register.

If any of these three tests fail, an error message is logged and the execution of the flight controller is aborted.

Gyro Calibration

In a previous chapter on the gyroscope, I mentioned that every gyroscope has some level of bias built in to its readings. Due to various factors, a gyroscopes values will never read perfectly 0.0, 0.0, 0.0 when sitting perfectly still (0 rotation movement along each axis). Instead, there will always be some small reading that persists consistently. To account for this bias, we must run a calibration in which we calculate the average reading in each of the three axes for a period of several seconds:

# measure gyro bias
print("Measuring gyro bias...")
gxs:list[float] = []
gys:list[float] = []
gzs:list[float] = []
started_at_ticks_ms:int = time.ticks_ms()
while ((time.ticks_ms() - started_at_ticks_ms) / 1000) < 3.0:
    gyro_data = i2c.readfrom_mem(mpu6050_address, 0x43, 6) # read 6 bytes (2 for each axis)
    gyro_x = (translate_pair(gyro_data[0], gyro_data[1]) / 65.5)
    gyro_y = (translate_pair(gyro_data[2], gyro_data[3]) / 65.5)
    gyro_z = (translate_pair(gyro_data[4], gyro_data[5]) / 65.5) * -1 # multiply by -1 because of the way I have it mounted on the quadcopter - it may be upside down. I want a "yaw to the right" to be positive and a "yaw to the left" to be negative.
    gxs.append(gyro_x)
    gys.append(gyro_y)
    gzs.append(gyro_z)
    time.sleep(0.025)
gyro_bias_x = sum(gxs) / len(gxs)
gyro_bias_y = sum(gys) / len(gys)
gyro_bias_z = sum(gzs) / len(gzs)
print("Gyro bias: " + str((gyro_bias_x, gyro_bias_y, gyro_bias_z)))

The variables gyro_bias_x, gyro_bias_y, and gyro_bias_z will be subtracted from every subsequent gyroscope reading. The resulting measurement is the calibrated reading that has been cleansed of bias and can be used as an accurate reading to be used by the quadcopter flight controller.

PID State Variables

As mentioned in the previous chapter on PID Controllers, the integral and derivative terms of a PID controller require state variables — in each loop, these terms build upon values from the previous loop. Thus, there are variables that must be declared outside of the infinite adjustment loop.

# State variables - PID related
# required to be delcared outside of the loop because their state will be used in multiple loops (passed from loop to loop)
roll_last_integral:float = 0.0
roll_last_error:float = 0.0
pitch_last_integral:float = 0.0
pitch_last_error:float = 0.0
yaw_last_integral:float = 0.0
yaw_last_error:float = 0.0

The PID Loop Begins: Telemetry Collection

Next, the infinite while loop begins. This loop will continuously run until the Scout Flight Controller loses power supply. As mentioned in previous chapters, a quadcopter flight controller must know two things - its current actual attitude rate of change the pilot's desired attitude rate of change. This is accomplished in the following snippets of code:

Capturing gyroscope telemetry from the onboard MPU-6050:

# Capture raw IMU data
# we divide by 65.5 here because that is the modifier to use at a gyro range scale of 1, which we are using.
gyro_data = i2c.readfrom_mem(mpu6050_address, 0x43, 6) # read 6 bytes (2 for each axis)
gyro_x = ((translate_pair(gyro_data[0], gyro_data[1]) / 65.5) - gyro_bias_x) * -1 # Roll rate. we multiply by -1 here because of the way I have it mounted. it should be rotated 180 degrees I believe, but it's okay, I can flip it here. 
gyro_y = (translate_pair(gyro_data[2], gyro_data[3]) / 65.5) - gyro_bias_y # Pitch rate.
gyro_z = ((translate_pair(gyro_data[4], gyro_data[5]) / 65.5) * -1) - gyro_bias_z # Yaw rate. multiply by -1 because of the way I have it mounted - it may be upside down. I want a "yaw to the right" to be positive and a "yaw to the left" to be negative.

Capturing control inputs from the pilot via the onboard RC receiver:

# Read control commands from RC
rc_data = rc.read()

# normalize all RC input values
input_throttle:float = normalize(rc_data[3], 1000.0, 2000.0, 0.0, 1.0) # between 0.0 and 1.0
input_pitch:float = (normalize(rc_data[2], 1000.0, 2000.0, -1.0, 1.0)) * -1 # between -1.0 and 1.0. We multiply by -1 because... If the pitch is "full forward" (i.e. 75), that means we want a NEGATIVE pitch (when a plane pitches it's nose down, that is negative, not positive. And when a place pitches it's nose up, pulling back on the stick, it's positive, not negative.) Thus, we need to flip it.
input_roll:float = normalize(rc_data[1], 1000.0, 2000.0, -1.0, 1.0) # between -1.0 and 1.0
input_yaw:float = normalize(rc_data[4], 1000.0, 2000.0, -1.0, 1.0) # between -1.0 and 1.0

Please note that in the above snippet, the received RC values are being normalized. The RC receiver typically provides a value of between 1,000 (minimum) and 2,000 (maximum) for each of the receiver’s six channels. Instead of working with this range, I’d prefer to work with the more intuitive and easier to understand standard ranges of -1.0 to 1.0 for pitch, roll, and yaw, and 0.0 to 1.0 for throttle input. The normalize function I wrote, found at the end of this code file, performs this linear normalization to within the aforementioned desired ranges.

If in Standby Mode

The Scout Flight Controller code then splits into two directions — depending on what “mode” is active, the flight controller performs different actions. The Scout Flight Controller has two modes:

• Standby — The Scout Flight Controller is standing by, awaiting further instruction. The motors are not turning. The quadcopter is sitting perfectly still.
• Flight — The Scout Flight Controller is prepared to fly. At a minimum, the motors are spun up to their idle speed — the minimum motor power that will spin each motor, but will not provide enough thrust to take off. The flight controller is considering the inputs of the pilot and applying these inputs to the PID controller loop, resulting in aerobatic maneuvers.

The Scout Flight Controller handles the Standby mode condition first:

# turn motors off completely
duty_0_percent:int = calculate_duty_cycle(0.0)
M1.duty_ns(duty_0_percent)
M2.duty_ns(duty_0_percent)
M3.duty_ns(duty_0_percent)
M4.duty_ns(duty_0_percent)

# reset PID's
roll_last_integral = 0.0
roll_last_error = 0.0
pitch_last_integral = 0.0
pitch_last_error = 0.0
yaw_last_integral = 0.0
yaw_last_error = 0.0

# set last mode
last_mode = False # False means standby mode

Firstly, the power to all four motors is set to 0.0% (the motors are not spinning whatsoever). Secondly, the aforementioned PID state variables are reset; this ensures that any “leftover” PID state from the previous flight will not be carried over into the next. Finally, the last_mode variable is set to False, indicating the most recent mode the Scout Flight Controller was in was Standby mode; this will be important for a critical safety check that is performed as soon as Flight mode is activated.

If in Flight Mode

If the Scout Flight Controller is in Flight mode, a very different set of steps occurs. Ultimately, this is the mode in which the PID loop occurs — where flight happens!

Firstly, a critical safety check occurs:

# if last mode was standby (we JUST were turned onto flight mode), perform a check that the throttle isn't high. This is a safety mechanism
# this prevents an accident where the flight mode switch is turned on but the throttle position is high, which would immediately apply heavy throttle to each motor, shooting it into the air.
if last_mode == False: # last mode we were in was standby mode. So, this is the first frame we are going into flight mode
    if input_throttle > 0.05: # if throttle is > 5%
        FATAL_ERROR("Throttle was set to " + str(input_throttle) + " as soon as flight mode was entered. Throttle must be at 0% when flight mode begins (safety check).")           

If the most recent last mode the Scout Flight Controller was in was Standby mode, this means that Flight mode was just now activated. Before continuing to apply power to the motors and apply the pilot’s input controls, the flight controller checks the position of the input throttle. If the input throttle is set to anything beyond 5% throttle, the Scout Flight Controller aborts the program and enters a failsafe mode, pulsating the onboard LED indicating it has aborted.

The reason for this safety check is to mitigate the risk of an extremely dangerous accident from occurring: the pilot flips into Flight mode with the throttle stick in a high position. Without this safety check, a large amount of power will instantly be applied to each of the four motors, causing the quadcopter to instantly fly into the air with tremendous power. This simple accident can cause major bodily injury and harm. This simple check ensures that this mistake doesn’t happen. The pilot must remember to place the throttle stick in the minimum position before activating Flight mode; if he doesn’t the program aborts!

After the safety check occurs, the adjusted throttle, adj_throttle, is calcualted. This the throttle level that will be applied to the PID loop, and thus, applied to each of the four motors. It is scaled to fit within the confines of the throttle_idle (minimum throttle required to spin up each motor at a minimum RPM) and max_throttle (throttle governor).

# calculate the adjusted desired throttle (above idle throttle, below governor throttle, scaled linearly)
adj_throttle:float = throttle_idle + (throttle_range * input_throttle)

Next, the errors required for each of the three PID controllers (one per axis) are calculated. As mentioned in previous chapters, the error is simply the difference between the pilot’s desired attitude rate of change in that axis and the actual attitude rate of change in that axis.

# calculate errors - diff between the actual rates and the desired rates
# "error" is calculated as setpoint (the goal) - actual
error_rate_roll:float = (input_roll * max_rate_roll) - gyro_x
error_rate_pitch:float = (input_pitch * max_rate_pitch) - gyro_y
error_rate_yaw:float = (input_yaw * max_rate_yaw) - gyro_z

These calculated errors are then used in the PID Controller calculations:

# roll PID calc
roll_p:float = error_rate_roll * pid_roll_kp
roll_i:float = roll_last_integral + (error_rate_roll * pid_roll_ki * cycle_time_seconds)
roll_i = max(min(roll_i, i_limit), -i_limit) # constrain within I-term limits
roll_d:float = pid_roll_kd * (error_rate_roll - roll_last_error) / cycle_time_seconds
pid_roll:float = roll_p + roll_i + roll_d

# pitch PID calc
pitch_p:float = error_rate_pitch * pid_pitch_kp
pitch_i:float = pitch_last_integral + (error_rate_pitch * pid_pitch_ki * cycle_time_seconds)
pitch_i = max(min(pitch_i, i_limit), -i_limit) # constrain within I-term limits
pitch_d:float = pid_pitch_kd * (error_rate_pitch - pitch_last_error) / cycle_time_seconds
pid_pitch = pitch_p + pitch_i + pitch_d

# yaw PID calc
yaw_p:float = error_rate_yaw * pid_yaw_kp
yaw_i:float = yaw_last_integral + (error_rate_yaw * pid_yaw_ki * cycle_time_seconds)
yaw_i = max(min(yaw_i, i_limit), -i_limit) # constrain within I-term limits
yaw_d:float = pid_yaw_kd * (error_rate_yaw - yaw_last_error) / cycle_time_seconds
pid_yaw = yaw_p + yaw_i + yaw_d

The resulting calculations of each of the three PID Controllers — pid_pitch, pid_roll, and pid_yaw, are then "mixed" together with the applied throttle (adj_throttle) to determine specifically how much power to apply to each of the four motors:

# calculate throttle values
t1:float = adj_throttle + pid_pitch + pid_roll - pid_yaw
t2:float = adj_throttle + pid_pitch - pid_roll + pid_yaw
t3:float = adj_throttle - pid_pitch + pid_roll + pid_yaw
t4:float = adj_throttle - pid_pitch - pid_roll - pid_yaw

Each of the four values above — t1, t2, t3, and t4, is a percent value between 0.0 and 1.0; what "percent" of throttle to apply to that specific motor. This signal is then sent to each motor via a PWM Duty Cycle:

# Adjust throttle according to input
M1.duty_ns(calculate_duty_cycle(t1))
M2.duty_ns(calculate_duty_cycle(t2))
M3.duty_ns(calculate_duty_cycle(t3))
M4.duty_ns(calculate_duty_cycle(t4))

Finally, the PID loop state variables are saved — remember, we need to do this because the error and integral calculations of this loop will be needed for next loop!

# Save state values for next loop
roll_last_error = error_rate_roll
pitch_last_error = error_rate_pitch
yaw_last_error = error_rate_yaw
roll_last_integral = roll_i
pitch_last_integral = pitch_i
yaw_last_integral = yaw_i

Target Cycle Time (250 Hz)

For the quadcopter to achieve stable flight, this PID adjustment loop must occur consistently at a uniform interval. In the Scout Flight Controller’s case, this loop is to occur at 250 Hz, or 250 times per second, or at 4 millisecond intervals. To ensure the PID loop does not occur any faster than this, the Scout Flight Controller waits at the end of the while loop until the 4 milliseconds (4,000 microseconds) period for the current loop is finished. This ensures that any excess time is burnt here before continuing on with the next loop.

# mark end time
loop_end_us:int = time.ticks_us()

# wait to make the hz correct
elapsed_us:int = loop_end_us - loop_begin_us
if elapsed_us < cycle_time_us:
    time.sleep_us(cycle_time_us - elapsed_us)

In Conclusion

And that’s it! Apart from some simple utility functions you can find below the main run function we just dissected, that is the entire Scout Flight Controller code. This code merges all the knowledge from preceding chapters, culminating in the creation of a robust flight controller that ensures stable flight.

In the next chapter, I’ll explain how to safely and effectively take your quadcopter for its first flight!