Monday 2 March 2020

Custom Flight Controller Part 4.2: Final Result

It's time to let my drone fly!

The following video shows the test flight. It could be improved by further tuning PID, but I personally is happy with the performance.


Conclusion:

Three years ago, I have thought about doing this as my final year project. And now I am just happy that I have finally finished this project.

Hope that this blog series could be helpful to whoever reading it and hope that the next project of  mine would not drag so long ever again.

Last but not least, some shot of my final quadcopter!

Custom Flight Controller Part 4.1: Assembly and Tuning PID

This is the final part of the project. In this part, with all the software and hardware ready,  I will document how I assemble all parts together and present my final quadcopter build.

Assembling quadcopter components is relatively easy and there are a lot of blog posts online elaborating the detailed process. In this post, I will simply document one challenge I faced. 

During assembly, I had to enforce the correct rotation direction of each motor by soldering the wires onto the ESC but there is no way of knowing which way motor will rotate with respect to a specific wire connection other than actually running it. However, since I write my own flight control software, I do not have to have a fixed motor rotation direction. That means it's OK for me to have front left motor spinning either clockwise or anti-clockwise. All I have to do is to compensate it in software later on. Therefore, what I needed to do was simply make sure that the the spinning direction of front left motor is the same as the spinning direction of back right motor and their spinning direction is opposite to that of motors on the front right motor and back left. To achieve that, I just needed to swap one wire connection for one pair of motor as elaborated in the picture below. With this trick, there is no need to actually connect the battery and run the motor. 
The wire connection for motor 2 and 4 is different from wire connection for motor 1 and 3.


Tuning PID


After assembling the quadcopter, it is time for the final but arguably most painful part of the whole project, tuning PID. There are a lot of guides about how to tune PID for quadcopter. However, most of them uses betaflight flight software which only has one set of PID. In my case, my control structure is more similar to Pixhawk 4's and thus I followed their guide mainly. Also, being able to adjust PID value using transmitter as documented in one of the previous post helped enormously.

As suggested, you have to tune inner loop attitude rate PID first. To do that, I added a macro defined flag UAV_CMD_ATT_RATE in my top-level UAV_Defines.h header file. When this flag is set, the flight control software will execute attitude control only and the data from transmitter will be assumed to set desired attitude rate. In another word, the outer attitude control loop is completed omitted in this mode. In existing flight control software, this is often referred to as Acro Mode.

The effect of different value of PID in this mode is clear. If P is too high, the quadcopter will start to oscillate, but if P is too low, the quadcopter will not respond to your command. D value will help prevent oscillation caused by large P but if D value is too high, it will lead to oscillation as well. My steps of tuning attitude rate PID is as follows:
  1. Set pitch and roll rate PID to all zero.
  2. Slowly increase yaw rate P value if needed so that the quadcopter's yaw is relatively stable.
  3. Slowly increase pitch and roll P value until the quadcopter starts to respond to user command or the quadcopter starts to oscillate.
  4. If the quadcopter starts to oscillate before it responds to user command, increase D until the oscillation disappears. 
  5. Repeat step 3 and 4 until the quadcopter is responsive enough.
  6. Increase yaw rate P value a little bit. Make sure it does not cause oscillation.

After the inner loop is tuned, I disabled the flag and put the flight control software back to attitude control mode, and proceed to tune attitude PID in this mode without changing the value of inner loop PID. The PID value is adjusted in similar fashion. This time, with a good P and D value, the quadcopter should be able to hover in air, pitch, roll, yaw in response to your command.

Finally, the quadcopter is ready to fly.




Custom Flight Controller Part 3.2: Hardware Components

This posts list all components used for building the quadcopter other than the electronic components mentioned in the previous posts.

Frame
DALRC DL265

ESC
DALRC Engine 40A Pro 4in1 ESC

Motor
T-Motor F40 Pro II 1600KV

Propoller
Gemfan 6040 Bullnose

RC Receiver
Frsky XSR 2.4GHz 16CH

RC Transmitter
Frsky Taranis Q X7

IMU breakout board
GY-9250

Battery
Turnigy nano-tech 3300mAh 4S 25~50C

Other
Battery straps
XT90 to deans converter for charging battery
Battery voltage checker

With the above components, the estimated performance of my quadcopter is summarized in the screenshot from eCalc below. As shown in the screenshot, this build is quite balanced overall.

Do note that the whole build costs more than 300 Singapore dollars which is relatively expensive. The main cost lies in the 4in1 ESC, the motor and the RC transmitter.


Custom Flight Controller Part 3.1: STM32F103 Blue Pill and Other Electronic Components

