From 1eb0d23fb9d3ed893aee31002ff7dd203b5c798a Mon Sep 17 00:00:00 2001
From: default <adithcool.5@gmail.com>
Date: Wed, 7 Jan 2026 20:24:50 +0000
Subject: [PATCH] feat: enhance ArduPilot SITL startup with improved dependency
 checks, detailed error messages, and a robust TCP port readiness wait.

---
 scripts/run_ardupilot_controller.sh | 68 ++++++++++++++++++++---------
 1 file changed, 48 insertions(+), 20 deletions(-)

diff --git a/scripts/run_ardupilot_controller.sh b/scripts/run_ardupilot_controller.sh
index 1b3c61b..9dec03f 100755
--- a/scripts/run_ardupilot_controller.sh
+++ b/scripts/run_ardupilot_controller.sh
@@ -25,7 +25,6 @@ if [ -f "$HOME/.ardupilot_env" ]; then
 fi
 
 # Activate ArduPilot venv (has empy and other dependencies)
-# This MUST be the active venv for sim_vehicle.py to work
 if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then
     source "$HOME/venv-ardupilot/bin/activate"
 fi
@@ -38,41 +37,70 @@ echo ""
 # Check dependencies
 if ! command -v sim_vehicle.py &> /dev/null; then
     echo "[ERROR] sim_vehicle.py not found"
-    echo "Add to PATH: export PATH=\$PATH:~/ardupilot/Tools/autotest"
+    echo ""
+    echo "Fix: Add ArduPilot tools to PATH:"
+    echo "  export PATH=\$PATH:~/ardupilot/Tools/autotest"
+    echo ""
+    echo "Or source the ArduPilot environment:"
+    echo "  source ~/.ardupilot_env"
     exit 1
 fi
 
-# Verify correct Python environment (ArduPilot venv should have empy)
+# Verify empy is available
 if ! python3 -c "import em" 2>/dev/null; then
-    echo "[WARN] empy not found in current Python environment"
-    echo "[INFO] Attempting to use ArduPilot venv..."
-    if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then
-        source "$HOME/venv-ardupilot/bin/activate"
-    fi
+    echo "[ERROR] empy not found"
+    echo ""
+    echo "Fix: Install empy in ArduPilot venv:"
+    echo "  source ~/venv-ardupilot/bin/activate"
+    echo "  pip install empy==3.3.4"
+    exit 1
 fi
 
-# Start SITL in background
+echo "[INFO] Environment OK"
+echo "[INFO] Python: $(which python3)"
+echo ""
+
+# Start SITL with console output
 echo "[INFO] Starting ArduPilot SITL..."
-sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy &
+echo "[INFO] This will build ArduCopter if needed (first run takes ~2 min)"
+echo ""
+
+# Run sim_vehicle in a way that shows output
+sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map &
 SITL_PID=$!
 
-# Wait for SITL to start
-sleep 5
-
-# Check if SITL started
-if ! kill -0 $SITL_PID 2>/dev/null; then
-    echo "[ERROR] SITL failed to start"
-    exit 1
-fi
-
-echo "[OK] SITL started (PID: $SITL_PID)"
+# Wait for SITL to be ready (check for TCP port 5760)
+echo "[INFO] Waiting for SITL to start (TCP 5760)..."
+TRIES=0
+MAX_TRIES=60  # 60 seconds max
+while ! nc -z 127.0.0.1 5760 2>/dev/null; do
+    sleep 1
+    TRIES=$((TRIES + 1))
+    if [ $TRIES -ge $MAX_TRIES ]; then
+        echo "[ERROR] Timeout waiting for SITL on port 5760"
+        kill $SITL_PID 2>/dev/null || true
+        exit 1
+    fi
+    # Check if SITL process is still running
+    if ! kill -0 $SITL_PID 2>/dev/null; then
+        echo "[ERROR] SITL process died"
+        exit 1
+    fi
+    echo -n "."
+done
 echo ""
+echo "[OK] SITL is ready on TCP 5760"
+echo ""
+
+# Give it a moment more to stabilize
+sleep 2
 
 # Trap to kill SITL on exit
 trap "echo ''; echo '[INFO] Stopping SITL...'; kill $SITL_PID 2>/dev/null; exit 0" INT TERM
 
 # Run controller
 echo "[INFO] Starting drone controller..."
+echo "[INFO] Press Ctrl+C to stop"
 echo ""
 cd "$PROJECT_DIR"
 python scripts/run_ardupilot.py "$@"