Simulation Exit Fixes

src/control/uav_controller.py

@@ -6,7 +6,6 @@ import argparse
 import numpy as np
 from time import sleep, perf_counter
 from typing import Tuple
-
 from pymavlink import mavutil
 import pymavlink.dialects.v20.ardupilotmega as mavlink
 from pymavlink.dialects.v20.ardupilotmega import (
@@ -25,7 +24,6 @@ LOWEST_ALT = 3.0
 GUIDED_MODE = 4
 GUIDED_NOGPS_MODE = 20
 
-
 DEFAULT_WPNAV_SPEED = 150
 DEFAULT_WPNAV_ACCEL = 100
 DEFAULT_WPNAV_SPEED_UP = 100
@@ -35,7 +33,6 @@ DEFAULT_LOIT_SPEED = 150
 
 
 class Controller:
-
     HOLD_ALT = HOLD_ALT
     LOWEST_ALT = LOWEST_ALT
 
@@ -133,23 +130,14 @@ class Controller:
 
     def arm(self, retries: int = 30):
         self.set_mode_guided()
-
         for i in range(retries):
             print(f"[UAV] Arm attempt {i+1}/{retries}...")
-
-            # Force arm: param2=21196 bypasses pre-arm checks
-            # (equivalent to MAVProxy's "arm throttle force")
             self.conn.mav.command_long_send(
                 self.conn.target_system,
                 self.conn.target_component,
                 mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-                0,
-                1,      # param1: 1 = arm
-                21196,  # param2: force arm magic number
-                0, 0, 0, 0, 0)
+                0, 1, 21196, 0, 0, 0, 0, 0)
 
-            # Wait up to 4 seconds for armed status instead of
-            # blocking forever with motors_armed_wait()
             armed = False
             for _ in range(20):
                 self._drain_messages()
@@ -157,16 +145,12 @@ class Controller:
                     armed = True
                     break
                 time.sleep(0.2)
-
             if armed:
                 self.armed = True
                 print("[UAV] Armed")
                 return True
-
             print(f"[UAV] Arm attempt {i+1} failed, retrying...")
             time.sleep(2)
-
-        print("[UAV] FAILED to arm after all retries")
         return False
 
     def disarm(self):
@@ -264,7 +248,6 @@ class Controller:
             print(f"\r[UAV] Climbing: {self.altitude:.1f}m / {target_alt:.1f}m  ",
                   end='', flush=True)
             sleep(0.2)
-        print(f"\n[UAV] Altitude timeout at {self.altitude:.1f}m")
         return False
 
     def wait_for_gps(self, timeout: float = 120.0):
@@ -282,7 +265,6 @@ class Controller:
                 return True
             elapsed = int(time.time() - t0)
             print(f"\r[UAV] Waiting for GPS ... {elapsed}s  ", end='', flush=True)
-        print("\n[UAV] GPS timeout")
         return False
 
     def configure_speed_limits(self,
@@ -292,7 +274,6 @@ class Controller:
                               wpnav_speed_dn=DEFAULT_WPNAV_SPEED_DN,
                               wpnav_accel_z=DEFAULT_WPNAV_ACCEL_Z,
                               loit_speed=DEFAULT_LOIT_SPEED):
-
         params = {
             'WPNAV_SPEED': wpnav_speed,
             'WPNAV_ACCEL': wpnav_accel,