Onboard Software Overview

Overview

In this post, I am going to give an overview of the code I've written for the onboard microcontroller as well as cover some of the most important features. The code was all written in the Visual Studio IDE using Platform.io to make it compatible with the Arduino microcontroller. The code itself is written in C++ but each file includes the Arduino header files allowing for the use of Arduino functions. All code is available on my Github.

main.cpp File

The main file structure is shown below. 

I'll walk through this step by step.

Safety Measures

Everything in this codebase and the larger system is designed with safety in mind. Though I only fly in large empty fields, I'd like to minimize the damage if something goes wrong. I want to make sure that if I have a power failure or the code freezes or something starts acting in a way I don't like, that I can regain control or bring the aircraft to the ground without letting it fly too far. 

The first step to achieve this is a hardware one. Although the throttle has a set of pins where the PWM signals are written to just like the other servos, there is another special set of pins that connect the throttle directly to the receiver in series with the microcontrollers input pins so that the signal can be read by the microcontroller but only changed by the receiver. This means that while connected to this set of pins, the throttle is only under my control and not the microcontroller. Therefore it cannot hurt anyone on the ground and if all else fails in the air, I can cut power and allow the plane to glide down.

In software, my biggest fear is that the code will hang and I will lose control in manual mode. I have two protections against this. First, I have added a watchdog timer set to 60ms. Since the expected operating frequency of the main loop is 50ms (20Hz), if the code hangs and takes 10ms longer than expected, the entire program will restart. For my second protection, I've actually removed the manual flight control out of the main loop and put it into an interrupt service routine (ISR). This means that no matter what is happening in the main loop, if the aircraft is in manual mode, the signals from the receiver will be mapped and passed directly to the servos. This is especially important because the setup of the sensors actually takes a decent bit of time. Sometimes the GPS, in particular, takes a few tries to correctly set up, meaning that if the watchdog timer triggers, it can be up to 3 seconds before the main loop starts again. 3 seconds is a long time to have no control. Fortunately, by setting up the ISR before the sensors, the ISR will run the entire time that the sensors are being set up allowing for the minimal break in manual control.

Setup

The setup file is split into two main parts. Critical setup, which runs first, and non-critical setup which runs second. 

The critical setup file only contains setup for features necessary for manual flight through the ISR. First, this is the setup of the aircraft servos, which connects the servos to their correct pins and sets the mapping between the PWM signal and the angle for each servo. Next, the RC timing code is set up. This includes a function that is triggered every time a change is detected on the pin change interrupt vector connected to the receiver. To take advantage of the fact that this function is already triggered by the receiver, I've added logic at the end of the ISR that first checks the state of all of the pins. If they are all zero, then that means that all 6 of the inputs have gone low. This is actually the case for the majority of the time on a PWM signal since the duty cycle is normally between 5% and 10%. If all pins are low, it then checks the most recent value of the pin that selects auto mode. If it is set to manual, then the 4 control signals (Throttle, Aileron, Elevator, Rudder) are written to their respective pins. If it's not in manual mode, it does nothing. This logic allows the manual controls to be written to once per receiver cycle without getting in the way of reading the receiver but while also independent of the main loop.

The non-critical setup sets up the sensors and the SD card. It first sets up the GPS. Then performs one update of the GPS in order to determine the current date and time. Next, it sets up the SD Card. There is a little bit of logic to check if the SD card is connected. If not, it skips this step and runs everything else without the SD card. If it is connected, it sets up the card as well as writes the header for the CSV that saves the flight values. An example of the header appears below. 

================================ Starting Log =================================

Current Time: 2021-5-18 23:33:39.200

Time (ms), Scaled. Acc x (mg), Scaled. Acc y (mg), Scaled. Acc z (mg), Gyr (DPS) x, Gyr (DPS) y, Gyr (DPS) z, Mag (uT) x, Mag (uT) y, Mag (uT) z, Tmp (C), Lat, Lon, Alt (mm), Speed (mm/s), Heading (degrees * 10^-5), pDOP, SIV, Pressure (hPa), Reciever Throttle Val (0-180), Reciever Aileron Servo (deg), Reciever Elevator Servo (deg), Reciever Rudder Servo (deg), Auto Mode (deg), Aux Mode,N (m), E (m), D (m), u (m/s), v (m/s), w (m/s), q1, q2, q3, q4, Servo Throttle Val (0-180), Servo Aileron Servo (deg), Servo Elevator Servo (deg), Servo Rudder Servo (deg)

Though the actual recording of data occurs later, I will go over the contents of the data files now. It starts with long lines to make it easier to read in a large text file. Next, it prints the current time. This was pulled from the previous GPS update and allows the user to identify which flight it was based on the time. Next, it records the microcontroller's internal time. The loop time is dynamic and can take slightly longer than the expected 50ms if needed so knowing the exact time between iterations is important for estimation. Next, all of the sensors are recorded in their lightweight formats. Next, the values that the receiver is outputting are recorded. Next, the estimates from the onboard estimator are recorded. You'll notice that acceleration and rotation rates are not among the states. This is because the onboard estimator is currently being set up to be a Navigation filter and not the full model-based filter. Finally, the values actually written to the servos are recorded. In the case of manual flight, these are the same as those outputted from the receiver.

