For some time now, I’ve been trying to send complex geofences programmatically via Python to a CubeOrange FC. My program was tested with versions 4.4.4 and 4.7.0-dev.
By ‘complex’, I mean geofences that consist of one inclusion zone and one or more smaller exclusion zones inside the inclusion zone.
I’m routing the traffic over mavproxy to see MAVLink responses in a console, using the command
mavproxy.py --out 127.0.0.1:14550 --master /dev/ttyACM0 --map --console
Using examples from Mavproxy’s fence module and for pymavlink, I’ve come up with the following code:
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_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]
)
class Fence():
def add_polygon(self, polygon, command):
fence_count = 0
message = dialect.MAVLink_param_request_read_message(target_system=vehicle.target_system,
target_component=vehicle.target_component,
param_id=FENCE_TOTAL,
param_index=PARAM_INDEX)
vehicle.mav.send(message)
while True:
message = vehicle.recv_match(type=dialect.MAVLink_param_value_message.msgname,
blocking=True)
# convert the message to dictionary
message = message.to_dict()
if message["param_id"] == "FENCE_TOTAL":
print("FENCE_TOTAL parameter original:", message)
fence_count = int(message["param_value"])
break
message = dialect.MAVLink_mission_count_message(target_system=vehicle.target_system,
target_component=vehicle.target_component,
count=len(polygon),
mission_type=dialect.MAV_MISSION_TYPE_MISSION)
vehicle.mav.send(message)
message = vehicle.recv_match(blocking=True)
message = message.to_dict()
print(message)
count = len(polygon)
for i in range(0, count):
coord = polygon[i]
lat = coord[0]
lon = coord[1]
m = mavutil.mavlink.MAVLink_mission_item_int_message(
vehicle.target_system,
vehicle.target_component,
0, # seq
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
command, # command
0, # current
0, # autocontinue
count, # param1,
0.0, # param2,
0.0, # param3
0.0, # param4
int(lat*1e7), # x (latitude)
int(lon*1e7), # y (longitude)
0, # z (altitude)
mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
)
vehicle.mav.send(m)
vehicle.param_set_send('FENCE_TOTAL', count, 3)
def add_polygon_mission(self, target_locations):
message = dialect.MAVLink_mission_count_message(target_system=vehicle.target_system,
target_component=vehicle.target_component,
count=len(target_locations),
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 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 = target_locations[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(target_locations),
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.add_polygon_mission(fence_list)
The method add_polygon successfully creates a geofence out of a polygon of arbitrary designation (inclusion or exclusion). However, I have no idea how to pass it multiple polygons or add a new polygon to an existing geofence. Putting inclusion and exclusions polygons into a single list results in the following response:
Got MISSION_ACK: TYPE_FENCE: ERROR
AP: Received incorrect vertex type (want=98 got=97)
AP: Fence validation failed
Does anybody have an idea how this is supposed to work? Am I even on the right track or is there a better way to upload multiple fence polygons programmatically?