Sample application (Follow the Leader)

Follow the Leader is a sample application that makes use of the interfaces exposed by MAVProxy to attain the functionality of Follower drone(s) following a single Leader.

The communication between the follower and the leader (central horizontal arrow in the diagram below, labeled "Broadcast") should follow the //Mission Control Protocol// (MCP), described elsewhere on this wiki. This protocol was developed for the CentMesh drones by the NCSU team for communicating overall mission parameters to the BBBKs on the [wiki:HardWare/MobileNode CentMesh Mobile Nodes], and is not covered by MAVLink or other formats.

Architecture

• Beaglebone Black: The 'brain' of the UAV
• APM: Autopilot
• MAVProxy: Program that runs on the Beaglebone black and connects to the APM on startup. Multiple Ground control stations can communicate with the autopilot via MAVProxy.
• GCS: Ground Control Station that receives telemetry data from the autopilot and can send commands to be executed by the UAV. In this setup, this is optional.
• On board Flight Control System (FCS): Program that runs on each Beaglebone black and communicates with MAVProxy.The FCS acts as either the Leader or Follower (based on its role in the configuration file).

Functionality

This section explains the functionality of Leader and Follower in the form of a FSM

FSM State, Events and Transitions for Leader

STATE: LEADER_INIT

State Description: Leader reads and loads configuration options from configuration file provided during invocation (these options include the UDP port to receive messages from MAVProxy, broadcast address and UDP port to be used for sending MCP messages to Followers). It opens a UDP server socket (port specified in configuration file) for messages from MAVProxy. It waits for messages on this socket.

Event and transition:

On receiving a message from MAVProxy (of any type), it notes the address used by MAVProxy for communication with the Leader and moves to LEADER_MAVP_CONNECTED state.

STATE: LEADER_MAVP_CONNECTED

State Description: An option in the configuration file specifies whether the UAV has to be set to AUTO mode or not. If it is to be set, a ‘set AUTO mode’ command is sent to the UAV.

Event and transition:

On receiving a heartbeat message from MAVProxy, the mode is verified and the Leader transitions to LEADER_PROCESS_MSGS if the mode is auto, else the Leader transitions to LEADER_MAVP_CONNECTED state (sets the mode again and waits for heartbeat).

On receiving any other message from MAVProxy, the Leader transitions to LEADER_MAVP_CONNECTED state (sets the mode again and waits for heartbeat).

TODO: We are currently setting the mode, but not waiting for message from MAVProxy. This functionality is to be updated.

STATE: LEADER_PROCESS_MSGS

State description: Leader waits passively for messages from MAVProxy.

Event and transition:

On receiving a heartbeat message from MAVProxy, the mode is verified (the heartbeat messages contain the MODE of APM) and the message is broadcasted if the mode is ‘Return to Launch’ or ‘Land’. Leader then transitions to LEADER_PROCESS_MSGS.

On receiving a GPS_RAW_INT message from MAVProxy, the message is broadcasted encapsulated in an MCP message if the last broadcast was more than 5 seconds ago.

On receiving message of any other type, the Leader transitions to LEADER_PROCESS_MSGS (i.e. the message is ignored). Follower: (Functionality of the follower with a probable state machine depiction)

FSM State, Events and Transitions for Follower

STATE: FOLLOWER_INIT

State Description: Follower reads and loads configuration options from configuration file provided during invocation (these options include the UDP port to receive messages from MAVProxy, UDP port to receive MCP messages from Leader). It opens a UDP server socket (port specified in configuration file) for messages from MAVProxy. It waits for messages on this socket.

Event and transition:

On receiving a message from MAVProxy (of any type), it notes the address used by MAVProxy for communication with the Leader and moves to FOLLOWER_MAVP_CONNECTED state.

STATE: FOLLOWER_PROCESS_MSGS

State Description: A UDP server socket is opened (port read in FOLLOWER_INIT state) for MCP messages from Leader.

Event and transition:

On receiving a ‘GPS_RAW_INT’ message from Leader, the Follower converts the message into a ‘set waypoint’ command, encapsulates it in an MCP message, and sends the command following the FSM given here: ​http://qgroundcontrol.org/mavlink/waypoint_protocol.

On receiving a heartbeat message from Leader, a ‘set RTL mode’ command is sent to the UAV. The Follower does not verify the type of the mode present in the heartbeat as it needs to ‘Return to Launch’ in both the cases – (a) Leader Landing (b) Leader returning to launch

The Leader does not broadcast the heartbeat message until it is in one of these two modes, the response by the Follower for both is same – set the UAV to ‘RTL’ mode.

On receiving any other message from Leader (not expected), the Follower transitions to ‘FOLLOWER_PROCESS_MSGS’ state.

TODO: We may need to set the mode to ‘GUIDED’.

Code flow

Following files provide the logic for this sample program:

• Leader.py : Module that has the class definition and functions for 'Leader'
• Follower.py : : Module that has the class definition and functions for 'Follower'
• follow_the_leader.py: Main program that runs as either the Leader or Follower based on the role specified in the configuration file.

Configuration file sample

Below is a sample configuration file for a Leader. The options have relevant comments and are mostly self-explanatory:

# NOTE:
# The matches are case-insensitive ('True', 'true', 'TRUE' all work)
# Do not leave trailing spaces after a line.
# Make sure that the keys are not duplicated, for example there should not
# be another mavpr**_port keyword in the file.

#role - true for L, false for F
leader: true

# UDP server port for MAVProxy connections
mavproxy_port: 12345

# Broadcast address used by the L to broadcast messages to F's
broadcast_address: 192.168.1.255

# UDP server port for messages from L
follower_port: 24551

# This flag determines if should L be set to AUTO mode
leader_check_for_auto: false

Explanation of code flow on the Leader and Follower.

Code flow (Leader)

Code flow (Follower)

Running the program

Code base can be checked out using:

git clone https://github.ncsu.edu/rmarmav/drone_CM/

Navigate to the directory containing the sample program:

cd drone_CM/sample_prog/

The program is run using the following command (after generating the config_file with the required configuration):

./follow_the_leader.py <path_to_configuration_file>

If the configuration file is not found or is not specified, the program exits. Based on the role specified in the configuration file, the program assumes the role of a leader or follower. The following keywords are mandatory and are expected in the configuration file (absence of which makes the program exit):

• leader: <true or false>
• broadcast_address: <Broadcast address to be used by the Leader>

UDP Port can be specified for communication with MAVProxy (if it is not specified, then 12345 is assumed). UDP Port can be specified for communication with Leader (if it is not specified, then 24551 is assumed). Care should be taken to make sure it is either specified on both Leader and Follower or not specified at all.

Debugging

Standalone Leader program

To verify that the Leader is sending out broadcast messages is trivial (tcpdump can be used to view packets sent out on the broadcast address), thus some level of functionality can be verified by a standalone Leader program running on a BBBK. The same can't be said about the Follower program. Since it responds to messages from the Leader, there MUST be a program providing a functionality similar to that of the Leader. One such program that does not expect the presence of MAVProxy and thus can run on any Linux system is available here: ​https://github.ncsu.edu/rmarmav/drone_CM/tree/master/sample_prog/standby_Leader

It is run in a similar fashion:

standby_follow_the_leader.py <configuration file>

Since it is a mock program, it just works with two GPS entries and keeps broadcasting either of them in an alternate fashion.

Sending commands to APM

This debugging help focuses on 'printing' commands sent by a GCS to APM. Using Mission planner and a slightly modified version of mavproxy.py, the content of messages sent can be viewed. This is not to be used as a starting point, but only a fallback (after checking the xml files).

The function process_mavlink() in mavproxy.py gets invoked whenever commands are sent by 'slaves' connected to MAVProxy. 'msgs' in this function has the commands that are sent. This can be used this to observe the message content sent by any GCS (ex: Mission Planner). The modified mavproxy.py can be accessed here: ​https://github.ncsu.edu/rmarmav/drone_CM/tree/master/sample_prog/debug