Uploading fence to Ardupilot by dronekit

[rasheeddo]

I am using Cube purple with ArduRover 4.0, and am using dronekit for many of my applications. But I tried to upload fence points by creating a similar mission file as QGC WPL 110 format (same like when we download from Mission Planner). On that text file, I changed the mission command value to 5002 and 5004 and also corresponding param1 and MAV_FRAME value, and I used the same method as describe here to upload the fence points, but it didn't work, and I found that it stuck at the while loop here.

After sometime of looking the code of dronekit and pymavlink and some trial and error, I found out some way to upload fence mission. So in case some one is looking for a similar thing, you can try this out.

• On Dronekit on __init__.py at line 46

https://github.com/dronekit/dronekit-python/blob/27b292b8eb32e465a3682e84bab3a56fb83bba0c/dronekit/__init__.py#L46 we need to change from pymavlink.dialects.v10 import ardupilotmega to pymavlink.dialects.v20 import ardupilotmega

from Command class, https://github.com/dronekit/dronekit-python/blob/27b292b8eb32e465a3682e84bab3a56fb83bba0c/dronekit/__init__.py#L2871

I added one extra function name as upload_fence(), which I copied from original upload()

On the new upload_fence(), instead of self._vehicle._master.waypoint_count_send(self._vehicle._wploader.count()), I changed to self._vehicle._master.fence_count_send(self._vehicle._wploader.count()). And I comment out self._vehicle._master.waypoint_clear_all_send() because I don't want it to clear all my previous waypoint mission. Then we need to create a function fence_count_send() as well on mavutil.py

• On pymavlink on mavutil.py at def waypoint_count_send(self,seq) function here I copied and made a new function from that which called def fence_count_send(self, seq), and on that new function I changed mission_count_send() to mission_fence_count_send()

on dialects/v20/ardupilotmega.py at line 18439, I copied mission_count_send(self, target_system, target_component, count, mission_type=0, force_mavlink1=False) function and made a new function called mission_fence_count_send(self, target_system, target_component, count, mission_type=1, force_mavlink1=False). Please make sure to have mission_type = 1 on the new function.

• On your script make sure you set this one os.environ["MAVLINK20"] = "1" on top

I have followed the way to download/upload waypoints from here, so on that page you will see an example of upload_mission() function, so I made a copy of that function and called upload_fence(). So the code is almost same but just the for loop the original mission is like this

for command in missionlist:
    cmds.add(command)

But for fence, you will need to add command.mission_type=1 like this

for command in missionlist:
    command.mission_type = 1
    cmds.add(command)

because the mission message will set mission_type default as 0 which is for MISSION, so we need to change it to 1 for FENCE. More detail of mission_type is here

So your fence points file should be in QGC WPL 110 format, you can simply make some dummy fence and save file from Mission Planner and take a look in there how the format looks like. Your MISSION_ITEM command should be 5000, 5002, 5003, or 5004 depends on what type of fence you need, please check detail from here and the param1 and frame values are different for each fence type, so from my observation are here

Example1. This is in case of MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION (5002)

QGC WPL 110
0   0   0   5002    4.00000000  0.00000000  0.00000000  0.00000000  35.84147800 139.52423140    0.000000    1
1   0   0   5002    4.00000000  0.00000000  0.00000000  0.00000000  35.84151770 139.52433130    0.000000    1
2   0   0   5002    4.00000000  0.00000000  0.00000000  0.00000000  35.84149100 139.52434140    0.000000    1
3   0   0   5002    4.00000000  0.00000000  0.00000000  0.00000000  35.84144430 139.52426430    0.000000    1

0 on third column is MAV_FRAME_GLOBAL or Absoulute in Mission Planner, more detail of MAV_FRAME https://mavlink.io/en/messages/common.html#MAV_FRAME 4 on fifth column is param1, and in this case it represents 4 points of polygon fence

Example2. This is in case of MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION (5004)

QGC WPL 110
0   0   3   5004    2.00000000  0.00000000  0.00000000  0.00000000  35.84141770 139.52420800    0.000000    1
1   0   3   5004    6.00000000  0.00000000  0.00000000  0.00000000  35.84131330 139.52420260    0.000000    1

3 on third column is MAV_FRAME_GLOBAL_RELATIVE_ALT 2 and 6 on fifth column are param1 of each point, and it represents a radius of circle fence

Here is my full example code

import time
import os
import json
os.environ["MAVLINK20"] = "1"
from pymavlink.dialects.v20 import common as mavlink2
from dronekit import connect, VehicleMode, Command #
from pymavlink import mavutil

vehicle = None
is_vehicle_connected = False

################################### Functions #############################################
def vehicle_connect():
    global vehicle, is_vehicle_connected

    if vehicle == None:
        try:
            # if sim.startswith('sitl'):
            # print("Connecting to Ardupilot on SITL...")
            # vehicle = connect('udp:127.0.0.1:14551', wait_ready=True)
            # else:
            print("Connecting to Ardupilot....")
            # vehicle = connect('/dev/usb_uart', wait_ready=True, baud=921600)
            vehicle = connect('/dev/ttyUSB0', wait_ready=True, baud=921600)
            # vehicle = connect('udp:127.0.0.1:14551', wait_ready=True)

        except Exception as e:
            print(e)
            print("Device might not be found...check the udevrules")
            # quit()
            time.sleep(1)

    if vehicle == None:
        is_vehicle_connected = False
        return False
    else:
        is_vehicle_connected = True
        return True

