PyMavlink GeoFence

Hello everyone.

I’m trying to upload a GEOFence using pymavlink however I can not manage to do it. I looked at the forums and tried the code reference but still no luck. Can anyone help me find the issue?

I tried two different codes.

from pymavlink import mavutil

mav = mavutil.mavlink_connection('tcp:localhost:8100')

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

points = ['four lat lon points']

for i in points:
    mav.mav.command_long_send(mav.target_system, mav.target_component,
                              mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0,
                              len(points), 0, 0, 0, i[0], i[1], 0)

This first one is a simple one using the MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION command from MAVLink however the SITL reply is: Got COMMAND_ACK: NAV_FENCE_POLYGON_VERTEX_INCLUSION: UNSUPPORTED

import time
from pymavlink import mavutil, mavwp

mav = mavutil.mavlink_connection('tcp:localhost:8100')

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

fenceloader = mavwp.MAVFenceLoader(mav.target_system, mav.target_component)

points = ['four lat lon points']

frame = mavutil.mavlink.MAV_FRAME_GLOBAL

for i in points:
    fenceloader.add_latlon(float(i[0]), float(i[1]))

fenceloader.target_system = mav.target_system
fenceloader.target_component = mav.target_component
fenceloader.reindex()
action = mav.param_fetch_one('FENCE_ACTION')
mav.param_set_send('FENCE_ACTION', 0, 3)
mav.param_set_send('FENCE_TOTAL', fenceloader.count(), 3)

for i in range(fenceloader.count()):
    p = fenceloader.point(i)
    print(p)
    mav.mav.send(p)
    print(mav.recv_match(type='COMMAND_ACK', blocking=True))
    mav.mav.fence_fetch_point_send(mav.target_system, mav.target_component, i)
    tstart = time.time()
    p2 = None
    while time.time() - tstart < 3:
        p2 = mav.recv_match(type='FENCE_POINT', blocking=False)
        print(p2)
        if p2 is not None:
            break
        time.sleep(0.1)
        if p2 is None:
            print("Failed to fetch point %u" % i)

    if p2 is None:
        mav.param_set_send('FENCE_ACTION', 0, 3)

    if (p.idx != p2.idx or
            abs(p.lat - p2.lat) >= 0.00003 or
            abs(p.lng - p2.lng) >= 0.00003):
        print("Failed to send fence point %u" % i)
        mav.param_set_send('FENCE_ACTION', action, 3)

mav.param_set_send('FENCE_ACTION', action, 3)

This second code is my best shot at replicating the MAVProxy implementation which is the one that is recommended in this forum.

However, this code gives me: AP: Invalid FENCE_TOTAL.

And the python console says: Failed to send fence point 0

Any help to solve this issue is appreciated.
Thank you in advance.

Here is an example code

I copied and pasted this code and python tells me:

AttributeError: module 'pymavlink' has no attribute 'mavlink_connection'

I then changed the import of pymavlink to:

from pymavlink import mavutil

And get the error:

AttributeError: type object 'MAVLink_param_value_message' has no attribute 'msgname'

What am I doing wrong ?

Now it works.

I changed dialect.MAVLink_param_value_message.msgname to "PARAM_VALUE" and dialect.MAVLink_fence_point_message.msgname to "FENCE_POINT".

Thank you so much for your code.

Do you know where I can get info or an example code of how to do the RTK/GPS Inject via pymavlink ?

Do you mean RTCM?

There is an example in the above link.