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

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.