Rust-based Quadcopter Part 1

This post covers the details of how I made a flight controller for a quadcopter using Rust and an STM32.

Introduction

This project began as a desire to familiarize myself with embedded development. I wanted to challenge myself by using technology I was not familiar with or had no prior knowledge of. This led me to the idea of making a Rust-based quadcopter. I will walk through the different components and software I used to realize this project.

Components

To simplify things on my end, I decided to buy a drone kit that came with the following items:

• Chassis
• 4 30A Electronic Speed Controllers
• 4 BLDC Motors
• 4 Propellers
• Screws & Bolts

Other components needed:

• STM32F411CEU6
• FlySky Transmitter & Receiver
• LiPO Battery
• IMU (LSM6DSOX)

Rust

Rust is a systems programming language designed to prioritize performance and stability. It is particularly advantageous for embedded development due to its extensive community support. There are Hardware Abstraction Layers (HALs) available for various microcontrollers, including several STM32 models. Notably, the svd2rust crate enables users to convert SVD files into crates, providing APIs to access the microcontrollers’ peripherals

I am using the STM32F411CEU6 for my flight controller, so I will be using the stm32f4xx-hal. Another important crate in the embedded-rust community is probe-rs which provides a direct interface to various embedded MCUs and debug probes, eliminating the GDB layer unlike similar projects such as OpenOCD.

Real-Time Interrupt-Driven Concurrency

RTIC is a Rust-based framework that allows real-time interrupt-driven concurrency for ARM Cortex-M microcontrollers. It is similar to an RTOS in that they both perform scheduling. Where they differ is that RTIC uses the hardware on the Cortex-M MCUs to perform the scheduling instead of a scheduling kernel.

IMU & Kalman Filter

IMU, short for inertial measurement unit, can measures the angular rate and acceleration of a quadcopter’s roll, pitch, and yaw. These measurements can be used to determine the attitude (roll, pitch, and yaw) of my flight controller.

The IMU I will be using, the LSM6DSOX, consists of a gyroscope and accelerometer. The gyroscope allows for the measurement of the angular rates of the IMU’s roll, pitch, and yaw axes. The accelerometer allows for the measurement of the acceleration of the IMU’s roll, pitch, and yaw axes.

This IMU supports I2C and SPI communication protocols. These protocols allow for communication with other devices. In my case, I need to send the reported measurements from my IMU to the STM32. While the list of I2C and SPI drivers written in Rust may be extensive, there was not one yet for the LSM6DSOX as of the date of this project, so I wrote my own I2C driver.

I2C Driver

Before writing an I2C driver in Rust, I first took a look at how other drivers were created in Rust. This is a pretty good list to look at. It even contains a few drivers for other IMUs.

Most of them were built on top of the embedded-hal crate. This crate serves as a foundation for building platform-agnostic drivers, allowing a driver to work on a microcontroller regardless of its platform.

Before I go further into details about my I2C driver, a basic understanding of the I2C protocol might be helpful. The I2C protocol uses two wires for serial communication between a Master and a Slave. These wires are labeled the serial data line (SDA) and the serial clock line (SCL). The first line is for data transfer and the second line is for communication. When attempting to read data through the SDA bus, it is first necessary for the Master (or the STM32 in my case) to write the address of the slave onto the bus it wants to read from. This is necessary so that any slave can respond if they have the corresponding address.

This was a very basic overview of the I2C protocol, and a lot of the underlying mechanisms of how it works were not covered, mainly because the embedded-hal crate does a great job at abstracting away the underlying details.

With that basic understanding of the protocol, my driver will make more sense. In order to get the gyroscope and accelerometer values, I just have to write the address of my IMU on the I2C bus, and then read from the address of the register that stores the desired data. To find the IMU slave address, and the address of the registers where the data is stored, I had to read the LSM6DSOX datasheet. I have created two files, gyro.rs and accel.rs, to contain the registers necessary to read their values.

gyro.rs

#[allow(non_camel_case_types)]
#[allow(dead_code)]
#[derive(Copy, Clone)]
pub enum Register {
    // Gyro Configuration Register
    CTRL2_G = 0x11,

    // Angular rate pitch axis (X) output
    OUTX_L_G = 0x22,
    OUTX_H_G = 0x23,

    // Angular rate roll axis (Y) output
    OUTY_L_G = 0x24,
    OUTY_H_G = 0x25,

    // Angular rate yaw axis (Z) output
    OUTZ_L_G = 0x26,
    OUTZ_H_G = 0x27,
}

impl Register {
    pub fn addr(&self) -> u8 {
        *self as u8
    }
}

accel.rs

#[allow(non_camel_case_types)]
#[allow(dead_code)]
#[derive(Copy, Clone)]
pub enum Register {
    // ACCELEROMETER CONFIG REG
    CTRL1_XL = 0x10,

    // Accel rate X axis (X) output
    OUTX_L_A = 0x28,
    OUTX_H_A = 0x29,

