You are here:-----PX4 Research Log [11] – Adding a new flight mode (create application)

PX4 Research Log [11] – Adding a new flight mode (create application)

In the last post we have set up the PX4 source code for our new flight mode, the flip mode. In this post I will continue finishing the flip mode by creating a new application in the firmware, which commands the attitude during the flip.


1. Create the flip_control application

Creating a new application in PX4 is easy. Go to the PX4 source folder, in the “Firmware/src/modules” directory, let’s make a new directory called “flip_control”. Make a new source code file “flip_control.cpp” as well as a cmake list file called “CMakeLists.txt” in the same directory,  as shown in the screenshot:

Create new files in the flip_control directory

1.1 CMakeLists.txt

This is quite easy, just copy and paste the following code into the text file:

px4_add_module(
    MODULE modules__flip_control
    MAIN flip_control
    COMPILE_FLAGS -Os
    STACK_MAIN 1200
    SRCS
        flip_control.cpp
    DEPENDS
        platforms__common
    )

1.2 flip_control.cpp

Copy or amend the following code and then paste into flip_control.cpp:

#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
/*
 * flip_control.cpp
 *
 *  Created on: 28Sep.,2016
 *      Author: Zihao
 */


extern "C" __EXPORT int flip_control_main(int argc, char *argv[]);

class FlipControl
{
public:
    /**
     * Constructor
     */
    FlipControl();

    /**
     * Destructor, also kills the main task
     */
    ~FlipControl();

    /**
     * Start the flip state switch task
     *
     * @return OK on success
     */
    int start();

    /**
     * This function handles the Mavlink command long messages
     * It will execute appropriate actions according to input
     */
    void handle_command(struct vehicle_command_s *cmd);

    /**
     * little function to print current flip state
     */
    void print_state();

    /**
     * check for changes in vehicle control mode
     */
    void vehicle_control_mode_poll();

private:
    bool 		_task_should_exit; 		/**< if true, main task should exit */
    int 		_flip_task;				/**< task handle */
    enum FLIP_STATE {
            FLIP_STATE_DISABLED = 0,
            FLIP_STATE_START = 1,
            FLIP_STATE_ROLL = 2,
            FLIP_STATE_RECOVER = 3,
            FLIP_STATE_FINISHED = 4
        }_flip_state;					/**< flip state */

    /* subscriptions */
    int 		_command_sub;
    int 		_vehicle_control_mode_sub;
    int 		_vehicle_attitude_sub;

    /* publications */
    orb_advert_t 	_vehicle_control_mode_pub;
    orb_advert_t 	_vehicle_rates_setpoint_pub;

    struct vehicle_command_s 		_command;				/**< vehicle commands */
    struct vehicle_control_mode_s 	_vehicle_control_mode; 	/**< vehicle control mode */
    struct vehicle_attitude_s 		_attitude;				/**< vehicle attitude */
    struct vehicle_rates_setpoint_s _vehicle_rates_setpoint;			/**< vehicle rate setpoint */

    /**
     * Shim for calling task_main from task_create
     */
    static void task_main_trampoline(int argc, char *argv[]);

    /**
     * Main attitude control task
     */
    void 		task_main();
};

namespace flip_control
{
FlipControl *g_flip;
}

FlipControl::FlipControl() :
        _task_should_exit(false),
        _flip_task(-1),
        _flip_state(FLIP_STATE_DISABLED),

        _command_sub(-1),
        _vehicle_control_mode_sub(-1),
        _vehicle_attitude_sub(-1),

        _vehicle_control_mode_pub(nullptr),
        _vehicle_rates_setpoint_pub(nullptr)

{
    memset(&_command, 0, sizeof(_command));
    memset(&_vehicle_control_mode, 0, sizeof(_vehicle_control_mode));
    memset(&_attitude, 0, sizeof(_attitude));
    memset(&_vehicle_rates_setpoint, 0, sizeof(_vehicle_rates_setpoint));
}

