Failsafes and geofences

Example 1 Checking points against inclusion and exclusion fences

The inclusion fence is a 400 × 300 m rectangle with an exclusion fence around a tall building inside. Ray casting checks whether a point is inside, and the distance to the nearest edge is computed.

import math

INCLUSION = [(0, 0), (400, 0), (400, 300), (0, 300)]            # m
EXCLUSION = [(250, 120), (320, 120), (320, 200), (250, 200)]    # tall building
MARGIN = 2.0                                                    # FENCE_MARGIN

def inside(p, poly):
    x, y, c = p[0], p[1], False
    for i in range(len(poly)):
        (x1, y1), (x2, y2) = poly[i], poly[(i + 1) % len(poly)]
        if (y1 > y) != (y2 > y) and x < x1 + (y - y1) * (x2 - x1) / (y2 - y1):
            c = not c
    return c

def dist_to_edge(p, poly):
    best = math.inf
    for i in range(len(poly)):
        (ax, ay), (bx, by) = poly[i], poly[(i + 1) % len(poly)]
        dx, dy = bx - ax, by - ay
        t = max(0.0, min(1.0, ((p[0] - ax) * dx + (p[1] - ay) * dy) / (dx * dx + dy * dy)))
        best = min(best, math.hypot(p[0] - (ax + t * dx), p[1] - (ay + t * dy)))
    return best

for p in [(100, 100), (398.5, 150), (300, 150), (410, 50)]:
    if not inside(p, INCLUSION):
        verdict = "BREACH: outside inclusion fence"
    elif inside(p, EXCLUSION):
        verdict = "BREACH: inside exclusion zone"
    elif min(dist_to_edge(p, INCLUSION), dist_to_edge(p, EXCLUSION)) < MARGIN:
        verdict = "within margin of a fence edge"
    else:
        verdict = "ok"
    print(f"{p}: {verdict}")

(100, 100): ok
(398.5, 150): within margin of a fence edge
(300, 150): BREACH: inside exclusion zone
(410, 50): BREACH: outside inclusion fence

The point only 1.5 m from the edge has not breached the fence but is within the margin, which ArduPilot uses to keep missions or guided flight from getting too close to the edge. Checking missions against fences with a program before upload catches misplaced waypoints before flight.

Example 2 Did the failsafe act on time?

The times at which the aircraft received GCS heartbeats, and the time the log recorded the mode change to RTL, from a SITL test (simulated data).

TIMEOUT = 5.0                                   # FS_GCS_TIMEOUT
TOLERANCE = 0.5                                 # s, tolerance accepted by the team
heartbeats = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9.1, 10, 16.4, 17.4, 18.4]   # s
mode_change_rtl = 15.2                          # s from the log

gaps = [(heartbeats[i], heartbeats[i + 1] - heartbeats[i]) for i in range(len(heartbeats) - 1)]
for start, gap in gaps:
    if gap > TIMEOUT:
        expected = start + TIMEOUT
        diff = mode_change_rtl - expected
        print(f"link lost after t={start}s for {gap:.1f}s -> failsafe expected at {expected:.1f}s")
        print(f"logged RTL at {mode_change_rtl}s ({diff:+.1f}s) -> {'as configured' if abs(diff) <= TOLERANCE else 'CHECK SETTINGS'}")
print(f"largest gap without failsafe: {max(g for _, g in gaps if g <= TIMEOUT):.1f}s")

link lost after t=10s for 6.4s -> failsafe expected at 15.0s
logged RTL at 15.2s (+0.2s) -> as configured
largest gap without failsafe: 1.1s

The failsafe acted only slightly after the expected time, so it worked as configured. A gap of just over 1 second did not trigger it, which is the intended behaviour. If the timing differs a lot, check whether FS_GCS_ENABLE is set correctly and whether another GCS is still sending heartbeats.