def readmission(aFileName):
    global cmds
    """
    Load a mission from a file into a list. The mission definition is in the Waypoint file
    format (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).

    This function is used by upload_mission().
    """
    print("\nReading mission from file: %s" % aFileName)
    # cmds = vehicle.commands
    missionlist=[]
    with open(aFileName) as f:
        for i, line in enumerate(f):
            if i==0:
                if not line.startswith('QGC WPL 110'):
                    raise Exception('File is not supported WP version')
            else:
                linearray=line.split('\t')
                ln_index=int(linearray[0])
                ln_currentwp=int(linearray[1])
                ln_frame=int(linearray[2])
                ln_command=int(linearray[3])
                ln_param1=float(linearray[4])
                ln_param2=float(linearray[5])
                ln_param3=float(linearray[6])
                ln_param4=float(linearray[7])
                ln_param5=float(linearray[8])
                ln_param6=float(linearray[9])
                ln_param7=float(linearray[10])
                ln_autocontinue=int(linearray[11].strip())
                cmd = Command( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
                # cmd = CommandSequence( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
                missionlist.append(cmd)

    f.close()

    return missionlist

def upload_mission_from_file():
    global cmds
    """
    Upload a mission from a file. 
    """
    path = os.getcwd()
    file_path = os.path.join(path, "MISSION2.waypoints")
    if os.path.exists(file_path):
        #Read mission from file
        missionlist = readmission(file_path)
        # print("missionlist", missionlist)
        print("\nUpload mission from a file: %s" % file_path)
        #Clear existing mission from vehicle
        print('Clear old mission...')
        # cmds = vehicle.commands
        cmds.clear()
        #Add new mission to vehicle
        # print("missionlist", missionlist)
        for command in missionlist:
            print("command", command)
            cmds.add(command)

        ## sometime there is error of Nonetype not iterable
        ## but the mission already uploaded, so just pass it
        try:
            vehicle.commands.upload()
            print('MISSION Uploaded')
        except Exception as e:
            print("Exception!!! error as -> %s, but don't care..." %e)
            print('MISSION Uploaded with some exception')
            pass

        return True
    else:
        print("ERROR missing MISSION.txt file")
        return False

def upload_fence_from_file():
    global cmds
    """
    Upload a mission from a file. 
    """
    path = os.getcwd()
    file_path = os.path.join(path, "FENCE3.waypoints")
    if os.path.exists(file_path):
        #Read mission from file
        missionlist = readmission(file_path)
        # print("missionlist", missionlist)
        print("\nUpload fence from a file: %s" % file_path)
        #Clear existing mission from vehicle
        # print('Clear old mission...')
        # cmds.clear()
        #Add new mission to vehicle
        print(missionlist)
        for command in missionlist:
            command.mission_type = 1
            print("command", command)
            cmds.add(command)

        ## sometime there is error of Nonetype not iterable
        ## but the mission already uploaded, so just pass it
        print("Start Uploading...")
        try:
            # vehicle.commands.upload()
            # cmds.upload()
            cmds.upload_fence()
            print('FENCE Uploaded')
        except Exception as e:
            print("Exception!!! error as -> %s, but don't care..." %e)
            print('FENCE Uploaded with some exception')
            pass

        return True
    else:
        print("ERROR missing FENCE.waypoints file")
        return False

############################# Initialized CUBE ##########################
print("INFO: Connecting to vehicle.")
while (not vehicle_connect()):
    pass
print("INFO: Vehicle connected.")

cmds = vehicle.commands

# upload_mission_from_file()
upload_fence_from_file()

I am not sure the proper way to do it, but this seems to work for temporary. I will need to check that other of my feature like guide mode yaw and others still working properly after I changed to pymavlink.dialects.v20. So I am looking forward for a high-level of geofence uploading on dronekit without these modification by myself :) . If some of you guys want to do a PR according to my code here, feel free!

[alexwien7]

For mavlink1 vs mavlink´2 issues have you tried the workaround:

self.vehicle = dk.connect(address,.....
  ....
 # enforce mavlink 2
 # read also https://github.com/dronekit/dronekit-python/issues/935
  time.sleep(1)
  self.vehicle._master.first_byte = True
  self.log.debug("fix dronekit to reevaluate to use mavlink 2")

[ahmedzahran12]

I tried your code but I get this error : Exception!!! error as -> 'CommandSequence' object has no attribute 'upload_fence', but don't care... FENCE Uploaded with some exception

[Casavantii]

Hi There! Right now entering a fence erases the others. How would you go about keeping them? I am looking as well, but your help would be appreciated. Thank you by the way!!

[ssancheze]

Hi, I've tried your code -thanks!- and managed to get it working maybe twice before getting the error: WARNING:autopilot:Fence upload timeout

Now it's stuck in the loop: while False in self._vehicle._wp_uploaded:

In _wp_uploaded, only the first element is true.

Any ideas on how to fix it? Thanks