SirBlob/simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: Defer geofence activation until GPS lock and home position are established, and update readiness check to wait for both.

Browse Source
This commit is contained in:
default
2026-02-09 06:27:39 +00:00
parent af4ea49efd
commit d3acd8789f
1 changed files with 45 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
src/autonomous_controller.py
View File

@@ -128,7 +128,7 @@ class AutonomousController:
"""Configure for simulated GPS-denied operation.
GPS is enabled in EKF for:
1. Geofence safety
1. Geofence safety (enabled after GPS lock)
2. LOCAL_POSITION_NED telemetry
But we use GUIDED_NOGPS mode and send velocity/attitude commands,
@@ -147,48 +147,68 @@ class AutonomousController:
# Disable GPS failsafe (we're simulating GPS-denied)
self.set_param("FS_GPS_ENABLE", 0)
# Setup geofence (safety cage)
print("[CTRL] Setting up geofence: Alt=10m, Radius=20m")
self.set_param("FENCE_ENABLE", 1)
self.set_param("FENCE_TYPE", 3) # Alt + Circle
self.set_param("FENCE_ACTION", 2) # Land on breach
self.set_param("FENCE_ALT_MAX", 10)
self.set_param("FENCE_RADIUS", 20)
self.set_param("FENCE_MARGIN", 2.0)
# DISABLE fence initially - will enable after GPS lock
self.set_param("FENCE_ENABLE", 0)
print("[CTRL] Setup complete (GPS for fence, GUIDED_NOGPS for control)")
print("[CTRL] Setup complete (fence will enable after GPS lock)")
time.sleep(1)
def wait_for_ready(self, timeout=60):
"""Wait for EKF and GPS to be ready."""
print("[CTRL] Waiting for system ready...")
def wait_for_ready(self, timeout=90):
"""Wait for GPS lock and home position."""
print("[CTRL] Waiting for GPS lock and home position...")
start = time.time()
gps_ok = False
home_ok = False
while time.time() - start < timeout:
self.update_state()
msg = self.mav.recv_match(type=['STATUSTEXT', 'GPS_RAW_INT'], blocking=True, timeout=1)
# Check multiple message types
msg = self.mav.recv_match(type=['STATUSTEXT', 'GPS_RAW_INT', 'HOME_POSITION'],
blocking=True, timeout=1)
if msg:
msg_type = msg.get_type()
if msg_type == 'STATUSTEXT':
text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
text = text.strip()
if text:
print(f"[SITL] {text}")
if 'EKF3' in text and 'active' in text.lower():
print("[CTRL] EKF ready!")
time.sleep(2)
return True
# Check for origin set (means home is set)
if 'origin set' in text.lower() or 'Field Elevation' in text:
home_ok = True
print("[CTRL] Home position set!")
elif msg_type == 'GPS_RAW_INT':
if msg.fix_type >= 3:
print(f"[CTRL] GPS fix: {msg.fix_type} (satellites: {msg.satellites_visible})")
time.sleep(1)
return True
if msg.fix_type >= 3: # 3D fix
if not gps_ok:
print(f"[CTRL] GPS fix: {msg.fix_type} (satellites: {msg.satellites_visible})")
gps_ok = True
if int(time.time() - start) % 10 == 0 and int(time.time() - start) > 0:
print(f"[CTRL] Waiting... ({int(time.time() - start)}s)")
elif msg_type == 'HOME_POSITION':
home_ok = True
print("[CTRL] Home position received!")
print("[CTRL] Ready timeout - continuing anyway")
# Check if ready
if gps_ok and home_ok:
print("[CTRL] GPS and home ready!")
# NOW enable geofence since we have position
print("[CTRL] Enabling geofence: Alt=10m, Radius=20m")
self.set_param("FENCE_ENABLE", 1)
self.set_param("FENCE_TYPE", 3)
self.set_param("FENCE_ACTION", 2)
self.set_param("FENCE_ALT_MAX", 10)
self.set_param("FENCE_RADIUS", 20)
time.sleep(1)
return True
elapsed = int(time.time() - start)
if elapsed % 10 == 0 and elapsed > 0:
status = f"GPS={'OK' if gps_ok else 'waiting'}, Home={'OK' if home_ok else 'waiting'}"
print(f"[CTRL] Waiting... ({elapsed}s) {status}")
print("[CTRL] Ready timeout - continuing anyway (fence disabled)")
return True
def set_mode(self, mode_name):