< Back

Custom Flight Controller Firmware in Rust

Introduction

I have written a proof-of-concept flight controller firmware in Rust. Rust has been my favorite programming language for a few years now, but I had not used it much for embedded devices yet. I have been thinking about a flight controller in Rust for some time now, but it seemed like a daunting task given the complexity of systems like PX4, Ardupilot and INAV.

Then I realized I can start with something relatively simple. For a proof-of-concept, I could get away with some bare minimums. This would include:

  • Read IMU data
  • Estimate attitude
  • Read radio receiver data
  • Calculate stabilized output commands
  • Write PWM to motor and servo's

This would allow me to fly an airplane in stabilized mode, which would be a great start! It is also a little bit inspired by dRehmFlight, another simple bare minimums flight controller.

Real-Time OS or cooperative scheduling

Traditionally embedded devices with complex software have been the domain for Real-Time Operating Systems (RTOS). PX4 runs on NuttX, ArduPilot runs on ChibiOS. INAV and BetaFlight have a simpler approach based on cooperative scheduling.

I have always found the RTOS approach a bit disappointing and unnecessarily complex. I'm also not a fan of blocking code. Each thread needs its own stack and you have to be very careful with allocating enough stack space. The sum of all unused RAM in these threads can add up to quite a lot. Then there is also the overhead from context switching. Multi-threading also introduces a whole range of potential bugs.

The cooperative scheduling approach is definitely simpler, but requires careful configuration. In order not to miss any high-priority deadlines, some lower priority tasks get postponed when there is not enough time to execute. While this works, it can leave a lot of CPU time un-utilized.

Real-Time Interrupt-driven Concurrency (RTIC)

Then I discovered this very interesting and promising third option. It is neither an OS, nor a scheduler. The Real-Time Interrupt-driven Concurrency (RTIC) framework leverages interrupts on the hardware (like STM32) to accomplish pre-emptive concurrency while everything runs on a single stack. It is so elegant and simple that it is almost ridiculous this is not widely used. The preface section of the RTICv2 book does an excellent job at explaining the concept. It's a highly recommended read!

With the RTICv2 framework, there is just a single stack while there are many different tasks running at different priorities. The overhead related to context-switching is just the overhead of handling an interrupt. Compared to cooperative schedulers, better CPU utilization is possible this way.

While the RTIC repository contains a large amount of examples, the examples are relatively small. I was curious if this framework could be a feasible option for a larger project, like a flight controller firmware. That worked out quite well.

Rust on the SpeedyBee F405 Wing Mini

To flash and debug the flight controller, I de-soldered the 2 indicator LEDs, as these are the CLK and IO pins for Serial Wire Debug (SWD). I soldered two wires to the pads where the 0402 resistors used to be and hooked it up to an STLink v2.

The Rust ecosystem for programming embedded devices is amazing. Cross-compiling is a breeze, especially compared to C or C++ projects. The tooling for flashing and debugging is excellent. I found the defmt crate very helpful, which does 'deferred formatting'. Instead of formatting debug strings on the STM32, it only sends the minimum required data and formats the strings on the PC.

I used the STM32F4xx Hardware Abstraction Layer which provides a very nice interface to interact with the hardware. It leverages the Rust type system to make sure that the configuration is valid. For example, it automatically knows what alternate function to use when passing a GPIO pin to a SPI peripheral. Also the compiler will throw errors when you try to use a DMA channel with a peripheral that is not compatible.

SWD wires soldered to the SpeedyBee

Tasks and priorities

In my initial setup, I have the following tasks with associated priorities. Higher number is higher priority. Messages between these tasks are passed via channels. It is also possible to share data between tasks, which I used only for resetting the AHRS.

  • PWM Output Task (10)
    • Listens to commands from the control task. If there is no command for 100ms, a failsafe value is set. This uses async for the timeout.
  • DMA ready interrupt from IMU (5)
    • Runs when the request to the IMU is finished. If a new IMU sample is obtained, this is forwarded to the state estimator at priority 2.
  • USART byte received interrupt for radio receiver (5)
    • This is a byte from the radio receiver. It is forwarded to the CRSF parser on priority 3.
  • CRSF parser (3)
    • This receives bytes from the task above and parses them. If a full radio control messages is ready, it is forwarded to the control task at priority 2.
  • Control task (2)
    • This tasks listens to both AHRS samples and remote control commands. It runs a PIFF controller and publishes the commands to the PWM output task. If there are no remote control messages for 500ms, failsafe is activated (stabilized glide).
  • State estimator (2)
    • This parses the IMU sample and feeds it to an attitude estimator (AHRS). The AHRS output is published to the control task.

PIFF controller

I implemented a very simple PIFF controller. I calculated the gains from those that I had used before when flying with INAV.

The angle error is multiplied by a P-gain to obtain a rate setpoint. The rate setpoint is multiplied by feed-forward. In addition, there is a PI-controller on the rate error. The integrator resets when the plane is disarmed.

Attitude Estimator

For attitude estimation, I just grabbed one from crates.io. It worked surprisingly well, even without doing any accelerometer and gyro calibration. Also when flying, it didn't show any signs of horizon drift which was pleasantly surprising. It's a bit heavy on the CPU, but that might improve when I compile it to use the the Floating Point Unit. For now, it's quite ugly: I throw away 9 out of 10 IMU samples, so the AHRS is only updating at 100Hz.

IMU driver

The IMU on the SpeedyBee is an ICM-42688p. I managed to set it up using DMA, so that the bytes are transferred and received in the background without using up CPU cycles. I had to do some customizations to the HAL to make it accept variable length buffers, depending on the message type I'm sending.

Basically I'm continuously querying the IMU to tell me how many samples are ready in the FIFO buffer. If there is at least one sample, I ask it to send those samples.

The configuration is done in the init function, which uses some blocking code for setting the registers. The configuration is heavily inspired from INAV and Ardupilot. The IMU generates samples at 1000Hz.

CRSF radio message parser

One of the benefits of using Rust with RTICv2 is that I can write async functions. I used an async function to parse radio messages. This is a very primitive and incomplete parser, but the async concept will remain usable when further expanding it to different message types. This is a small part of the parser as example:

loop {
	// Check sync byte
	if rx.recv().await.unwrap() != 0xC8 {
		continue;
	}
	let length = rx.recv().await.unwrap();
	if length != 24 {
		continue;
	}
	// Check message type RC Channels Packed Payload
	if rx.recv().await.unwrap() != 0x16 {
		continue;
	}
	let mut channel_bytes = [0_u8; 22];
	for i in 0..22 {
		channel_bytes[i] = rx.recv().await.unwrap();
	}
	let crc_byte = rx.recv().await.unwrap();

	...
}

Source code

Although the code is in quite an ugly state right now, I published it to Github. Please see this just as a proof-of-concept. It will need a lot more work to be considered good code.

I wonder if it would be possible to make this almost as easy to understand as dRehmFlight. Rust is not the easiest language and the compiler errors can be quite challenging for beginners. However I also do see some potential benefits of Rust compared to C, C++ or even Arduino. The option to use async functions can potentially make the code a lot more readable. The CRSF parser is a nice example of that, but perhaps this can be applied in other areas too.

I would love to continue working on this and turn it into a proper flight controller firmware. It will be an insane amount of work, but at the same time very satisfying. For now I will use this project to try out some alternative control algorithms, which is very easy to do with this.