FlipControl::~FlipControl()
{
    _task_should_exit = true;
    flip_control::g_flip = nullptr;
}

void FlipControl::print_state()
{
    warnx("Current flip state is %d", _flip_state);
}

void FlipControl::handle_command(struct vehicle_command_s *cmd)
{
    switch (cmd->command) {
    case vehicle_command_s::VEHICLE_CMD_FLIP_START:

        warnx("Flip initiated");

        _flip_state = FLIP_STATE_START;
        break;

    case vehicle_command_s::VEHICLE_CMD_FLIP_TERMINATE:

        warnx("Flip terminated");

        _flip_state = FLIP_STATE_FINISHED;
        break;
    }
}

void FlipControl::vehicle_control_mode_poll()
{
    bool updated;

    /* check if vehicle control mode has changed */
    orb_check(_vehicle_control_mode_sub, &updated);

    if (updated) {
        orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_vehicle_control_mode);
    }
}

void FlipControl::task_main_trampoline(int argc, char *argv[])
{
    flip_control::g_flip->task_main();
}

void FlipControl::task_main()
{
    /* make sure slip_state is disabled at initialization */
    _flip_state = FLIP_STATE_DISABLED;

    // inner loop sleep time
    const unsigned sleeptime_us = 50000;

    // first phase roll or pitch target
    float rotate_target_45 = 45*3.14/180;
    // second phase roll or pitch target
//	float rotate_target_90 = 89*3.14/180;
    // rotate rate set point
    float rotate_rate = 400*3.14/180;

    // use this to check if a topic is updated
    bool updated = false;

    int poll_interval = 100; // listen to the topic every x millisecond

    /* subscribe to vehicle command topic */
    _command_sub = orb_subscribe(ORB_ID(vehicle_command));
    /* subscribe to vehicle control mode topic */
    _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
    /* subscribe to vehicle attitude topic */
    _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));

    /* advertise control mode topic */
    _vehicle_control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_vehicle_control_mode);

    /* advertise rate setpoint topic */
    _vehicle_rates_setpoint_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_vehicle_rates_setpoint);

    /*
     * declare file descriptor structure, # in the [] means the
     * # of topics, here is 1 since we are only
     * polling vehicle_command
     */
    px4_pollfd_struct_t fds[1];

    /*
     * initialize file descriptor to listen to vehicle_command
     */
    fds[0].fd = _command_sub;
    fds[0].events = POLLIN;

    /* start main slow loop */
    while (!_task_should_exit) {

        /* set the poll target, number of file descriptor, and poll interval */
        int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), poll_interval);

        /*
         * this means no information is coming from the topic in the set interval
         * skip loop
         */
        if (pret == 0) {
            continue;
        }

        /*
         * this means some error happened, I don't know what to do
         * skip loop
         */
        if (pret < 0) {
            warn("poll error %d %d", pret, errno);
            continue;
        }

        /*
         * if everything goes well, copy the command into our variable
         * and handle command
         */
        if (fds[0].revents & POLLIN) {
            /*
             * copy command structure from the topic to our local structure
             */
            orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);

            handle_command(&_command);
        }

        /*
         * check for updates in other topics
         */
        vehicle_control_mode_poll();


        /*
         * switch to faster update during the flip
         */
        while ((_flip_state > FLIP_STATE_DISABLED)&&(_vehicle_control_mode.flag_control_flip_enabled)){

            // update commands
            orb_check(_command_sub, &updated);
            if (updated) {
                orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
                handle_command(&_command);
            }

            bool topic_changed = false;

            // copy vehicle control mode topic if updated
            vehicle_control_mode_poll();

            // disable _v_control_mode.flag_control_manual_enabled
            if (_vehicle_control_mode.flag_control_manual_enabled) {
                _vehicle_control_mode.flag_control_manual_enabled = false;
                topic_changed = true;
            }

            // disable _v_control_mode.flag_conttrol_attitude_enabled
            if (_vehicle_control_mode.flag_control_attitude_enabled) {
                _vehicle_control_mode.flag_control_attitude_enabled = false;
                topic_changed = true;
            }

            // publish to vehicle control mode topic if topic is changed
            if (topic_changed) {
                orb_publish(ORB_ID(vehicle_control_mode), _vehicle_control_mode_pub, &_vehicle_control_mode);
            }

            // update vehicle attitude
            orb_check(_vehicle_attitude_sub, &updated);
            if (updated) {
                orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_attitude);
            }

            // decide what to do based on current flip_state
            switch (_flip_state) {
            case FLIP_STATE_DISABLED:
                // shoudn't even enter this but just in case
                // do nothing
                break;

            case FLIP_STATE_START:
                /*
                 * 400 degree/second roll to 45 degrees
                 */
            {
                _vehicle_rates_setpoint.roll = rotate_rate;
                _vehicle_rates_setpoint.pitch = 0;
                _vehicle_rates_setpoint.yaw = 0;
                _vehicle_rates_setpoint.thrust = 1;

                orb_publish(ORB_ID(vehicle_rates_setpoint), _vehicle_rates_setpoint_pub, &_vehicle_rates_setpoint);

                if ((_attitude.roll > 0.0f && _attitude.roll > rotate_target_45) || (_attitude.roll < 0.0f && _attitude.roll < -rotate_target_45)) {
                    _flip_state = FLIP_STATE_ROLL;
                }

                break;
            }

            case FLIP_STATE_ROLL:
                /*
                 * 400 degree/second roll to 90 degrees
                 */
            {
                _vehicle_rates_setpoint.roll = rotate_rate;
                _vehicle_rates_setpoint.pitch = 0;
                _vehicle_rates_setpoint.yaw = 0;
                _vehicle_rates_setpoint.thrust = 0.75;

                orb_publish(ORB_ID(vehicle_rates_setpoint), _vehicle_rates_setpoint_pub, &_vehicle_rates_setpoint);

                if ((_attitude.roll > 0.0f && _attitude.roll < rotate_target_45) || (_attitude.roll < 0.0f && _attitude.roll > -rotate_target_45)) {
                    _flip_state = FLIP_STATE_RECOVER;
                }
            }
                break;

            case FLIP_STATE_RECOVER:
                /*
                 * level the vehicle
                 */
                _vehicle_control_mode.flag_control_attitude_enabled = true;
                orb_publish(ORB_ID(vehicle_control_mode), _vehicle_control_mode_pub, &_vehicle_control_mode);

                _flip_state = FLIP_STATE_FINISHED;
                break;

            case FLIP_STATE_FINISHED:
                /*
                 * go back to disabled state
                 */

                // enable manual control and attitude control
                _vehicle_control_mode.flag_control_manual_enabled = true;
                _vehicle_control_mode.flag_control_attitude_enabled = true;
                orb_publish(ORB_ID(vehicle_control_mode), _vehicle_control_mode_pub, &_vehicle_control_mode);

                // switch back to disabled flip state
                _flip_state = FLIP_STATE_DISABLED;
                break;

            }

            // run at roughly 100 hz
            usleep(sleeptime_us);
        }
    }
}