    // Accel rate Y axis (Y) output
    OUTY_L_A = 0x2A,
    OUTY_H_A = 0x2B,

    // Accel rate Z axis (Z) output
    OUTZ_L_A = 0x2C,
    OUTZ_H_A = 0x2D,
}

impl Register {
    pub fn addr(&self) -> u8 {
        *self as u8
    }
}

I then defined the class/instance of my i2c driver with the paramter I2C. Users will be required to define an I2C object to use this library.

// lsm6dsox driver
pub struct Lsm6dsox<I2C> {
    i2c: I2C,
}

impl<I2C, E> Lsm6dsox<I2C> 
    where 
        I2C: WriteRead<Error = E> + Write<Error = E>,
    {       
    pub fn new(i2c: I2C) -> Result<Self, E> {
        let lsm6dsox = Lsm6dsox { i2c };
        Ok(lsm6dsox)
    }

    ...
}

The code above serves to implement a struct Lsm6dsox with a parameter I2C such that I2C has the methods WriteRead and Write.

pub fn configure_accel(&self, i2c: &mut I2C) -> Result<(), E>{
    // ODR_XL[7:4] = 0100; sets accelerometer to work at 200 Hz
    // FS[3:2] = 10; sets accelerometer full-scale selection to 4g
    // LPF2_XL_EN[1] = 1; output from first stage digital filtering
    // 0100 10 1 0
    let configuration: u8 = 0x48;
    match i2c.write(SLAVE_ADDRESS, &[CTRL1_XL, configuration]) {
        Ok(_) => Ok(()),
        Err(e) => Err(e),
    }
}

In the function above, I configure the accelerometer in my IMU. Referring to the datasheet, I observe that each bit in the configuration register influences aspects such as the frequency of readings. I utilize the write method to send the 8 bit value configuration to the CTRL1_XL register of the device, specifying the slave address as SLAVE_ADDRESS. The process of configuring the gyroscope mirrors that of the accelerometer, albeit with different configuration values and register addresses.

// Read Accelerometer Data
pub fn read_accel(&mut self) -> Result<[f32; 3], E> {
    
    let mut accel_data: [f32; 3] = [0.0, 0.0, 0.0];
    let mut buffer: GenericArray<u8, U6> = unsafe { MaybeUninit::uninit().assume_init() };

    { 
        let buffer: &mut [u8] = &mut buffer;
        self.i2c.write_read(SLAVE_ADDRESS, &[accel::Register::OUTX_L_A.addr()], buffer)?;
        accel_data[0] = ((buffer[1] as i16) << 8 | (buffer[0] as i16)) as f32 * 4.0 / 32768.0;
        accel_data[1] = ((buffer[3] as i16) << 8 | (buffer[2] as i16)) as f32 * 4.0 / 32768.0;
        accel_data[2] = ((buffer[5] as i16) << 8 | (buffer[4] as i16)) as f32 * 4.0 / 32768.0;
    }
    Ok(accel_data)
}

In the function above, I am reading accelerometer data. Each axis’s data is represented with 16 bits, spread across 2 8-bit registers. To get each axis’s value, I read from 6 consecutive registers (3 axis multiplied by 2 register per axis). The write_read method writes SLAVE_ADDRESS to the I2C bus and reads enough bytes from the device with the address of SLAVE_ADDRESS to fill buffer in one go. This reads all accelerometer registers if buffer has 6 bytes. The IMU is configured by default to automatically increment a register address during a multiple byte access with a serial interface.

Accelerometer data for each axis is 16-bit. The IMU stores the 8 most significant bits in one register and the 8 least significant bits in another. For the accelerometer, OUTX_H_A is the MSB for the X-axis, and OUTX_L_A is the LSB. I left shift buffer[1], buffer[3], and buffer[5] by 8 bits, and bitwise OR these values with their corresponding LSBs to reconstruct each axis’s value.

Deriving the Attitude

So I’m able to get the angular rates and acceleration of each axis using my IMU. But how can this be used to get the angles of roll, pitch, and yaw axis? I’ll discuss two ways of obtaining the attitude from the gyrscope and accelerometer.

Angular rate is just the rate at which the angle changes in a moment of time. By integrating this value, we can get the the angle of each axis. Integration in the context of a microcontroller would have to be discretized. This simplifies the calculations a lot. To get the angle of an axis, you would do a summation of the corresponding angular rate multiplied by the delta time. In our case, the delta time would be the amount of time elapsed between the previous and current loop.

let mono_timer = MonoTimer::new(cp.DWT, cp.DCB, &clocks);
let start_time = mono_timer.now();

loop {
    let time = start_time.elapsed();
    let delta_time = time - prev_time;
    let delta_sec = delta_time as f32/ clocks.sysclk().raw() as f32 * 2.0;

    gyro_data = imu.read_gyro().unwrap();

    x_gyro += delta_sec * gyro_data[0]; // Angle of flight controller in the x-axis
    y_gyro += delta_sec * gyro_data[1]; // Angle of the flight controller in the y-axis
    z_gyro += delta_sec * gyro_data[2];

    prev_time = time;
}

