Skip to content

Software

Jump to bottom
Y. Deemo Chen edited this page May 16, 2024 · 20 revisions

Flight Control

The Flight Software on the Raspberry Pi Pico is written in pure C with the Pico C/C++ SDK.

Code Repo: https://github.com/MotionStudioCornell/BritneyDrone/tree/master/software

Components Used

The parts being directed connected to the Pico are:

(The schematics is not yet updated, thus does not reflect the current build)

How it Works

The current state of the flight software is a one-core, single-threading system with interrupts. It takes input from the handheld controller, sends data to the radio receiver, decodes the throttle information, converts to motor control PWM, collects IMU data, and sends the control signal to the ESC, which generates a three-phase voltage to control the speed of the motors.

System Overview

The flight control system can be divided into two subsystems: State Estimation and State Control. Blank diagram (5)

State Estimation

The controller system has its own de-noise filter implemented, namely the "leaky filter."

Which is a simple low-pass filter with one hyper-parameter $\alpha$. Its output can be expressed as:

$$\text{output(t+1)}=\alpha * \text{input(t)} + (1-\alpha) * \text{ouput(t)}$$

And we use it to do a simple low-pass for gyroscope and accelerometer data.

With the gyroscope and accelerometer values denoised. We use complementary filters to estimate the drone's roll, pitch, and yaw.

The specifies of complementary filters can be found on Hunter's website. However, due to its limitations, we can only estimate the roll and pitch. Thus, yaw is just estimated by integrating the gyro z value.

State Control

We implement a simple PID controller, which can be orthogonalized into PID roll, PID pitch, and PID yaw control.

Each of them is a 2D PID controller.

For roll, we are controlling the rpm of the left and right side propellers.

For pitch, we are controlling the rpm of the front and back side propellers.

For yaw, we are controlling the rpm of the diagonal propellers.

What is worth noting is we can use the gyro readings directly as the values needed for the derivative term computation.

An example to calculate the roll contribution to the final thrust:

  float roll_P = my_controller->Kp * (error.roll);
  float roll_I = my_controller->Ki * (my_controller->integral_error.roll);
  float roll_D = my_controller->Kd * (d_error.roll);

  motor1 += -roll_P - roll_I - roll_D;
  motor2 += -roll_P - roll_I - roll_D;

  motor3 += roll_P + roll_I + roll_D;
  motor4 += roll_P + roll_I + roll_D;

The pseudocode for the flight software entry point P2.c can be expressed as:

radio_interrput(){
  throttle = new_throttle
}

setup(){

  setup_serial()
  setup_LED()
  setup_radio()

  setup_IMU()
  calibrate_IMU()

  setup_ESC()
  calibrate_ESC()
  arm_ESC()

}

while(1){
  update_imu()
  control_motor(throttle)
}

Code Structure

- controller/
  - build/
  - src/
    - P2.c
    - CMakelists.txt
  - lib/
    - ESC/
      - ESC.h
      - ESC.c
    - MPU9250/
      - mpu9250.h
      - mpu9250.c

P2.c is the main entry point of the flight software. Under lib/, we have two main libraries we have developed for the ESC and the IMU.

Build

Requirement:

Raspberry Pi Pico C/C++ SDK installed.

In controller/src/

cd build

cmake ../src -DPICO_BOARD=pico_w

make

Flash

Find P2.uf2 in the build/ dir, and drag it onto the Pico MSD(Or any methods you prefer)

Libraries

Electronic Speed Control (ESC) Library

Related files: esc.h, esc.c

As for our ESC, which is loaded with the BLHeli32 firmware(Datasheet), its control signal to the motors is represented by a three-phase voltage signal.

To command the ESC from Pico, we need to send PWM signals with different duty cycles to represent 0%-100% throttle. Turns out, it expects a 50Hz PWM signal, with a 5% duty-cycle representing min throttle(0%) and a 10% duty-cycle representing max throttle(100%).

The ESC library consists of a struct ESC to store information and several functions:

  • esc_setup() takes in an ESC struct, PWM pin information, and other PWM characteristics to initialize the corresponding Pin with the desired frequency/wrap/slice/level and enables the PWM.

  • cali_motor() calibrates the ESC to determine the min and max throttle. It should be done on an occasional basis; every time the ESC is calibrated, the calibration information is stored on the ESC memory.

  • arm_motor() initiates the arming sequence PWM to the ESC to start taking in motor control commands. In our case, it sends the 0% throttle(10% duty-cycle) PWM signal until two beeps are heard.

  • motor_control() takes in an ESC struct, a percent_throttle variable in [0,1], and a MOTOR_NUM variable. It converts the throttle signal to the corresponding PWM signal and sends it to the desired motor.

Inertia Measurement Unit (IMU) Library

Related files: mpu9250.h, mpu9250.c

We chose the MPU9250 IMU for its SPI connection, which enables a higher data transmission frequency than a regular I2C protocol. This library is universal for any generic MPU9250s.

Data Sheet: https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf

Register Map: https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-9250-Register-Map.pdf

(This library is quite low-level, as it involves register-level interaction.)

The MPU9250 library consists of a struct mpu9250 to store sensor data(raw, bias, calibrated) and SPI pin.

Low-Level Functions:

  • cs_select() clears the assembly with nops and actives the chip select pin

  • cs_select() clears the assembly with nops and deactivates the chip select pin

  • read_registers() is a helper function that uses SPI to read imu register values

  • convert() is a helper function that converts the raw sensor data from the register to correct data using the corresponding sensor sensitivity.

High-Level Functions:

  • mpu9250_setup() takes a set of SPI pins and properly sets up. Then, it starts the SPI connection by sending reg 0x6B data 0x01.

  • gyro_cal() takes in a buffer_size variable, collects that many gyroscope data and calculates a bias by taking the average, then finally stores it in the mpu9250 struct

  • acc_cal() takes in a buffer_size variable, collects that many accelerometer data and calculates a bias by taking the average, then finally stores it in the mpu9250 struct

  • mpu9250_update() reads raw data from the register, converts it to correct data depending on the sensor sensitivity, and applies the offsets, finally stores it in the mpu9250 struct.

Radio Module "Library"

Related files: P2.c

The related code for the Radio receiver is not yet to be organized as a full-blown library, but its functionality is almost complete.

  • radio_setup() set up the UART connection with the radio receiver using uart1

  • radio_irq_handler() is the interrupt handler for the radio UART. It reads the UART character-by-character, stores them in a moving buffer, and converts the corresponding character to a global variable throttle that is being used to control the motor by calling motor_control()

Clone this wiki locally