Skip to content

Software

Jump to bottom
basfora edited this page Jul 29, 2024 · 1 revision

Before You Go Too Far...

You will need some experience with microcontroller programming(best if you use Pico) and some familiarity with C. In this document, we divide the software into TWO categories.

APIs: Libraries that do the messy work(The tools). They do work, but they also serve as guidance for new developers to improve and modify. For example, the radio library reads the UART data from the radio receiver and decodes the hex string. It works, but if a new protocol is being used, developers can print out the hex string themselves and figure out which controls which with minor modifications.

Demo Codes: Part of the code/libraries is written as a reference. For example, a minimal state estimation and a PID controller are implemented. Readers might want to design more sophisticated controllers/estimators.

System Overview

Let's start with an overview of a minimalistic flight control system. Such a flight controller is a one-core, single-threaded system with interrupts. There are two kinds of control: Human Input and Stabilization. Human Input is an outer-loop control signal, whereas stabilization control is an inner-loop signal.

Outer Loop Flow Chart:

Handheld Controller ==> Radio Receiver ==> Thrust A

Inner Loop Flow Chart:

IMU Data ==> Denoise => State Estimation ==> State Control ==> Thrust B

The final thrust will be the combination of the two, for example:

Final Thrust = Thrust A + Thrust B

This value will be passed into the ESC library to operate the motors.

API Documentation

PWM Electronic Speed Control (ESC) Library

Related files: ESC/esc.h, ESC/esc.c

As for our ESC, which is loaded with the BLHeli Based 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, 100], and a MOTOR_NUM variable. It converts the throttle signal to the corresponding PWM signal and sends it to the desired motor.

Example

The following code calibrates the ESC, arms it, and makes each motor spin at 30%.

#include <stdio.h>
#include <stdlib.h>
#include "pico/stdlib.h"
#include "../lib/ESC/esc.h"

// ESC const
#define PWM_WRAP 100000 // counts
#define PWM_FREQ 50     // hz
#define MAX_DUTY 0.1    // 10%
#define MIN_DUTY 0.05   // 5%

// PIN definition
#define PIN_PWM1 21
#define PIN_PWM2 20
#define PIN_PWM3 19
#define PIN_PWM4 18

static ESC esc;

int main()

{

  uint32_t PWM_PIN[4] = {PIN_PWM1, PIN_PWM2, PIN_PWM3, PIN_PWM4};
  esc_setup(&esc, PWM_PIN, PWM_FREQ, PWM_WRAP, MIN_DUTY, MAX_DUTY);

  //only needs to be done once with new batteries/motors
  //it makes a long sequence of beeps (check BLHeli Datasheet)
  cali_motor(&esc);
  //arm the motor so it can be commanded to spin
  //it makes a lower tone beep followed by a higher tone beep
  arm_motor(&esc);

  //set to be 30% throttle for all four motors
  motor_control(&esc, 30, 1);
  motor_control(&esc, 30, 2);
  motor_control(&esc, 30, 3);
  motor_control(&esc, 30, 4);


}

Radio Library

Related files: Radio/radio.h, ESC/radio.c

We are getting data from UART, and the data is presented under what's called the "CRSF" protocol, which is really not a well-documented protocol. Sometimes, when you are trying to figure out what happens when you push the joystick or some random button, you can print out the data in real-time and see what changes.