int FlipControl::start()
{
    ASSERT(_flip_task == -1);

    /*start the task */
    _flip_task = px4_task_spawn_cmd("flip_control",
                                    SCHED_DEFAULT,
                                    SCHED_PRIORITY_DEFAULT,
                                    2048,
                                    (px4_main_t)&FlipControl::task_main_trampoline,
                                    nullptr);

    if (_flip_task < 0) {
        warn("task start failed");
        return -errno;
    }

    return OK;

}

int flip_control_main(int argc, char *argv[])
{
    /* warn if no input argument */
    if (argc < 2) {
        warnx("usage: flip_control {start|stop|status|state}");
        return 1;
    }

    /* start flip_control manually */
    if (!strcmp(argv[1],"start")) {

        if (flip_control::g_flip != nullptr) {
            warnx("already running");
            return 1;
        }

        flip_control::g_flip = new FlipControl;

        if (flip_control::g_flip == nullptr) {
            warnx("allocation failed");
            return 1;
        }

        if (OK != flip_control::g_flip->start()) {
            delete flip_control::g_flip;
            flip_control::g_flip = nullptr;
            warnx("start failed");
            return 1;
        }

        return 0;
    }

    /* stop flip_control manually */
    if (!strcmp(argv[1], "stop")) {
        if (flip_control::g_flip == nullptr) {
            warnx("not running");
            return 1;
        }

        delete flip_control::g_flip;
        flip_control::g_flip = nullptr;
        return 0;
    }

    /* return running status of the application */
    if (!strcmp(argv[1], "status")) {
        if (flip_control::g_flip) {
            warnx("running");
            return 0;
        } else {
            warnx("not running");
            return 1;
        }
    }

    /* print current flip_state */
    if (!strcmp(argv[1], "state")) {
        flip_control::g_flip->print_state();

        return 0;
    }


    /* if argument is not in one of the if statement */
    warnx("unrecognized command");

    return 0;
}

 


