Controller Update 2

scripts/run_autonomous.sh

@@ -74,7 +74,7 @@ echo ""
 print_info "Starting Gazebo Harmonic..."
 gz sim -v4 -r $HOME/ardupilot_gazebo/worlds/$WORLD &
 GZ_PID=$!
-sleep 5
+sleep 8
 
 # Check if Gazebo started
 if ! kill -0 $GZ_PID 2>/dev/null; then
@@ -86,9 +86,21 @@ print_success "Gazebo running (PID: $GZ_PID)"
 # Step 2: Start ArduPilot SITL
 print_info "Starting ArduPilot SITL..."
 cd ~/ardupilot
-sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy -I0 &
+
+# Check if custom param file exists
+PARAM_FILE="$PROJECT_DIR/config/ardupilot_gps_denied.parm"
+if [ -f "$PARAM_FILE" ]; then
+    print_info "Loading GPS-denied parameters..."
+    sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy -I0 \
+        --add-param-file "$PARAM_FILE" &
+else
+    sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy -I0 &
+fi
 SITL_PID=$!
-sleep 10
+
+# Wait longer for SITL to fully initialize
+print_info "Waiting for SITL to initialize (this takes ~20 seconds)..."
+sleep 20
 
 # Check if SITL started
 if ! kill -0 $SITL_PID 2>/dev/null; then
@@ -101,6 +113,9 @@ print_success "ArduPilot SITL running (PID: $SITL_PID)"
 print_info "Starting Autonomous Controller..."
 print_info "Mission: $MISSION"
 
+# Wait a bit more, then start controller with retry
+sleep 5
+
 python3 "$PROJECT_DIR/src/autonomous_controller.py" --mission "$MISSION" &
 CTRL_PID=$!
 
@@ -110,7 +125,7 @@ print_info "==================================="
 print_info "  Simulation Running"
 print_info "==================================="
 print_info "The UAV will automatically:"
-print_info "  1. Wait for EKF initialization"
+print_info "  1. Wait for EKF initialization (~30-60s)"
 print_info "  2. Arm and enter GUIDED mode"
 print_info "  3. Take off to 5 meters"
 print_info "  4. Execute mission: $MISSION"
@@ -120,3 +135,4 @@ print_info "==================================="
 
 # Wait for controller to finish or user interrupt
 wait $CTRL_PID
+