While this method works in the short term, you will notice that the angles derived from the gyrscope drastically loses any accuracy as time progresses. This is due to the fact the there is an uncertainty in the readings of the angular rate, and as we integrate these uncertainties, the error will accumulate.

The second method is using the accelerometer. As previously mentioned the accelerometer measures the acceleration of each of its axis (X, Y, and Z). At rest, the Z axis will report a value of around ~ 1 (measured in g’s). If the IMU is in a different orientation, each of its axis will experience a different portion of gravity. Plugging these values into trigonometric equations can give us the orientation of the IMU in terms of degrees of each of its axis except for the z-axis. The z-axis cannot be calculated as this is inherent limitation to an accelerometer.

accel_data = imu.read_accel(&mut i2c).unwrap();

y_accel = atanf(accel_data[0] / sqrtf(accel_data[1] * accel_data[1] + accel_data[2] * accel_data[2]) ) * 180.0 / PI;
x_accel = atanf(accel_data[1] / sqrtf(accel_data[0] * accel_data[0] + accel_data[2] * accel_data[2]) ) * 180.0 / PI;

While the results from an accelerometer are mostly accurate, in moments of high vibrations the values are basically useless as there is too much noise.

So we have a scenario in which the gyrscope’s data is useful in the short-term, but useless in the long-term and the vice-versa for the accelerometer’s data. A Kalman Filter is perfect for this scenario.

What is a Kalman Filter?

The Kalman filter is an algorithm used for estimating the state of a system in the presence of noisy measurements. It works by iteratively predicting the next state based on a dynamic model and comparing it with actual measurements. The filter optimally combines predictions and measurements, dynamically adjusting their contributions based on their uncertainties. This allows the Kalman filter to provide a more accurate and stable estimate of the system’s true state, making it widely used in applications like sensor fusion for robotics, navigation, and control systems.

In my case, I can use it to fuse the data from the accelerometer and gyroscope that can minimize their respective downfalls to produce a more accurate result. I found this blog by TKJ Electornics to be very helpful in understanding how a Kalman Filter can be applied to a 6-axis IMU.

ESC

The electronic speed controller is a simple piece of circuit that controls the speed of the motors. There are a few methods to communicate with the ESC from my microcontroller to dictate the speed of the controllers. I will be using the simplest method, PWM signals.

What is PWM?

Pulse-width-modulation is essentially rectangular wave signal with a duty cycle and period. A duty signal is the percentage of the signal that is active. A period is the time between each succesive occurence.

For my ESC, I can dictate the desired speed of my motors by sending a 50Hz PWM signal. When the active portion is 1ms, the ESC interprets that as zero throttle. When the active portion is 2ms, the ESC interprets that as full throttle. Any signal with an active portion between 1ms to 2ms will set the speed proportionately. For example, a PWM signal with an active portion of 1.5ms will set the throttle to half the max speed.

For my ESC to work, it needs to initially register a zero throttle signal before it can receive any other signal.

PWM Implementation

I first needed to setup the timer channels. Each timer in the STM32 has two channels assosiated to itself. Thus, each timer can generate two differnt PWM signals. For your microcontroller, you must check the pinout sheet to see which pins to use.

let gpioa = dp.GPIOA.split();
let channels = (Channel1::new(gpioa.pa8), Channel2::new(gpioa.pa9));

In the codeblock below, I am setting up the channel as a PWM singal with a frequency of 50 Hz.

let pwm = dp.TIM1.pwm_hz(channels, 50.Hz(), &clocks).split();
let (mut ch1, _ch2) = pwm;
let max_duty = ch1.get_max_duty() as u32; 
ch1.enable();

In the stm32f4xx-hal, the set_duty method requires a value rather than a percentage. To send a zero throttle signal, I need to determine the duty cycle with an active portion of 1ms.

Given that a 50 Hz signal has a period of 20ms, the duty cycle for a zero throttle signal would be 1ms / 20ms, which equals 0.05. Therefore, to achieve a zero throttle signal, I multiply the maximum duty cycle by 5 / 1000, or equivalently, divide it by 20.

For the maximum throttle signal, the duty cycle would be 2ms / 20ms, which equals 0.1. Thus, the corresponding maximum throttle signal would be the maximum duty cycle divided by 10.

Additionally, I introduce a slight delay to allow the ESC to register the zero throttle signal. I can confirm this works because I hear specific beeps in a distinct melody.

rprintln!("Zero signal");
ch1.set_duty(ch1.get_max_duty() / 20);


rprintln!("Slight delay");
delay.delay_ms(10000_u32);

let min_limit = max_duty * 5 / 1000;
let max_limit = max_duty / 100;

