Failsafe และ geofence

ตัวอย่างที่ 1 ตรวจจุดกับรั้วรวมและรั้วกัน

รั้วแบบรวมเป็นสี่เหลี่ยม 400 × 300 m มีรั้วแบบกันรอบอาคารสูงข้างใน ใช้วิธีลากเส้นนับจุดตัด (ray casting) ตรวจว่าอยู่ข้างในหรือไม่ และหาระยะถึงขอบที่ใกล้ที่สุด

import math

INCLUSION = [(0, 0), (400, 0), (400, 300), (0, 300)]            # m
EXCLUSION = [(250, 120), (320, 120), (320, 200), (250, 200)]    # อาคารสูง
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

จุดที่ห่างขอบเพียง 1.5 m ยังไม่ฝ่ารั้ว แต่อยู่ในระยะเผื่อ ซึ่ง ArduPilot ใช้ป้องกันไม่ให้ภารกิจหรือการบินแบบนำทางพาเข้าใกล้ขอบเกินไป การตรวจภารกิจกับรั้วด้วยโปรแกรมก่อนอัปโหลดช่วยจับจุดเส้นทางที่วางผิดได้ก่อนบิน

ตัวอย่างที่ 2 failsafe ทำงานตรงเวลาหรือไม่

เวลาที่อากาศยานได้รับ heartbeat จาก GCS และเวลาที่ log บันทึกการเปลี่ยนโหมดเป็น RTL จากการทดสอบใน SITL (ข้อมูลจำลอง)

TIMEOUT = 5.0                                   # FS_GCS_TIMEOUT
TOLERANCE = 0.5                                 # s ความคลาดที่ทีมยอมรับ
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 จาก 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

failsafe ทำงานห่างจากเวลาที่คาดเพียงเล็กน้อย จึงถือว่าทำงานตามค่าที่ตั้ง ช่วงที่ heartbeat หายเกิน 1 วินาทีเล็กน้อยไม่ทำให้ failsafe ทำงาน ซึ่งเป็นพฤติกรรมที่ต้องการ ถ้าเวลาต่างจากที่คาดมาก ให้ตรวจว่าตั้ง FS_GCS_ENABLE ถูกหรือไม่ และ GCS ตัวอื่นยังส่ง heartbeat อยู่หรือไม่