Everything works well inside the fence. However when I send a mavlink SET_POSITION_TARGET_GLOBAL_INT command in Guided mode that references a target WP outside of this fence, it looks like nothing happens. I suspect that the FC itself is rejecting the command, but I am not seeing a corresponding fence breach message being communicated back, or any other way in MP to indicate that the operation was unsuccessful. In the absence of that, my script waits indefinitely for the craft to reach that point, which will never happen.
It’s okay that points outside of the fence are proactively rejected. I just need to either detect the situation before I submit the waypoint, or to detect the error afterwards. I am not seeing any way to do either right now… Any ideas? Thanks in advance.
Does mission:state() change when guided mode is active? That may help indicate a failure.
(EDIT: mission:state() is a Lua call - I misunderstood your question at first, and I don’t think that there is an analogous call exposed in the Python environment. You could perhaps include an onboard Lua script in concert with your offboard Python one to catch this scenario. Otherwise, see below, which is even easier now that I recognize that you are using Python on a GCS computer.)
Otherwise, the scripting engine is powerful enough to do geometric calculations to determine whether a point falls inside a polygon. You’d have to write the function and provide the fence point list. As long as you aren’t covering 10s of miles or more, you can assume a Cartesian coordinate plane and get reasonable results.
A somewhat less elegant solution might be to check for decreasing distance to the target waypoint and/or a ground velocity vector toward it. If you aren’t making significant progress (“significant” to be defined by you), kick out of the script with a failure warning to the user.
You won’t get a fence breach warning until the Copter actually crosses the line, which shouldn’t happen at all.
Thanks @Yuri_Rage . Writing my own geo calculations in Python for detecting attempted fence breaches would not be a problem, but I was hoping to avoid that if they already existed. Looks like MP already defines CurrentState.GeoFenceDist, which does some of the necessary underlying work, but not quite 100% what I need. I suspect most of the fence/breach logic is in the autopilot code instead.
What is the prescribed way to access the fence points from Python? Is it through CurrentState.parent.fencepoints (which has them encoded as mavlink data), or is there another/simpler way?
Thanks @hendjosh . As I suspected, that is within the FC code, which makes a lot of sense. I was looking to perform these geo checks in the script that runs on the GCS instead, before I send any attempted guided WPs to the FC via MAVlink. With fence information available in Mission Planner, it looks like this is doable by simply verifying whether the target location is within the fence or outside of it. Just a bit of math, nothing terrible. (aside from Python itself )
@Yuri_Rage done. Works beautifully. Here’s my solution for a reference, if anyone is interested at some point in the future… This btw only works for polygons, not circles, but it’s good enough for my use case. Thank you much!
def newWP(lat, lng, alt):
wp = MissionPlanner.Utilities.Locationwp()
MissionPlanner.Utilities.Locationwp.lat.SetValue(wp, lat)
MissionPlanner.Utilities.Locationwp.lng.SetValue(wp, lng)
MissionPlanner.Utilities.Locationwp.alt.SetValue(wp, alt)
return wp
# Returns a list of geofence points as defined in the GCS
def getFencePoints():
count = MAV.getWPCount(MAVLink.MAV_MISSION_TYPE.FENCE)
fencepoints = []
for idx in range(count):
fencepoints.append(MAV.getWP(idx, MAVLink.MAV_MISSION_TYPE.FENCE))
return fencepoints
# Returns true if the given point is within the specified polygon
def isPointInPolygon(point, polygon):
if (len(polygon) < 3):
return True
counter = 0
p1 = polygon[0]
num = len(polygon)
for i in range(num):
p2 = polygon[(i+1) % num]
if (point.lng > min(p1.lng, p2.lng)):
if (point.lng <= max(p1.lng, p2.lng)):
if (point.lat <= max(p1.lat, p2.lat)):
if (p1.lng != p2.lng):
xinters = (point.lng-p1.lng)*(p2.lat-p1.lat)/(p2.lng-p1.lng) + p1.lat
if (p1.lat == p2.lat or point.lat <= xinters):
counter += 1
p1 = p2;
return (counter % 2 != 0)
fencepoints = getFencePoints()
if (len(fencepoints) == 0):
print "No geofence was found."
else:
print "The geofence has " + str(len(fencepoints)) + " points."
# Test the current position
cur = newWP(cs.lat, cs.lng, cs.alt)
if (isPointInPolygon(cur, fencepoints)):
print 'Current position is within the geofence.'
else:
print 'Current position is outside the geofence!'
BTW you can monitor POSITION_TARGET_BLOBAL_INT to see if your message was received. Note not all parts of that message come back yet in the same form you put them in for altitude frames. PR for that is pending review.
Nice work. It appears this only works if you assume every available fence point defines the inclusion fence, which would fail if more than one fence is defined.
@Yuri_Rage – correct. A single polygon-style inclusion fence. This is sufficient for me ATM, but of course if at some point I need a variety of fences for the same mission, I will revisit. Thanks for your help!
)