for value in 0..50 {
    //ch1.set_duty(map(value, 0, 100, min_limit, max_limit));
    let throttle = map(value, 0, 100, min_limit, max_limit);
    ch1.set_duty(throttle as u16);
    delay.delay_ms(12_u32);
}
...
fn map(x: u32, in_min: u32, in_max: u32, out_min: u32, out_max: u32) -> u32 {
    (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
}

Remote Controller

For my remote controller, I am using the FlySky FS-IA6B. It comes with the transmitter and a receiver. All I had to do is read any bytes incoming from the receiver. FlySky has a feature called the IBus protocol, which essentially allows it to send the data of all the channels in 32 byte packets through the UART hardware.

Note
A channel in this context refers to each inputs on the transmitter, such as the joysticsk and switches.

To my luck, the stm32f4xx-hal repository already contains an example with UART and RTIC. This example uses a UART interrupt to immediately notify the CPU when the UART has new information.

Unfortunately, getting this to work for my usecase proved to be quite difficult. The UART would constantly get new data so it would starve the other tasks. Instead, I switched to polling the UART for new info and implemented a checksum verification to discard erroneous packets.

Note
Interrupts allow hardware to notify the CPU of a specific event. Polling is when the CPU checks on the hardware for any new events. This comes at the cost of additional CPU clock cycles.

#[task(binds = TIM3, priority = 1, local=[rc_timer, prod_flysky, rx])]
fn process_received_bytes(ctx: process_received_bytes::Context) {

    ctx.local.rc_timer.clear_all_flags();
    ctx.local.rc_timer.start(15.millis()).unwrap();
    
    let rx = ctx.local.rx;

    rprintln!("process received bytes");
        

    let mut bytes = [0u8; 32];
    let mut index = 0;
    let mut checksum: u16 = 0;

    while index != 32 {
        if let Ok(byte) = rx.read() {
            if index == 0 && byte != 32 {
                // Ignore bytes until the first byte is 32 (space character)
                continue;
            }
            bytes[index] = byte;
            index += 1;
        }
    }

    //rprintln!("bytes: {:?}", bytes);
    let mut channel_values: [u16; 16] = [0; 16];

    for i in (0..bytes.len()).step_by(2) {
        if i + 1 < bytes.len() {
            // Extract two bytes from the pair
            let byte1 = u16::from(bytes[i]);
            let byte2 = u16::from(bytes[i + 1]);

            if i < 30 {
                checksum += byte1 + byte2;
            }
            
            // Combine the bytes by multiplying the second number by 256
            //let combined_value = u16::from(byte1) + u16::from(byte2) * 256;
            let combined_value = byte1 | byte2 << 8;

            let channel_index = i / 2;
            //rprintln!("index: {:?}", channel_index);
            // Do something with the combined value (e.g., print or use it)

            channel_values[channel_index] = combined_value;
            //rprintln!("Combined Value: {}", combined_value);
        } else {
            break
        }
    }

    if 65535 - channel_values[15] != u16::from(checksum) {             
        rprintln!("{:?} != {:?}", 65535 - channel_values[15], checksum);                            
    } else {
    
        //rprintln!("Channels: {:?}", channel_values);
        let mut _throttle = channel_values[3] as f32;
        let mut _desired_roll = map(channel_values[1] as f32, 1000., 2000., -15., 15.);
        let mut _desired_pitch = map(channel_values[2] as f32, 1000., 2000., -15., 15.);

        match ctx.local.prod_flysky.enqueue([_throttle, _desired_roll, _desired_pitch]) {
            Ok(()) => {
                //rprintln!("Data sent");
            }

            Err(_err) => {
                // Other errors occurred, handle them appropriately
                // Example: println!("Error occurred while enqueueing data: {:?}", err);
                rprintln!("Flysky failed to enqueue");
            }
        }
    }
    
}

PID

A pid is a control loop concept that can use feedback to adjust the inputs to drive the system towards a desired state. I will be simplifing how PIDs work a lot as I probably won’t do a good job at explaining its complexities.

For my drone, I am able to get the attitude of my drone, and I can set the desired attitude with my remote controller. I need some sort of mechanism to force the drone to match the attitude set by the remote controller. A PID controller can do so by constantly adjusting the speed of each motor to match the desired angle. A PID must be configured to work properly by tuning its three parameters; proportional, controller, and integral.

Proportion

The porportional or P-controller takes a fraction of the error. For example, the pitch of my drone may be 15 degrees, but my desired pitch could be 0 degrees. The error would be 15 degrees. The proportion would multiply this error by a factor to adjust the input throttle to each motor.

Integral

The integral or I-controller helps with small errors by taking a fraction of the integral of the error.

Derivative

This controller takes into account on how rapidly the error is changing by taking a fraction of the error’s derivative.

RTIC

RTIC, as mentioned previously, is a real-time framework. In the context of embedded software, a real-time system is defined as one that is deterministic and meets specific timing requirements. This is in contrast to systems like Arduinos, which rely on one large loop. While Arduino-based quadcopters have been developed before, using a real-time framework like RTIC can provide greater reliability in certain scenarios. Determining which approach is better depends on the use case.

RTIC includes a scheduler that determines which task to run by programming the MCU’s hardware. In my project, I have three tasks. The first task polls the UART for new data. The second task employs the Kalman Filter to determine the drone’s attitude from the IMU data. The third task is the flight controller task, where I use a PID controller to generate the appropriate throttle signals for each motor. Each task operates as an interrupt service routine of a timer, allowing them to run approximately every 15ms with high accuracy. RTIC also includes an init task, responsible for configuring all the hardware and setting initial values.

#![no_main]
#![no_std]
#![feature(type_alias_impl_trait)]

use panic_halt as _;
mod kalman;
mod pid;

use rtic::app;

#[app(device = stm32f4xx_hal::pac, dispatchers = [SPI1, SPI2])]
mod app {
    use core::f32::consts::PI;
    use heapless::spsc::{Consumer, Producer, Queue};
    use rtic_monotonics::{create_systick_token, systick::Systick};
    use stm32f4xx_hal::{self as hal};
    

    use hal::{
        i2c::{I2c1, Mode}, 
        pac::{TIM1, TIM2, TIM3, TIM4, TIM5, USART1}, 
        prelude::*, rcc::RccExt, 
        serial::{Config, Rx, Serial}, 
        timer::{self, Channel1, Channel2, Event, PwmChannel}
    };

    use rtt_target::{rprintln, rtt_init_print};

    /** Kalman Filter Libraries **/
    use libm::{atanf, sqrtf};
    use crate::kalman::KalmanFilter;
    use crate::pid::Pid;
    use lsm6dsox_driver::Lsm6dsox;

    const FC_DELTA: u32 = 15;
    const KALMAN_DELTA: u32 = 15;

    #[shared]
    struct Shared {
    }

    #[local]
    struct Local {
        /** FlySky Buffer **/
        rx: Rx<USART1, u8>,
        prod_flysky: Producer<'static, [f32; 3], 10>,
        con_flysky: Consumer<'static, [f32; 3], 10>,
        rc_timer: timer::CounterMs<TIM3>,


        /** Kalman Filter Variables **/
        kalman_timer: timer::CounterMs<TIM4>,
        imu: Lsm6dsox<I2c1>,
        x_kalman: KalmanFilter,
        y_kalman: KalmanFilter,

        prod_kalman: Producer<'static, [f32; 2], 5>,
        con_kalman: Consumer<'static, [f32; 2], 5>,

        /** Flight Controller variables **/
        fc_timer: timer::CounterMs<TIM5>,

        /** ESC **/
        m1: PwmChannel<TIM1, 0>,
        m2: PwmChannel<TIM1, 1>,
        m3: PwmChannel<TIM2, 0>,
        m4: PwmChannel<TIM2, 1>,

        pitch_pid: Pid,
        roll_pid: Pid,
    }


    #[init(local = [
        rx_pool_memory: [u8; 400] = [0; 400],
        queue_kalman: Queue<[f32; 2], 5> = Queue::new(),
        queue_flysky: Queue<[f32; 3], 10> = Queue::new(),
    ])]
    fn init(cx: init::Context) -> (Shared, Local) {

        rtt_init_print!();
        rprintln!("Init");

        let (prod_kalman, con_kalman) = cx.local.queue_kalman.split();
        let (prod_flysky, con_flysky) = cx.local.queue_flysky.split();        

        let token = create_systick_token!();
        Systick::start(cx.core.SYST, 100_000_000, token);

        let dp: hal::pac::Peripherals = cx.device;

        let rcc = dp.RCC.constrain();

        //let clocks = rcc.cfgr.freeze();
        //let clocks = rcc.cfgr.hclk(8.MHz()).freeze();
        let clocks = rcc.cfgr.use_hse(25.MHz()).sysclk(36.MHz()).hclk(25.MHz()).freeze();
        // let clocks = rcc.cfgr
        //     .use_hse(8.MHz())
        //     .sysclk(36.MHz())
        //     .pclk1(36.MHz())
        //     .freeze();

        let gpioa = dp.GPIOA.split();
        let rx_pin = gpioa.pa10; // RX

        // Configure USART1 with a baud rate of 115200
        let rx: Rx<USART1, u8> =
            Serial::rx(dp.USART1, rx_pin, Config::default().baudrate(115200.bps()), &clocks).unwrap();

        let mut rc_timer = dp.TIM3.counter_ms(&clocks);
        rc_timer.start(10000.millis()).unwrap();
        rc_timer.listen(Event::Update);

        let gpiob = dp.GPIOB.split();

        //// LSM6DSOX IMU \\\\

        let scl = gpiob.pb6.into_open_drain_output();
        let sda = gpiob.pb7.into_open_drain_output();

        let i2c = dp.I2C1.i2c(
            (scl, sda),
            Mode::Standard {
                frequency: 104.kHz(),
            },
            &clocks,
        );

        let mut imu = Lsm6dsox::new(i2c).unwrap();
        rprintln!("LSM6DSOX IMU initialized");
        let id = imu.read_id().unwrap();
        rprintln!("id is {:#b}: ", id);

        imu.configure_accel().unwrap();
        imu.configure_gyro().unwrap();

        // Kalman Filter
        let x_kalman = KalmanFilter::new();
        let y_kalman = KalmanFilter::new();

        // Kalman Filter Timer
        let mut kalman_timer = dp.TIM4.counter_ms(&clocks);
        kalman_timer.start(10000.millis()).unwrap();
        kalman_timer.listen(Event::Update);

        // Motor Timer Setup
        let channels = (Channel1::new(gpioa.pa8), Channel2::new(gpioa.pa9));
        let channels_1 = (Channel1::new(gpioa.pa0), Channel2::new(gpioa.pa1));
        
        let pwm = dp.TIM1.pwm_hz(channels, 50.Hz(), &clocks).split();
        let pwm_1 = dp.TIM2.pwm_hz(channels_1, 50.Hz(), &clocks).split();

        let (mut m1, mut m2) = pwm;
        let (mut m3, mut m4) = pwm_1;

        m1.enable();
        m2.enable();
        m3.enable();
        m4.enable();

        rprintln!("Zero signal");
        m1.set_duty(m1.get_max_duty() / 20);
        m2.set_duty(m2.get_max_duty() / 20);
        m3.set_duty(m3.get_max_duty() / 20);
        m4.set_duty(m4.get_max_duty() / 20);

        //rprintln!("m1 Value: {:?}", [m1.get_max_duty() / 20, m1.get_max_duty() / 10]);

        // PID
        let pitch_pid = Pid::new(2.0, 0.1, 0.1);
        let roll_pid = Pid::new(2.0, 0.1, 0.1);

        // Flight Controller Timer
        let mut fc_timer = dp.TIM5.counter_ms(&clocks);
        fc_timer.start(10000.millis()).unwrap();
        fc_timer.listen(Event::Update);

        //process_received_bytes::spawn().ok();

        (
            Shared {  },
            Local {
                rx,
                prod_flysky,
                con_flysky,
                rc_timer,

                imu,
                x_kalman,
                y_kalman,

                prod_kalman,
                con_kalman,
                kalman_timer,

                fc_timer,

                m1, m2, m3, m4,

                pitch_pid,
                roll_pid,
            },
        )
    }

    #[task(binds = TIM3, priority = 1, local=[rc_timer, prod_flysky, rx])]
    fn process_received_bytes(ctx: process_received_bytes::Context) {

        ctx.local.rc_timer.clear_all_flags();
        ctx.local.rc_timer.start(15.millis()).unwrap();
        
        let rx = ctx.local.rx;

        rprintln!("process received bytes");
            

        let mut bytes = [0u8; 32];
        let mut index = 0;
        let mut checksum: u16 = 0;

        while index != 32 {
            if let Ok(byte) = rx.read() {
                if index == 0 && byte != 32 {
                    // Ignore bytes until the first byte is 32 (space character)
                    continue;
                }
                bytes[index] = byte;
                index += 1;
            }
        }

        //rprintln!("bytes: {:?}", bytes);
        let mut channel_values: [u16; 16] = [0; 16];

        for i in (0..bytes.len()).step_by(2) {
            if i + 1 < bytes.len() {
                // Extract two bytes from the pair
                let byte1 = u16::from(bytes[i]);
                let byte2 = u16::from(bytes[i + 1]);

                if i < 30 {
                    checksum += byte1 + byte2;
                }
                
                // Combine the bytes by multiplying the second number by 256
                //let combined_value = u16::from(byte1) + u16::from(byte2) * 256;
                let combined_value = byte1 | byte2 << 8;

                let channel_index = i / 2;
                //rprintln!("index: {:?}", channel_index);
                // Do something with the combined value (e.g., print or use it)

                channel_values[channel_index] = combined_value;
                //rprintln!("Combined Value: {}", combined_value);
            } else {
                break
            }
        }

        if 65535 - channel_values[15] != u16::from(checksum) {             
            rprintln!("{:?} != {:?}", 65535 - channel_values[15], checksum);                            
        } else {
        
            //rprintln!("Channels: {:?}", channel_values);
            let mut _throttle = channel_values[3] as f32;
            let mut _desired_roll = map(channel_values[1] as f32, 1000., 2000., -15., 15.);
            let mut _desired_pitch = map(channel_values[2] as f32, 1000., 2000., -15., 15.);

            match ctx.local.prod_flysky.enqueue([_throttle, _desired_roll, _desired_pitch]) {
                Ok(()) => {
                    //rprintln!("Data sent");
                }

                Err(_err) => {
                    // Other errors occurred, handle them appropriately
                    // Example: println!("Error occurred while enqueueing data: {:?}", err);
                    rprintln!("Flysky failed to enqueue");
                }
            }
        }
        
    }

    /** Mapping Function **/
    
    fn map(x: f32, in_min: f32, in_max: f32, out_min: f32, out_max: f32) -> f32 {
        (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
    }
    


    #[task(binds = TIM4, local=[kalman_timer, imu, x_kalman, y_kalman, prod_kalman], priority = 1)]
    fn sensor_fusion(ctx: sensor_fusion::Context) {
        ctx.local.kalman_timer.clear_all_flags();
        ctx.local.kalman_timer.start(15.millis()).unwrap();

        rprintln!("Sensor fusion");

        let delta_sec = 0.015;

        let accel_data: [f32; 3];
        let gyro_data: [f32; 3];

        let x_accel: f32;
        let y_accel: f32;

        let imu = ctx.local.imu;
        
        accel_data = imu.read_accel().unwrap();
        gyro_data = imu.read_gyro().unwrap();

        y_accel = atanf(accel_data[0] / sqrtf(accel_data[1] * accel_data[1] + accel_data[2] * accel_data[2])) * 180.0 / PI;
        x_accel = atanf(accel_data[1] / sqrtf(accel_data[0] * accel_data[0] + accel_data[2] * accel_data[2])) * 180.0/ PI;

        ctx.local.x_kalman.process_posterior_state(gyro_data[0], x_accel, delta_sec);
        ctx.local.y_kalman.process_posterior_state(gyro_data[1], y_accel, delta_sec);
        
        // [roll, pitch]
        match ctx.local.prod_kalman.enqueue([ctx.local.y_kalman.get_angle(), ctx.local.x_kalman.get_angle()]) {
            Ok(()) => {
                //rprintln!("Data sent");
            }

            Err(_err) => {
                // Other errors occurred, handle them appropriately
                // Example: println!("Error occurred while enqueueing data: {:?}", err);
                rprintln!("IMU Data failed to send");
            }
        }
    }

    #[task(binds = TIM5, 
           local = [fc_timer, con_kalman, con_flysky, 
                    m1, m2, m3, m4,
                    pitch_pid, roll_pid], 
            priority = 1)]
    fn flight_controller(ctx: flight_controller::Context) {
        ctx.local.fc_timer.clear_all_flags();
        ctx.local.fc_timer.start(FC_DELTA.millis()).unwrap();

        let m1 = ctx.local.m1;
        let m2 = ctx.local.m2;
        let m3 = ctx.local.m3;
        let m4 = ctx.local.m4;

        let pitch_pid = ctx.local.pitch_pid;
        let roll_pid = ctx.local.roll_pid;

        let mut kalman_data: [f32; 2] = [0., 0.];
        let mut fly_sky_data: [f32; 3] = [0., 0., 0.];

        rprintln!("FC");

        if let Some(data) = ctx.local.con_kalman.dequeue() {
            kalman_data = data;
            rprintln!("Kalman Data: {:?}", data);
        }
        
        if let Some(data) = ctx.local.con_flysky.dequeue() {
            fly_sky_data = data;
            rprintln!("Flysky Data: {:?}", data);
        } 

        // Error
        let _error_roll =  kalman_data[0];
        let _error_pitch = kalman_data[1];

        // PID
        let _pitch_pid_value = pitch_pid.compute(_error_pitch, FC_DELTA as f32 / 1000.);
        let _roll_pid_value = roll_pid.compute(_error_roll, FC_DELTA as f32 / 1000.);


        let mut throttle: [f32; 4] = [0.; 4];

        throttle[0] = fly_sky_data[0] - _pitch_pid_value - _roll_pid_value; // front left
        throttle[1] = fly_sky_data[0] - _pitch_pid_value + _roll_pid_value; // front right
        //throttle[1] = 1000.;
        
        throttle[2] = fly_sky_data[0] + _pitch_pid_value - _roll_pid_value; // rear left
        throttle[3] = fly_sky_data[0] + _pitch_pid_value + _roll_pid_value; // rear right

        for i in 0..4 {
            if throttle[i] < 1000. {
                throttle[i] = 1000.;
            }
            if throttle[i] > 2000. {
                throttle[i] = 2000.;
            }   
        }
        rprintln!("Pitch PID: {:?}, Roll PID: {:?}", _pitch_pid_value, _roll_pid_value);
        rprintln!("Throttle: {:?}", throttle);


        m1.set_duty(map(throttle[0], 1000., 2000., m1.get_max_duty() as f32 / 20., m1.get_max_duty() as f32 / 10.) as u16); 
        m2.set_duty(map(throttle[1], 1000., 2000., m2.get_max_duty() as f32 / 20., m2.get_max_duty() as f32 / 10.) as u16);
        m3.set_duty(map(throttle[2], 1000., 2000., m3.get_max_duty() as f32 / 20., m3.get_max_duty() as f32 / 10.) as u16);
        m4.set_duty(map(throttle[3], 1000., 2000., m4.get_max_duty() as f32 / 20., m4.get_max_duty() as f32 / 10.) as u16);
    }
}

