Offboard Control with Mavros (C++)

Note: This tutorial is applicable for all Kerloud UAV products.

The mavros ROS package serves as a gateway between computers running ROS, MAVLink enabled autopilots, and MAVLink enabled GCS. The installation procedures can be referred in: https://dev.px4.io/v1.9.0/en/ros/mavros_installation.html or https://github.com/mavlink/mavros/tree/master/mavros#installation.

We provide a ready-to-use workspace for customers to develop their applications in the directory: ~/catkinws_open, which consists of three packages: mavlink, mavros and off_mission. Due to the network problem in China, users might encounter interruptions while trying to access github resources or rosdep if they would like to start from scratch.

Due to continuous software upgrade, the workspace can be located in the directory alternatively: ~/src/uav_workspace/catkinws_offboard

Pixhawk Setup

To enable the offboard control mode for Pixhawk, users are advised to follow instructions in https://dev.px4.io/master/en/ros/offboard_control.html and https://dev.px4.io/master/en/companion_computer/pixhawk_companion.html. In our setting, you might set the following Pixhawk onboard parameters as below:

• For the default Kerloud mini autopilot equipped in Kerloud UAVs:

    MAV_0_CONFIG = TELEM 1
    MAV_0_MODE = Normal
    MAV_0_FORWARD = Enable
    SER_TEL1_BAUD = 57600
  
    MAV_1_CONFIG = TELEM 2
    MAV_1_MODE = Onboard
    MAV_1_FORWARD = Enable
    SER_TEL2_BAUD = 921600

How to Build

We adopt the catkin-tools for the build process, and it is much faster than the conventional catkin_make method. The details of catkin tools can be referred in https://catkin-tools.readthedocs.io.

To build the workspace, users can follow the commands below:

    cd ~/catkinws_open
    catkin init  # initialize the workspace
    catkin build -j2  # build the workspace with two threads only to avoid overloading in Jetson Nano

It can take up to 10 minutes for the build process to finish, so be patient for the first time.

To clean the workspace, use the command:

    cd ~/catkinws_open
    catkin clean

Sample Code Illustration

The sample program in the provided off_mission package performs a waypoint mission task. The waypoints are defined in the yaml file: src/ off_mission/launch/waypoints_xyzy.yaml with ENU coordinates and corresponding yaw values. The package supports SITL (software in the loop) simulation as well, so users could test their programs before real flights.

For clarity, we illustrate the main() function in sequence

(1) Declarations of subscriptions, publications and services

    //subscriptions, publications and services
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber local_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose", 10, local_pose_cb);
    ros::Subscriber rc_input_sub = nh.subscribe<mavros_msgs::RCIn>
            ("mavros/rc/in", 5, rc_input_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

We usually declare ROS subscriptions, publications and services at the beginning of a main program.

To obtain the state information or send commands, various topics or services from the mavros package can be easily utilized, and details are given in: http://wiki.ros.org/mavros. For example, to obtain the local position information for the UAV, the mavros/setpoint_position/local topic should be subscribed. Kindly note that the coordinates are defined in the ENU (East-North-Up) frame.

(2) Load the waypoint data, and check for the simulation flag

    // load yaml files
    XmlRpc::XmlRpcValue wp_list;
    private_nh.getParam("waypoints",wp_list);

    // simulation flag
    int simulation_flag = 0;
    private_nh.param("simulation_flag", simulation_flag, 0);

The waypoint data is then loaded from the parameter manager, and they are initialized as waypoint targets.

(3) Arm the UAV in SITL simulation

    if (simulation_flag == 1)
    {
        //set offboard mode, then arm the vehicle
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard mode enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
    }

In real flights, we can simply arm the drone and switch to the OFFBOARD mode to start the mission. While in SITL mode, we have to arm the drone by the service mavros/cmd/arming.

(4) Publish the current waypoint

    pose = waypoints.at(current_wpindex);
    local_pos_pub.publish(pose);

The program then publishes the current waypoint coordinate to the drone via the mavros topic. Note that the topic has to be published all the time in the OFFBOARD mode, otherwise the failsafe mechanism will be activated in the Pixhawk and then the OFFBOARD mode will be turned off.

(5) Disarm when landed

    if( arming_client.call(disarm_cmd) &&
        arm_cmd.response.success){
        ROS_INFO("Vehicle disarmed");
    }

When the last waypoint is reached and the drone lands successfully, the program will disarm the done automatically by calling the mavros/cmd/arming service.

How to Use

SITL Simulation

The SITL (Software In The Loop) simulation can be performed with PX4 firmware. We recommend users to download our maintained firmware version in https://gitee.com/cloudkernel-tech/Firmware to ensure software compatibility.

Due to software environment dependencies, the SITL simulation can only be run on a PC with Ubuntu 18.04 instead of the onboard computer. The detailed procedure for setting up the environment is given in http://cloudkernel-tech.gitee.io/kerloud_mini/en/ADVANCEDDEV.html. Users also have to mitigate the provided offmission control workspace located in ~/catkinws_open/ to the target PC.

Assuming that the firmware repo is in the directory ~/src/Firmware, the gazebo-based SITL simulation can be started with:

    cd ~/src/Firmware
    make px4_sitl_default gazebo

Then we can launch the offboard control node via:

    cd ~/catkinws_open
    source devel/setup.bash
    roslaunch launch/wpmission_sitl.launch

Real Flights

For real flights, follow the sequences below exactly to avoid unexpected behaviors:

• Mount the battery ready with a battery cable, make sure that it will not fall down during flight.

• Make sure that all switches are in their initial position and the throttle channel is lowered down.

• Power on with the battery.

• Wait for the GPS to be fixed: usually we can hear a reminding sound after 2-3 minutes, and the main LED in the pixhawk will turn green.

• Make sure that the safety pilot is standby, and there are no people surrounding the UAV.

• Prearm the UAV by pressing the safety button for 1 second, and the button will be double-blinking.

• Wait a few minutes for the onboard computer to boot, log into it remotely via a local wifi network, confirm the waypoint mission in ~/catkinws_open/src/off_mission/launch/waypoints_xyzy.yaml, and run commands below to start the offboard control modules:

    # set read/write privilege for the USB-ttl port
    sudo chmod a+rw /dev/ttyPixhawk
  
    cd ~/catkinws_open
    source devel/setup.bash
    roslaunch launch/wpmission.launch

  Tip: the computer can also be set to run the offboard mission automatically during booting. Users can refer to the startup.sh in the workspace directory as a template.

• Arm the UAV by lowering down the throttle and move the yaw stick to the right, then we will hear a long beep from the machine.

• Switch to the OFFBOARD mode with the specified stick (e.g. channel 7), then the UAV will start the waypoint mission autonomously. Cheers!