How to upload a complex fence (multiple polygons)

[Janis Blank]

I'm currently trying to send a complex geofence to my ardupilot quadcopter, with "complex" meaning a fence consisting of multiple polygons of inclusion and exclusion type.

For testing purposes, I wrote a python script using pymavlink that does the following:

1. Send the MISSION_COUNT command with the number of all points as argument.
2. Repeat
   1. Keep waiting for MISSION_REQUEST_INT
      
   2. Send the next point with the sequence number found in the last MISSION_REQUEST_INT
      
   3. Break if MAV_MISSION_ACCEPTED is received.

This works perfectly when uploading a single polygon of either type, but I don't know how to upload several inclusion and exclusion polygons. The naive approach, just sending the polygon points consecutively with the last point of each one being identical to the first, didn't work. The documentation of the mission protocol didn't help either.

So how do I send the other polygons? Is there a certain list format I don't know of, or do I have to call the script once for every polygon with specific arguments?

This is the aforementioned script:

from pymavlink import mavutil

import pymavlink.dialects.v20.all as dialect

vehicle = mavutil.mavlink_connection("localhost:14550")

vehicle.wait_heartbeat()

print("Heartbeat from system (system %u component %u)" % (vehicle.target_system, vehicle.target_component))

fence_list = ([51.119594, 6.484470, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION],

[51.080141, 7.678193, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION],

[50.472869, 7.687169, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION],

[50.358485, 6.556273, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION]

)

message = dialect.MAVLink_mission_count_message(target_system=vehicle.target_system,

target_component=vehicle.target_component,

count=len(fence_list),

mission_type=dialect.MAV_MISSION_TYPE_FENCE)

# send mission count message to the vehicle

vehicle.mav.send(message)

message = vehicle.recv_match(blocking=True)

print(message)

# this loop will run until receive a valid MISSION_ACK message

while True:

# catch a message

message = vehicle.recv_match(blocking=True)

# convert this message to dictionary

message = message.to_dict()

# check if this message is MISSION_REQUEST

if message["mavpackettype"] == dialect.MAVLink_mission_request_message.msgname:

# check if this request is for mission items

if message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE:

# get the sequence number of requested mission item

seq = message["seq"]

print(message)

# create mission item int message that contains the home location (0th mission item)

coord = fence_list[seq]

lat = coord[0]

lon = coord[1]

message = dialect.MAVLink_mission_item_int_message(target_system=vehicle.target_system,

target_component=vehicle.target_component,

seq=seq,

frame=dialect.MAV_FRAME_GLOBAL,

command=coord[2],

current=0,

autocontinue=0,

param1=len(fence_list),

param2=0,

param3=0,

param4=0,

x=int(lat*1e7), # x (latitude)

y=int(lon*1e7), # y (longitude)

z=0,

mission_type=dialect.MAV_MISSION_TYPE_FENCE)

# send the mission item int message to the vehicle

vehicle.mav.send(message)

# check if this message is MISSION_ACK

elif message["mavpackettype"] == dialect.MAVLink_mission_ack_message.msgname:

# check if this acknowledgement is for mission and it is accepted

if message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE and \

message["type"] == dialect.MAV_MISSION_ACCEPTED:

# break the loop since the upload is successful

print("Mission upload successful")

break

elif message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE:

print (message)

Thanks in advance

[Janis Blank]

I managed to find it out by analyzing what what QGroundControl sends to the vehicle when creating a fence (using Wireshark)

I was essentially right with my previous script, the only difference is parameter 1 in the MAV_CMD_NAV_FENCE_POLYGON_VERTEX_xxx messages. I always set it to the number of all fence items but it has to be set to the number of items in the current fence polygon. So if you have one inclusion polygon with 4 points and one exclusion one with 5, the values for seq and param 1 when sending the points would be:


9/4
9/4
9/4
9/4
9/5
9/5
9/5
9/5
9/5

The following adjusted script takes a list of polygon lists and constructs a fence:

from pymavlink import mavutil, mavwp

import pymavlink.dialects.v20.all as dialect

vehicle = mavutil.mavlink_connection("localhost:14550")

vehicle.wait_heartbeat()

print("Heartbeat from system (system %u component %u)" % (vehicle.target_system, vehicle.target_component))

fence_items = [[[51.080141, 7.678193, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0],[50.472869, 7.687169, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0],[50.358485, 6.556273, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0],[51.119594, 6.484470, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0]], [[50.8561802, 7.333573, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0],[50.8564579, 7.3358493, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0],[50.8550949, 7.3360746, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0],[50.8554064, 7.3345064, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0]]]

class Fence():

def set_fence(self, polygons):

seq = 0

poly_index = []

# polygons = polygons[0]

for polygon in polygons:

count = len(polygon)

for point in polygon:

poly_index.append([count, point])

seq+=1

message = dialect.MAVLink_mission_count_message(target_system=vehicle.target_system,

target_component=vehicle.target_component,

count=len(poly_index),

mission_type=dialect.MAV_MISSION_TYPE_FENCE)

# send mission count message to the vehicle

vehicle.mav.send(message)

message = vehicle.recv_match(blocking=True)

print(message)

while True:

# catch a message

message = vehicle.recv_match(blocking=True)

# convert this message to dictionary

message = message.to_dict()

# check this message is MISSION_REQUEST

if message["mavpackettype"] == dialect.MAVLink_mission_request_message.msgname:

# check this request is for mission items

if message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE:

# get the sequence number of requested mission item

seq = message["seq"]

print(message)

# create mission item int message that contains the home location (0th mission item)

coord = poly_index[seq][1]

vertex_count = poly_index[seq][0]

lat = coord[0]

lon = coord[1]

message = dialect.MAVLink_mission_item_int_message(target_system=vehicle.target_system,

target_component=vehicle.target_component,

seq=seq,

frame=dialect.MAV_FRAME_GLOBAL,

command=coord[2],

current=0,

autocontinue=0,

param1=vertex_count,

param2=0,

param3=0,

param4=0,

x=int(lat*1e7), # x (latitude)

y=int(lon*1e7), # y (longitude)

z=0,

mission_type=dialect.MAV_MISSION_TYPE_FENCE)

# send the mission item int message to the vehicle

vehicle.mav.send(message)

# check this message is MISSION_ACK

elif message["mavpackettype"] == dialect.MAVLink_mission_ack_message.msgname:

# check this acknowledgement is for mission and it is accepted

if message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE and \

message["type"] == dialect.MAV_MISSION_ACCEPTED:

# break the loop since the upload is successful

print("Mission upload is successful")

break

elif message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE:

print (message)

fence = Fence()

fence.set_fence(fence_items)

[Hamish Willee]

Thanks. FYI docs updated to reflect this in https://github.com/mavlink/mavlink/pull/2222