Finally Part 3! In this part, I will talk about hardware component for my flight controller project. This post focuses on the MCU I chose to be the main processing unit for my flight controller as well as other electronic components I installed on the breadboard to form the complete circuit as my custom flight controller.

As mentioned in previous post, originally I purchased STM32F429 Nucleo board because I intended to run Quaternion Kalman Filter. But it turned out that the Nucleo board is too big for my quadcopter frame. In addition, as mentioned before, Madgwick filter performs equally well and is much more lightweight. There is really no strong reason to use the larger Nucleo board anymore. Therefore, I decided to switch to a much smaller board widely known as the "blue pill" which features STM32F103. Although it does not have a floating point calculation unit and is much slower than STM32F429, it should be still fast enough to run Madgwick Filter. Also, Joop Brokking's YMFC-32 project used it and it seems to suffice for controlling quadcopter.

The figure above shows the pinout for STM32F103 on blue pill. I need one I2C for communication with MPU9250, 4 timer PWM outputs for motor control, two UART communication, one for receiver and another for debug. 

With blue pill, I am able to fit all components on two bread boards and install them on my quadcopter frame. 



Note that the receiver SBUS signal passes through a logic inverter before entering STM32F103. This is because SBUS protocol is inverted. More on SBUS will be described in later posts. 

I purchased an AMS1117 3.3V regulator module to get 3,3V from 5V ESC BEC output to power STM32F103 as well as MPU9250. Receiver is powered directly from 5V. This means that the SBUS signal sent from receiver is 5V. Luckily, UART3 pins of STM32F103 are 5V tolerant. 

For debugging, I purchased FT232RL FTDI USB to TTL Serial converter so that the debug message sent through UART2 can be received by computer through USB. 

For downloading code into STM32F103, instead of the using popular method through Arduino, I used ST-Link V2 on Nucleo board. You just have to remove two jumpers from Nucleo board and connect the wires accordingly. Note that using this method, you do not have to toggle the jumper positions. 


Custom Flight Controller Part 2.10: (Optional) Tune PID using Transmitter


This post documents how to add support for tuning PID using transmitter. The complete code can be found in my Github.

PID tuning is known to be a very tedious process. Being able to tune using transmitter will greatly save time as one does not need to do re-flash the program every time after changing just one single value. 

To tune PID using transmitter, one first needs to enable more channels in RC transmitter. The transmitter I use is Frsky Taranis Q X7. I referred to this video to set up channel 5 of the RC transmitter to report the value of SF switch. 

In the main application, if the value of channel 5 is detected to be at its maximum, it will enter PID tuning mode. In this mode, instead of controlling quadcopter, roll, pitch and yaw sticks will adjust P, I, D values respectively. An example is given below 

if (cmd.desiredPitch == CMD_PITCH_MIN) {
    float curKp = Controller::GetInstance().mAttController_pitch.GetKp() - 0.1;
    LOGI("TunePID: Kp to %f\r\n", curKp);
    Controller::GetInstance().mAttController_roll.SetKp(curKp);
    Controller::GetInstance().mAttController_pitch.SetKp(curKp);
    LED_Blink(LED_ONBOARD, 4);
}
If channel 5 is detected to be at its minimum, the main application will switch back to normal operation mode and user can proceed to arm and fly the quadcopter normally. 

Despite the above setup, I still had to manually try and adjust the resolution of change of each PID value. Furthermore, I need to remember how many times I have adjusted each value and update the default PID values accordingly after each trial. So tuning PID still remains a relatively tedious process especially in my case where I do not have a set of working default value. 

Custom Flight Controller Part 2.9: Main Application

This post documents the implementation of the main application. The complete code can be found in my Github.

In previous posts, I have documented various components of the flight controller software. With all these components in place, all that left is to write a main application to call these components when desired.

At the start of main_app.c, it will try to initialize all components. This is where the app waits for RC transmitter to connect for example. After all the initialization is finished, it will enter an infinite loop in which it calls respective components at a fixed frequency.

To control frequency of each action, I used system interrupt service routine to set respective flag at fixed timer count. Since I am using STM32 HAL library, there is already a SysTick_Handler() in stm32f1xx_it.c and this handler gets called every 1ms regardless of your system clock frequency. I added my handler MainApp_OnCoreTimerTick() to the SysTick_Handler() function and it looks like the following.