After the SD card is set up, the IMU and the internal LED are set up. The LED is used to communicate Auto or Manual mode.

Loop

The loop starts with logic determining whether or not it is time for this iteration to run. This uses the line:

The clear flaw here is that this cannot guarantee that the system iterates at exactly 20Hz but instead at a maximum of 20Hz. However, through testing, I've found that each iteration takes around 35ms and so it very rarely, if ever, doesn't hit the 50ms mark. If I truly wanted to guarantee a 20Hz frequency, I could put everything into a separate ISR that runs off an internal clock. However, to do this, I'd have to be very careful that this ISR was out of phase with the receiver's ISR so that they did not overlap each other. This is possible but will be held as a future step.

Once the loop has been entered, some local variables are initialized and the state is retrieved through the function getRCSignalAngle which pauses interrupts briefly and retrieves the most recent information from the receiver, and maps it from PWM to angle.

Next, each sensor independently updates, though the values are not passed into the main file. Instead, each sensor has specific "get" functions for its outputs that retrieve the most recent information for whatever function requires it. Here they are only updated so that synchronization can be maintained.

Filter/Estimator

Next comes the state estimation step, outlined below.

The measurement vector, referred to as $z$ in previous posts, is first retrieved. This vector is not simply the sensor outputs. Some have unit conversions, others, like GPS position, have to be converted to the local coordinate frame as well as subtracted from the initial value. After the measurement vector is produced, the filter performs the prediction and update steps outlined in the "Offline Filter" post.

This process requires the use of a number of complicated algebraic functions, two of which are large jacobians. To avoid doing out the math by hand or even doing out the math in MATLAB and converting to C++ by hand, I am taking advantage of Python Sympy and the codegen tool. With Sympy, I can produce the $f$ and $h$ functions symbolically and take their jacobians relative to the state vector. I can then use the codegen() function to produce both header and c files for each function. These are easily callable by the Filter function and should produce the same results as writing out the functions myself more efficiently and with fewer places for error.

Auto and Manual Mode

Next, the loop decides whether to enter manual or auto mode based on the value of the auto mode switch on the servo. If it is in manual mode, the values from the receiver are simply copied to the values of the servo output for the SD card to record. The actual servo control occurs in the ISR without influence from main.

If it is in auto mode, the stabilize function takes the most recent state estimate, converts the quaternions to Euler angles, and calculates the error of each term by subtracting the equilibrium values. The control input is produced through the equation:

$$ \textbf{u} = -K\textbf{x}$$

using the K calculated offline for both pitch and roll axes. These values are written to the servos through the writeServosAngle function which automatically limits the signals to the appropriate output range.

The servo values are also passed into main to be recorded on the SD card.

SD Recording

The final step is to record all data from the most recent iteration on the SD card. There is one trick that I use here. In order to write to the SD, the file must be opened, written to, and then closed. Through testing, I found that opening and closing the file during every iteration was very slow, adding about 20ms to the total loop time. In order to avoid this. The file is opened when the auxiliary switch on the remote is set to 1 (it has three modes) and closed when it is set to 0. This drastically speeds up the writing but also creates some problems. It means that I have to remember to close the file after the flight before disconnecting power. It also means that if there is a power failure, the data taken before the failure will be lost. 

I have a number of potential solutions for this problem that I haven't yet implemented. One is to close and open the file once every second or so. Slowing down the loop occasionally but providing a backup for the data. I suspect the time it takes to close the file might be proportional to how much data it is writing so that might not help much and requires more research and experimentation. Another option is to use the FRAM memory on the external board. Again, I've done little research into this but it may be able to record faster than the SD. The final option is to use wireless communication from the aircraft to the ground and record the data on the ground. I purchased these medium-range transceivers and would like to connect them to the SPI port that the SD card currently uses. By communicating each line of data to the ground and writing it to an SD card there using another microcontroller or even my computer, I might be able to save time. I have not tested this at all though so it is possible that the transceiver cannot pass the amount of data I need quickly enough or it might take longer than writing to the SD card does now. 

I would like to involve the transceiver eventually though in order to get real-time telemetry on the ground.  This will be very useful further in the future when auto mode specifies actual flight paths instead of simple stabilization. 

Conclusion

Like everything else in this system, the onboard code is a work in progress. I'm still in the process of adding the estimator and controller code as well as fixing some bugs with the manual mode servo writing. That said, this code, or at least a previous version, was used for the flight that got my best data so I know that it can get the job done.

I look forward to making changes so that it becomes simpler, better organized, and more capable of what I need it to do. Overall I've learned a lot and will keep learning as I go. Hope you learned something too.