If you have programmed Ardupilot and now you switch to PX4, you would really appreciate how easy it is to implement a new flight mode in Ardupilot… PX4 is really good if you just want to write an App to do something, but implementing a new flight mode actually requires a lot of digging around in the lower level of the code base. The lack of documentation and examples on the PX4 website is also not helping for our beginners.

I have been digging around the PX4 code base myself for the last 3 months, and I can finally add a new flight mode. For the purpose of this tutorial, I will be showing you how to implement a flip mode in PX4, it is similar to the Ardupilot flip mode, but in PX4. I will also show you how to switch to this mode via the commander app in serial console. You can also switch to your new mode via RC or MAVlink but it will not be discussed here.

I am still not sure if this is the recommended way to implement new flight modes, so if you see any mistakes in this post please comment.

*This post assumes you know where the uORB topics are and how to use the orb function. If you have questions about uORB please see research log

[4]. If you want to use my development environment please follow research log [2]. To learn how to connect to the Pixhawk serial console, checkout research log [3].


1. Add MAIN_STATE_FLIP to commander_state.msg

First thing you need to do is to let PX4 know that there will be a new flight mode, and the correct place to add this is in the uORB topic “commander_state.msg”. Add a new line:
uint8 MAIN_STATE_FLIP = 14

at the end of the list of main states. The number at the end will be the number of the last existing state +1, so for me MAIN_STATE_MAX is the last state and it has a value of 13, therefore my new flip state will be 14. See screenshot below:

commander_state

commander_state


2. Add option to switch to flip mode in commander app

This step is required if you want to switch to your new flight mode as soon as possible, without recompiling QGroundControl source code or messing with the RC scripts.

Open “commander.cpp” in the /src/modules/commander directory, around line 283 you will see the function “int commander_main(int argc, char *argv[])”. In this function, every if statement corresponds to an app input. Go to the line where the if statement “if (!strcmp(argv[1], “mode”)” is, you will see that the inner most if statement contains a lot of else if statements, and each corresponds to one of our familiar PX4 flight modes. You can add the flip mode option here like in the image below:

commander main function change

The actual mode transition will be handled by “main_state_transition” function in “state_machine_helper.cpp”. Let’s add flip mode in there, and treat this mode similar to Acro and Rattitude mode:

state_machine_helper

2.1 Commander option for displaying current flight mode

Because we are not going to recompile QGroundControl, when we switch to flip mode, QGroundControl is not going to know what this new flight mode is. Therefore to view what our current flight mode is, we need to do it in the console. You can add a new if statement anywhere in the “commander_main” function like in the screen shot below:

commander_currentmode

The numbers corresponds to the integer value in “commander_state.msg”. Don’t forget to let other people know this option exists, do this easily in the “void usage(const char *reason” function, so the PX4_INFO will look like this:
PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|transition|mode|currentmode}\n");


3. Set navigation state

This steps is essential to let PX4 know what navigation state flip mode should be in. The navigation will determine what control mode to use. And for the purpose of our tutorial we will create our own navigation state. Find the topic “vehicle_status.msg” and add the line after the list of navigation state values:
uint8 NAVIGATION_STATE_FLIP = 21 # Flip mode

add_nav_state

Again it is not necessarily 21. Now that PX4 knows what NAVIGATION_STATE_FLIP means, we need to tell PX4 to switch to this navigation state when we switch to flip mode. First we create a case in “set_nav_state” function in “state_machine_helper.cpp” for flip main state:

set_nav_state

Then in the following if statement, in the else, we create a case for flip main state and set navigation state to flip:

set_nav_state_2


4. Set control mode flags

The control mode flags are useful in the mc_att_control app, for example, if “flag_control_attitude_enabled” is set to false, you loose stabilization. When we switch to flip mode, we need to make sure this is set to false, otherwise the controller will be fighting our attitude rate commands. In addition, we will add a new control mode flag called “flag_control_flip_enabled” and set it to true only when we are in flip mode, so that other flight modes cannot flip.

First we add a line at the end of the “vehicle_control_mode.msg”:

add_control_mode

Then we add a case in “set_control_mode” function in “commander.cpp” for flip mode:

set_control_mode

Notice I have set flip, manual and rates to true, since we need rate controller. The manual flag should be set to false as well since we prohibit manual control in flip, but I feel like it can be useful in some situations so I have let it true for now.

You should also set flip flag for all other navigation states as shown in the image, I have set it to false for NAVIGATION_STATE_MANUAL.


5. Check if you can switch to flip mode now

All the preparation for flip mode is now done. To check if your changes are working, boot up your pixhawk and connect to the serial console (alternatively launch SITL with jMAVSim and use the terminal to see the console), type “commander mode flip” followed by “commander currentmode”, you should see a message similar to “current flight mode is flip”.

console_output