Pixhawk 4 + Raspberry pi, GeoFence is not setting up in SITL

greatsum · May 20, 2022, 10:33am

Hello everyone,

I am working on a sea vessel.

The electronics we using are :

Pixhawk PIXHAWK fligt controller for helicopter, racing drone, fixed wing, airplane, glider, cars and boats
Raspberry pi 4 Buy a Raspberry Pi 4 Model B – Raspberry Pi

Our aim is to set a mission, send GeoFence points to pixhawk from raspberry pi.

We are NOT using mission planner in this Project to set up this Fencing and waypoints.

We successfully sent waypoints to the pixhawk, however in GeoFencing, we are having an issue.

from pymavlink import mavutil, mavwp

master = mavutil.mavlink_connection(device= "tcp:127.0.0.1:5762")
master.wait_heartbeat()

fence = mavwp.MAVFenceLoader(master.target_system, master.target_component)
fence.load(“fence.txt”)

for i in range(fence.count()):
    master.mav.send(fence.point(i))

master.mav.command_long_send ( master.target_system , master.target_component , mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 0 )
master.arducopter_arm( )
print ( " Waiting for the vehicle to arm " )
master.motors_armed_wait ( )
print ( ' Armed ! ' )

i am having “Armed !” message but in SITL geofence is not activated. what can be the problem?

fence.txt file

34.5314348 35.6353176
34.5319927 35.6348468
34.5317996 35.6340448
34.5306516 35.6335741
34.5292354 35.6346115
34.5279801 35.6339751
34.5294499 35.6323100
34.5332694 35.6327546
34.5343530 35.6349340
34.5325291 35.6363811
34.5314670 35.6353524

Mustafa_Gokce · May 20, 2022, 12:03pm

Take a look at it here.

github.com

ArduPilot/MAVProxy/blob/master/MAVProxy/modules/mavproxy_fence.py

"""
    MAVProxy geofence module
"""
import os, time, platform
from pymavlink import mavwp, mavutil
from MAVProxy.modules.lib import mp_util
from MAVProxy.modules.lib import mp_module
if mp_util.has_wxpython:
    from MAVProxy.modules.lib.mp_menu import *

class FenceModule(mp_module.MPModule):
    def __init__(self, mpstate):
        super(FenceModule, self).__init__(mpstate, "fence", "geo-fence management", public = True)
        self.fenceloader_by_sysid = {}
        self.last_fence_breach = 0
        self.last_fence_status = 0
        self.present = False
        self.enabled = False
        self.healthy = True
        self.add_command('fence', self.cmd_fence,

This file has been truncated. show original

github.com

ArduPilot/MAVProxy/blob/master/MAVProxy/modules/mavproxy_fence.py#L220

      
        
                try:
                    self.fenceloader.target_system = self.target_system
                    self.fenceloader.target_component = self.target_component
                    self.fenceloader.load(filename.strip('"'))
                except Exception as msg:
                    print("Unable to load %s - %s" % (filename, msg))
                    return
                print("Loaded %u geo-fence points from %s" % (self.fenceloader.count(), filename))
                self.send_fence()
            
            
def send_fence(self):
                '''send fence points from fenceloader'''
                # must disable geo-fencing when loading
                self.fenceloader.target_system = self.target_system
                self.fenceloader.target_component = self.target_component
                self.fenceloader.reindex()
                action = self.get_mav_param('FENCE_ACTION', mavutil.mavlink.FENCE_ACTION_NONE)
                self.param_set('FENCE_ACTION', mavutil.mavlink.FENCE_ACTION_NONE, 3)
                self.param_set('FENCE_TOTAL', self.fenceloader.count(), 3)
                for i in range(self.fenceloader.count()):
                    p = self.fenceloader.point(i)

send_fence function is what you’re looking for.

greatsum · May 21, 2022, 3:12pm

i also tried this but didnt work

from pymavlink import mavutil

master =  mavutil.mavlink_connection(device="tcp:127.0.0.1:5762")
master.wait_heartbeat()

poli = [(35.63604046,34.53116655), 
(35.63638925,34.53209996),
(35.63584863,34.53270078),
(35.63536032,34.53252912),
(35.63520337,34.53167081),
(35.63554344,34.53127384)]

#enable fence
master.mav.command_long_send(
            master.target_system,
            master.target_component,
            mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, 0,
            1, 0, 0, 0, 0, 0, 0)

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

master.arducopter_arm()

print("Waiting for the vehicle to arm")
master.motors_armed_wait()
print('Armed!')

Loki2020 · August 17, 2022, 9:25pm

Did you manage to solve it? I’m stuck trying to send GeoFence points via MAVLink