void MainApp_OnCoreTimerTick(void)
{
    if (!sStarted) return;
    ++sTimerCnt;
    if (sTimerCnt >= sReadSensorCnt) {
        sReadSensorCnt += READ_SENSOR_CNT;
        sReadSensorFlag = true;
    }
    if (sTimerCnt >= sEstimateStateCnt) {
        sEstimateStateCnt += ESTIMATE_STATE_CNT;
        sEstimateStateFlag = true;
    }
    if (sTimerCnt >= sListenCmdCnt) {
        sListenCmdCnt += LISTEN_CMD_CNT;
        sListenCmdFlag = true;
    }
    if (sTimerCnt >= sControllerAttCnt) {
        sControllerAttCnt += CONTROL_ATT_CNT;
        sControllerAttFlag = true;
    }
    if (sTimerCnt >= sControllerAttRateCnt) {
        sControllerAttRateCnt += CONTROL_ATT_RATE_CNT;
        sControllerAttRateFlag = true;
    }
    if (sTimerCnt >= TIMER_CNT_MAX) {
        sTimerCnt = 0;
        sReadSensorCnt = READ_SENSOR_CNT;
        sEstimateStateCnt = ESTIMATE_STATE_CNT;
        sListenCmdCnt = LISTEN_CMD_CNT;
        sControllerAttCnt = CONTROL_ATT_CNT;
        sControllerAttRateCnt = CONTROL_ATT_RATE_CNT;
    }
}
Inside my handler, I just increased the main timer count and set a flag to true if the timer count reaches the desired threshold. For example, the attitude rate control loop needs to run at 100Hz. Therefore, sControllerAttRateCnt is set to be 10. As a result, every time when sTimerCnt reaches 10, 20, 30 etc, sControllerAttRateFlag will be set to true and the infinite loop in main_app will run attitude rate control.

There are in total five actions performed in the infinite loop as shown in the code snippet above. Their respective frequency is summarized as follows.

#define READ_SENSOR_CNT 10 // 1000/100hz
#define ESTIMATE_STATE_CNT 20 // 1000/50hz
#define CONTROL_ATT_CNT 50 // 1000/20hz
#define CONTROL_ATT_RATE_CNT 10 // 1000/100hz
#define LISTEN_CMD_CNT 250 // 1000/4hz

Every 10 millisecond, the main application will read sensor measurements from MPU9250 using I2C as documented in Part 2.2 as well as run the attitude rate control loop. Every 20 ms, the main application will run sensor fusion to estimate the current attitude of the quadcopter as documented in Part 2.3. Every 50ms, main application will run attitude control loop. Finally every 250ms, the main application will get user command from RC receiver via SBUS protocol as documented in Part 2.5.

In addition, I also implemented an arming and disarming sequence. Basically, you do not want arm ESC immediately every time when flight controller is powered on. This might be dangerous as any accidental throttle command will start to spin propellers. In this project, the arming and disarming is achieved by enabling and disabling PWM output to ESC. Particularly, if user pushes both transmitter sticks towards the bottom center, the quadcopter will be armed. If user pushes pushes transmitter sticks towards opposite bottom direction, the quadcopter will be disarmed. This is shown in pictures below.
arm
disarm



Implementing this in code is straight forward. When both stickers are pushed to the center, the throttle, roll and pitch command are at their minimum while yaw command is at its maximum as shown in the code snippet below.
static bool ToArm(FCCmdType& cmd)
{
    if (cmd.desiredPitch == CMD_PITCH_MIN && cmd.desiredRoll == CMD_ROLL_MIN
        && cmd.desiredYawRate == CMD_YAW_RATE_MIN && cmd.desiredAccZ == CMD_ACC_MIN) {
            return true;
        }
    return false;
}

Custom Flight Controller Part 2.8: Gyro and Accelerometer Calibration

This post documents the implementation of the gyro and accelerometer calibration. The complete code can be found in my Github.

Unfortunately raw IMU data tends to have bias and noise, and they need to be calibrated. For Gyro, the calibration aims to remove static bias, that is, when the quadcopter lies still, the gyro reading should be zero across all axes. Similarly, we want accelerometer to report acceleration only in vertical z direction when quadcopter lies still on a horizontal surface. 

With the above principle, I implemented a simple piece of code to calibrate IMU. When CalibrateSensorBias() is called, it will collect 100 data from both gyrocopter and accelerometer in each axis and average them to be the bias for each axis. Afterwards, the bias calculated will be deducted from raw sensor reading when GetGyroData() or GetAccData() is called.

In my case, my accelerometer is found to have about 0.024 m/s/s in x direction, 0.01 m/s/s in y direction and 0.045 m/s/s in z direction. After calibration, it helped mitigate the problem that my quadcopter keeps drifting to one side.