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;
}