PCB

I initially used a breadboard for prototyping and testing each component. Afterwards I got each component working together, I designed a PCB to make connections for the ESC, IMU, FlySky Receiver, and the microcontroller on one board.

Current Issues

While the drone is assembled, stable flight has not yet been achieved. I suspect the root cause is the sporadic values from the IMU due to the vibrations from the motor messing with the accelerometer values. As a result of the IMU’s messy values, the PID is unable to get a reliable error to correct on. I’m not yet sure if this is due to my Kalman Filter implementation, if it can yet be improved with some tweaks on the R or Q values, or if I may have to switch to a different Kalman Filter crate such as yakf. This issue can be observed in the video below and the program’s output.

FC
Kalman Data: [-0.90039134, 0.12164825]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -6.5124507, Roll PID: -10.382492
Throttle: [1239.8949, 1219.13, 1226.87, 1206.1051]
Sensor fusion
process received bytes
FC
Kalman Data: [-2.0230548, 0.48110035]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 3.458548, Roll PID: -11.619747
Throttle: [1231.1613, 1207.9218, 1238.0782, 1214.8387]
Sensor fusion
process received bytes
FC
Kalman Data: [1.2617773, 1.6009094]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 10.767213, Roll PID: 24.335114
Sensor fusion
FC
Sensor fusion
FC
FC
Kalman Data: [-2.1917682, 0.08220053]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -1.3731648, Roll PID: -14.535976
Throttle: [1238.9092, 1209.8372, 1236.1628, 1207.0908]
Sensor fusion
process received bytes
FC
Kalman Data: [1.738786, 1.4941175]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 12.501015, Roll PID: 29.591108
Throttle: [1180.908, 1240.0901, 1205.9099, 1265.092]
Sensor fusion
process received bytes
FC
Kalman Data: [-2.5784974, 0.37395483]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -6.6198416, Roll PID: -34.032913
Throttle: [1263.6528, 1195.5869, 1250.4131, 1182.3472]
Sensor fusion
process received bytes
FC
Kalman Data: [0.20860016, 1.2831023]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 8.727188, Roll PID: 18.904137
Throttle: [1195.3687, 1233.177, 1212.823, 1250.6313]
Sensor fusion
process received bytes
FC
Kalman Data: [-0.7358496, 0.118415]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -7.427752, Roll PID: -7.862849
Sensor fusion
process received bytes
FC
Kalman Data: [1.1448559, 1.5381482]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 7.8262134, Roll PID: 19.894714
Throttle: [1195.279, 1235.0686, 1210.9314, 1250.721]
Sensor fusion
process received bytes
FC
Kalman Data: [-2.4780583, 0.006271422]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -10.099969, Roll PID: -29.20888
Throttle: [1262.3088, 1203.8911, 1242.1089, 1183.6912]
Sensor fusion
process received bytes
FC
Kalman Data: [-1.4668975, 0.97744393]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 8.529371, Roll PID: 3.7072783
Throttle: [1210.7633, 1218.1779, 1227.8221, 1235.2367]
Sensor fusion
process received bytes
FC
Kalman Data: [-0.10233903, 0.5720601]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -1.4584388, Roll PID: 8.792378
Throttle: [1215.6661, 1233.2509, 1212.7491, 1230.3339]
Sensor fusion
process received bytes
FC
Kalman Data: [0.4794494, 1.6821591]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 10.864978, Roll PID: 4.738208
Sensor fusion
FC
Sensor fusion
FC
FC
Kalman Data: [-2.4574478, 0.19115067]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -9.944306, Roll PID: -28.328514
Throttle: [1261.2728, 1204.6158, 1241.3842, 1184.7272]
Sensor fusion
process received bytes
FC
Kalman Data: [1.0313929, 1.3718957]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 10.7154255, Roll PID: 25.22327
Throttle: [1187.0613, 1237.5078, 1208.4922, 1258.9387]
Sensor fusion
process received bytes
FC
Kalman Data: [-0.7749517, 0.5282671]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -4.4676566, Roll PID: -13.691817
Throttle: [1241.1594, 1213.7759, 1232.2241, 1204.8406]
Sensor fusion
process received bytes
FC
Kalman Data: [-0.1934669, 0.8849101]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 4.2474403, Roll PID: 3.3897264
Throttle: [1215.3628, 1222.1423, 1223.8577, 1230.6372]
Sensor fusion
process received bytes
FC
Kalman Data: [-1.54417, 0.5238842]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: -1.2590712, Roll PID: -12.1930275
Sensor fusion
FC
Sensor fusion
FC
Kalman Data: [0.26610875, 1.2130305]
Flysky Data: [1223.0, -0.72000027, -5.16]
Pitch PID: 6.4048543, Roll PID: 10.804463
Throttle: [1205.7906, 1227.3995, 1218.6005, 1240.2094]
Sensor fusion
process received bytes