2. Register flip_control app in Nuttx and Posix for SITL

The flip_control app wouldn’t get recognised by cmake without specifically asking cmake to do it. Here I will show you how to include our new app in the build process for px4fmu-v2 (pixhawk, pixfalcon) and also the software in the loop (SITL) simulation.

2.1 Nuttx px4fmu-v2

Go to “Firmware/cmake/configs” and open the file “nuttx_px4fmu_v2_default.cmake”. Scroll down until you see # Vehicle Control, add modules/flip_control to the end of the code block:

Add flip control module to the default px4fmu-v2 build

You will also need to tell Nuttx to start this application during boot. Go to “Firmware/ROMFS/px4fmu_common/init.d” and open the file “rc.mc_apps”. Scroll down until you see mc_pos_control_start and add flip_control startto the next line.:

Nuttx will start flip control app at boot once we add this line to rc.mc_apps

This setup also applies to hardware in the loop simulation (HIL), since HIL is basically using the same firmware with a different frame configuration.

2.2 Posix for SITL

The process for SITL is very similar, just two different files in two different locations. First, go to “Firmware/cmake/configs” and open the file “posix_sitl_default.cmake”. Scroll down until you see  modules/vtol_att_control , and add  modules/flip_control to the next line.

Again we need to let SITL to start flip control application during boot. Go to “Firmware/posix-configs/SITL/init/efk2”  and open the file “iris”. (if you are using a relatively older source code base you may find it in “Firmware/posix-configs/SITL/init” and the file is “rcS_jmavsim_iris”)  Scroll down until you see  mc_att_control start , and add  flip_control start to the next line.


3. Activating the flip

There are many ways one can activate the flip once in flip mode. You can use a transmitter switch, a button in your ground control station,  or your transmitter stick just to name a few. In this tutorial I will utilise the MAVLink COMMAND_LONG. We will have two new commands, one for starting the flip, one for terminating the flip.

First we need to inform PX4 that we are adding two new commands, and we start by adding the definition in the uORB topic “vehicle_command.msg”.

Two new messages are defined, 31001 for starting the flip and 31002 for terminating the flip

At this point the commander app’s command handler would have no idea what to do with these two strange commands, so let’s inform commander what is going on by adding two cases in the handle_command method in the commander.cpp:

There are many ways t0 send MAVLink commands, the easiest way is of course through ground control station. Luckily, in QGroundControl there is a nice little widget called “custom command”, you can find it in the top menu bar on macOS. The widget requires a custom .qml file to define some buttons which can be used to send custom MAVLink commands. It looks like this:

The qml file I have used in this example:

// This is an example Custom Command Qml file. You have full access to the entire Qml language
// for creating any user interface you like. From the ui you can affect the following changes
// with respect to your vehicle:
// 1) Sending COMMAND_LONG commands out over mavlink using QGCButton control
// 2) Modifying parameters
//
// When developing custom Qml file implementations. You must restart QGroundControl to pick up
// the changes. You need to do this even if you select Clear Qml file. Not sure what at the this
// point. Qt must be caching the files somewhere.

import QtQuick 2.2

import QGroundControl.Controls 1.0
import QGroundControl.FactSystem 1.0
import QGroundControl.FactControls 1.0
import QGroundControl.Controllers 1.0

FactPanel {
 id: panel
 
 property var qgcView: null 

 CustomCommandWidgetController { id: controller; factPanel: panel }

 // Your own custom changes start here - everything else above is always required

 Column {
 spacing: 20

 QGCButton {
 text: "Send command_long ID 31001"
 onClicked: controller.sendCommand(31001, 0, 0, 0, 0, 0, 0, 0, 0, 0)
 }

 QGCButton {
 text: "Send command_long ID 31002"
 onClicked: controller.sendCommand(31002, 0, 0, 0, 0, 0, 0, 0, 0, 0)
 }

 }

 // Your own custom changes end here - everything else below is always required
}

4. Testing

It is strongly recommended to test your code first in SITL and HIL. Fly the vehicle well above the ground first, switch to flip mode, and send the command, and enjoy the flip.

This simple tutorial does not have any safety consideration designed in the code, it is merely an example to get you familiar with the PX4 code base, I am not responsible for any damage you may cause if you decide  to use my code on a real vehicle.


5. Note

This works on PX4 firmware version 1.4.4stable, the newest firmware 1.5.5 broke the jMAVsim HITL, I have no idea how to fix it. In addition, the newest firmware change the vehicle_attitude topic so that it only gives quaternion. You will need to write your own quaternion to euler angle function.

By |2017-09-12T09:18:33+00:00March 15th, 2017|Categories: Flight Control, PX4, Software, Tutorials|21 Comments

About the Author:

21 Comments

  1. Lohan March 16, 2017 at 10:18 pm - Reply

    Great post!

  2. strang March 16, 2017 at 10:38 pm - Reply

    Hello. I am very newbie with PX4 and I have a question:

    After you add the new variables to the .msg file why don’t you have a problem with the Mavlink message? Don’t you have to redefine the mavlink message referring to this topic?

    • zihao March 19, 2017 at 11:59 pm - Reply

      The .msg file is just a definition for uORB topics, they don’t really have anything to do with Mavlink messages unless the topic is about Mavlink.

  3. Jose March 17, 2017 at 10:41 pm - Reply

    Why do you include vehicle_status.h?

    How is it supposed to work? I was trying to set a waypoint and then send the command so the vehicle would flip and continue to follow the point. (Sorry if that is not they way it is supposed to work).

    I followed this and the previous posts but it doesn’t work out (SITL). I send the command through Qgroundcontrol but nothing happens (I only see the message printed “Flip initiated”). I keep checking the “currentmode” but it never changes.

    Also, once I send the command (31001), the vehicle doesn’t follow the waypoints anymore, the only command it follows is RTL or land. It seems like the vehicle stops to obey Qgroundcontrol once I send the command to initiate flip.

    Thanks.

    • zihao March 20, 2017 at 12:03 am - Reply

      PX4 and QGroundControl changed quite a bit since I made this, it works on PX4 1.4.4 stable and QGroundControl pre-2.9 builds, depending on what you use you need to find out how to make it work yourself. I write this post in hope that you can get some idea from it.

      I will update this post if I can find out where things went wrong in the latest firmware.

  4. Ayesha March 18, 2017 at 8:08 am - Reply

    Hello!
    I have an urgent query regarding sending waypoints from the PX4 code. I’m trying to circle the done around a target in Follow me mode. I have calculated the waypoints for the circle. How do I tell my copter to fly to these waypoints ?

    • zihao March 19, 2017 at 11:58 pm - Reply

      You can use a companion computer to achieve this, using offboard control you can send SET_POSITION_TARGET_LOCAL_NED commands in a script.

      • Ayesha March 20, 2017 at 10:28 am - Reply

        Is it not possible to do this without a companion computer ? Like, could I set the setpoint triplet and publish it from my rotation function ?
        The requirement is to do the circling in Follow Me mode.

        • zihao March 21, 2017 at 2:57 am - Reply

          Yes it is possible to do without a companion computer, it can be done by writing an app that keeps updating local position commands.

          • Ayesha March 21, 2017 at 6:09 am

            Could you please elaborate on this?

          • Ayesha March 21, 2017 at 8:22 am

            I have made some changes in follow target module for circling the target. I have calculated the waypoints for the circle and using position setpoint triplet to use these new waypoints. However, the circling doesn’t seem to happen. Could you please have a look at these diff files ?
            https://drive.google.com/drive/folders/0B5ZIS2qkt6IBWV8zRkJJNmUyUVU

            From the prints, I see that the current copter position and waypoints are changing but the copter just shows slightest arbitrary movement in qGC.
            I’m using jmavsim SITL to test this.

  5. Prince March 21, 2017 at 2:09 pm - Reply

    I want to point out my admiration for your kind-heartedness in support of those people who really need assistance with that niche. Your special commitment to passing the message all around came to be amazingly effective and has in every case helped men and women much like me to achieve their aims. Your personal valuable help and advice denotes this much to me and somewhat more to my colleagues. Many thanks; from each one of us.

  6. Alexis March 23, 2017 at 7:37 am - Reply

    Hey, I implemented the mode switcher from the nutshell and was just doing it for QGroundcontrol and was wondering if it was possible to make a switch that directly selects the mode we are in in the commander. Does it work the same way as doing it in the cpp file of you flip mode?

    • zihao April 1, 2017 at 5:15 am - Reply

      If you mean selecting it from QGroundControl, yes that is totally possible. But you will need to learn the QGroundControl source code and find the particular line where you can introduce the new flight mode.

  7. Ayesha April 1, 2017 at 10:51 am - Reply

    Hey!
    I’m facing an issue with Follow Me mode. Would really appreciate if you could have a look and give some suggestions.
    https://github.com/PX4/Firmware/issues/6953

  8. Marcel May 16, 2017 at 7:25 am - Reply

    Amazing post! Keep up the good work.

    I’m developing code for the px4 flight stack since 2 years now, mainly developing flight control algorithms with Matlab/Simulink and integrating them with code generation. I know digging around in the px4 low level code is a lot of work.

  9. Thanakorn July 18, 2017 at 5:26 pm - Reply

    I have a question.

    For the step at which you added “flip_control start” into “Firmware/ROMFS/px4fmu_common/init.d”.
    Was it the same for all developing boards?

    For example, if I use QualComm SnapDragon board, where I should add “flip_control start” into.

  10. Wonkeun Youn August 29, 2017 at 1:29 pm - Reply

    thank you for the great information.

    Would you please share the original source code for flip_control.cpp?

    (Since I cannot past the flip_control.cpp in your webpage)

    • zihao September 8, 2017 at 1:21 am - Reply

      Hi Wonkeun

      I have fixed the inline codes, you should be able to copy them now.

      • wonkeun September 11, 2017 at 3:22 pm - Reply

        Thank you for the kind reply
        I have tried your website, and everything works except the last one.

        When I load qml file in Qgroundcontrol, and it was succesfull

        But When I click the qml button,

        Qgroundcontrol output “MAV_CMD(31001) command not supported”.

        Would you please let me know how to fix it?

        • wonkeun youn September 12, 2017 at 1:36 pm - Reply

          MAV_CMD(31001) command was successfully implemented.

          When I click the buttion in qml of Qgroundcontrol,

          And following messeages is displayed, which means everything work properly.

          warnx(“Flip initiated”);
          warnx(“Flip terminated”);

          However, quadrotor in JMAVsim does not respond to flip

Leave A Comment