We have a radio structure to store the necessary information. An interrupt handler automatically processes the new UART data into the struct(it uses uart1).

  • radio_setup() takes in the radio struct, pin_rx, and pin_tx to setup the radio.

  • radio_irq_handler() is an IQR handler that reads the UART data and stores them in a buffer. It then processes the buffer by extracting useful information(such as the joystick's position). You should take a look at the original code to see how to print out all the data.

Example

The following code initializes the radio and sets up the IQR. And prints out the position of the left joystick in percentage.

#include <stdio.h>
#include <stdlib.h>
#include "pico/stdlib.h"
#include "hardware/uart.h"
#include "hardware/irq.h"
#include "../lib/Radio/radio.h"

#define RADIO_TX 8
#define RADIO_RX 9

static radio rdo;

int main()

{
  radio_setup(&rdo, RADIO_RX, RADIO_TX);
  print(rdo.throttle);

}

MPU9250 Inertia Measurement Unit (IMU) Library

Related files: MPU9250/mpu9250.h, MPU9250/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

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.

Example

The following code prints out the gyroscope and accelerometer data in the serial port.

#include <stdio.h>
#include <stdlib.h>
#include "pico/stdlib.h"
#include "../lib/MPU9250/mpu9250.h"

// Connection
// GPIO 4(pin 6)MISO / spi0_rx→ ADO on MPU9250 board
// GPIO 5(pin 7)Chip select → NCS on MPU9250 board
// GPIO 6(pin 9)SCK / spi0_sclk → SCL on MPU9250 board
// GPIO 7(pin 10)MOSI / spi0_tx → SDA on MPU9250 board

#define PIN_MISO 4
#define PIN_CS 5
#define PIN_SCK 6
#define PIN_MOSI 7

static mpu9250 imu;

int main()

{
  // Serial
  stdio_init_all();

  //Set up the IMU and calibrate it.
  mpu9250_setup(&imu, PIN_CS, PIN_MISO, PIN_SCK, PIN_MOSI);
  printf("Calibrating Gyro....Keep it Still...\n");
  gyro_cal(&imu, 50);
  acc_cal(&imu, 50);
  printf("Done. Offsets: w_x: %.5f, w_y:%.5f, w_z:%.5f \n", 
  imu.w_offsets[0], imu.w_offsets[1], imu.w_offsets[2]);

  while (1)
  {

    mpu9250_update(&imu);

    printf("wx:%f, wy:%f, wz:%f, ax:%f, ay:%f, az:%f, \n",
    imu.w[0], imu.w[1], imu.w[2],
    imu.a[0], imu.a[1], imu.a[2]);
   
  }

}

Demo Code

In this section, we will talk about currently running code representing a minimalistic feedback controller.

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 specifications for complementary filters can be found on Hunter's website. However, we can only estimate the roll and pitch due to its limitations. Thus, yaw is just estimated by integrating the gyro z value. It is highly suggested to look into filters like Madgwick, which uses the magnetometer data(we have it, but it is not implemented in the IMU library)

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;

fc

Leaky Low-Pass Filter Library

Related files: Filter/leaky_LP.h, Filter/leaky_LP.c

  • leaky_init() takes in the struct and the alpha value and sets the struct

  • leaky_update() takes in the struct and the input and output array and computes the output

Example

The following code shows how to setup the LP filter and filter IMU gyro and accelerometer data.

#include <stdio.h>
#include <stdlib.h>
#include "pico/stdlib.h"
#include "../lib/Filter/leaky_LP.h"

static leaky_lp w_filter;
static leaky_lp a_filter;

int main()

{
  // Serial
  stdio_init_all();

  float wf[3];
  float af[3];

  //See the IMU library doc for imu.a and imu.w
  leaky_update(&w_filter, imu.w, wf);
  leaky_update(&a_filter, imu.a, af);

}

PID Controller Library

The controller library also includes the complementary filter for state estimation.

  • init_controller() takes in the controller struct, complementary filter alpha value, timestep size, the PID parameters for pitch/roll, and the PID parameters for yaw. And sets up the structure.

  • update_controller() takes in the controller struct, and the denoised IMU data. It does the state estimation using a complementary filter and then updates the control throttle.

Example

The following code sets up the controller, runs for fixed control loop timing, and prints out the throttle.

#include <stdio.h>
#include <stdlib.h>
#include "pico/stdlib.h"
#include "../lib/Control/controller.h"
#include <math.h>
#include <time.h>


#define Default_Kp 0.2
#define Default_Ki 10
#define Default_Kd 0.015

#define Default_yaw_Kp 0.2
#define Default_yaw_Ki 10
#define Default_yaw_Kd 0.015

// controller
static controller fc;

// control cycle timing
#define dT_ms 20
#define dT_s dT_ms / 1000.0

int main()

{
  // Serial
  stdio_init_all();

  init_controller(&fc, 0.98f, dT_s, Default_Kp, Default_Ki, Default_Kd, Default_yaw_Kp, Default_yaw_Ki, Default_yaw_Kd);

  while (1)
  {
    absolute_time_t startTime = get_absolute_time();

    //given some denoised imu data:
    float wf[3];
    float af[3];
    update_controller(&fc, wf, af);
    
    printf("motor1: %.2, motor2: %.2, motor3: %.2, motor4: %.2 \n ", fc.u.t1,fc.u.t2,fc.u.t3,fc.u.t4);

    absolute_time_t endTime = get_absolute_time();
    int64_t time_diff_ms = absolute_time_diff_us(startTime, endTime) / 1000;
    // to achieve a fixed control loop time, if possible
    u_int32_t sleep_time_ms = dT_ms - time_diff_ms > 0 ? dT_ms - time_diff_ms : 0;
    sleep_ms(sleep_time_ms);

  }


}

@ 2024 | Motion Studio

Clone this wiki locally