Offboard Control in a Companion Computer

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

The companion computer enables the possibility for users to realize more sophisticated missions including trajectory following, path planning, obstacle avoidance, etc, and this is achieved by offboard control and the customized mavros gateway.

Package Information

Customers will be invited to join our private repository in https://gitee.com/cloudkernel-tech/offboard_kerloudvtol, wherein we maintain an offboard control package. The package provides users a starting template to develop applications with supported APIs.

There are several branches in the repository:

  • dev_kerloudvtol: This is a basic branch for easy catch-up. It provides an initial view for project skeleton, and realizes a normal waypoint mission for Kerloud VTOL.

  • demo_fieldtest: This is a branch with complete configurations for a normal waypoint mission, and it can be directly deployed in real flights.

  • demo_vtol_guidance: This is a branch for advanced users with all features: position control, velocity control, message logging, etc. The branch can be tested in the provided SITL environment for further comprehension, and cannot be directly used for flight tests.

Key Functions

The following key functions are worthy of sufficient attention ( with reference to the demo_fieldtest branch):

  • updatePositionSetpoint(): The function updates the position setpoint for all flight modes (multicopter, fixed-wing and transition).

  • updateTransBehaviorStatus(): The function handles a state machine for both forward and backward transitions. It generates position and yaw setpoints for transition processes.

  • requestForwardTransition(): The function requests and trigger a forward transition from the autopilot, and it should be called in multicopter mode only.

  • requestBackwardTransition(): The function requests and trigger a backward transition from the autopilot, and it should be called in fixed-wing mode only.

  • constructLocalPosTargetForCustomizedGuidance(): The function constructs a raw position setpoint target (corresponding to the topic mavros/setpoint_raw/local) for the fixed-vector position guidance mode. The UAV will fly to the target position with a L1 guidance law, and then fix its heading after reaching the position target. The fixed-vector guidance will be disengaged when a new position target is constructed.

  • constructVelocityTargetForCustomizedGuidance(): The function constructs a raw position setpoint target (corresponding to the topic mavros/setpoint_raw/local) for the velocity guidance mode. The UAV will track requested forward speed, banking angle, local z position, and yaw setpoint. Note that the rudder control is deactivated by default.

Basic 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 Software-In-The-Loop Simulation simulation, 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

// note: only part of program is shown here for simplicity
/*subscriptions, publications and services*/
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 5, state_cb);

//subscription for vehicle VTOL state
ros::Subscriber extended_state_sub = nh.subscribe<mavros_msgs::ExtendedState>
        ("mavros/extended_state", 2, extendedstate_cb);

//subscription for local position
ros::Subscriber local_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 5, local_pose_cb);

ros::Subscriber rc_input_sub = nh.subscribe<mavros_msgs::RCIn>("mavros/rc/in", 5, rc_input_cb);

//publication for local position setpoint (ENU position)
ros::Publisher local_pos_sp_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 5);

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.

  1. Load the waypoint data

// load yaml files
XmlRpc::XmlRpcValue wp_list;
private_nh.getParam("waypoints",wp_list);
initTagetVector(wp_list);
ROS_INFO("waypoint yaml loaded!");

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

  1. 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.

  1. Handle transition status, update desired position and yaw setpoints, and also waypoint switching logic. The code provides a state machine for transition behaviors to ensure safety.

updateTransBehaviorStatus();

updatePositionSetpoint();

updateWaypointIndex();

How to Use

A template ROS workspace is provided in the companion computer under the directory ~/src/catkinws_vtol, and it consists of three packages: mavlink, mavros and offboardcontrol. The mavlink and mavros are customized as documented in Application Programming Interface (API).

Simulation Test

A typical workflow for simulation test is as followed:

  1. Start the provided SITL environment as described in Software-In-The-Loop Simulation:

cd <sitl-space directory> \
&& bash sitl_run.sh $PWD/bin/px4 none gazebo standard_vtol

  1. Launch the mavros node:

cd ~/src/catkinws_vtol \
&& source devel/setup.bash \
&& roslaunch mavros px4.launch fcu_url:=udp://:14540@localhost:14557

  1. Launch the offboard control node with simulation setting.

cd ~/src/catkinws_vtol \
&& source devel/setup.bash \
&& roslaunch off_mission off_mission.launch simulation_flag:=1

Real Flights

The real flight case is more complicated than the simulation test above, as users have to take into consideration of necessary hardware factors as well. The workflow is suggested as:

  1. Calibrate all sensors for the first flight, including accelerometer, gyroscope, magnetometer and airspeed.

  2. Make sure that the flying zone is clean and without electromagnetic interference. Check the GPS condition with QGC, and be certain that the number of available satellites is above 11 at least and the GPS quality is sufficient.

  3. Set the transmitter ready, and be certain that the return home switch is allocated.

  4. Set a wifi hotspot via a cellphone, and log into the companion computer remotely by following instructions in Powering and Programming Interface.

  5. Switch to the desired branch for the offboard control package. Edit the waypoint definition file properly for your mission. Rebuild the workspace with catkin build command if necessary.

  6. Communicate with your team members about the mission and possible malfunctions. The pilot should standby and guarantee the safety of the UAV.

  7. Launch the nodes remotely via commands:

# terminal 1: launch the mavros node
cd ~/src/catkinws_vtol \
&& source devel/setup.bash \
&& roslaunch mavros px4.launch fcu_url:=/dev/ttyPixhawk:921600

# terminal 2: launch the offboard control node
cd ~/src/catkinws_vtol \
&& source devel/setup.bash \
&& roslaunch off_mission off_mission.launch simulation_flag:=0

  1. Finally a kind reminder: be safe, and always prioritize human lives over the aircraft.

Caution

Users should always test their code rigorously before real flights!

Demo

A simulation demo is shown in the video below. The UAV first takes off and flies through several waypoints. The offboard control will activate the fixed-vector guidance mode for the 3rd waypoint. After the UAV reaches the 3rd waypoint for 3 seconds, the offboard control will activate the velocity guidance mode. Finally the offboard control is interrupted by the return home command from QGC, and the UAV flies back to its home position at the end. Note that the simulation is conducted with the demo_vtol_guidance branch of our package.