Controller Update

src/control/uav_controller.py

@@ -29,16 +29,24 @@ class UAVController(Node):
         self.declare_parameter('takeoff_altitude', 5.0)
         self.declare_parameter('position_tolerance', 0.3)
         self.declare_parameter('namespace', '/uav')
+        self.declare_parameter('auto_arm', True)
+        self.declare_parameter('auto_takeoff', True)
+        self.declare_parameter('startup_delay', 15.0)  # Wait for EKF
         
         self.takeoff_altitude = self.get_parameter('takeoff_altitude').value
         self.position_tolerance = self.get_parameter('position_tolerance').value
         ns = self.get_parameter('namespace').value
+        self.auto_arm = self.get_parameter('auto_arm').value
+        self.auto_takeoff = self.get_parameter('auto_takeoff').value
+        self.startup_delay = self.get_parameter('startup_delay').value
         
         self.state = FlightState.DISARMED
         self.mavros_state = None
         self.current_pose = None
         self.target_pose = None
         self.home_position = None
+        self.startup_complete = False
+        self.ekf_ready = False
         
         self.state_sub = self.create_subscription(
             State, f'{ns}/mavros/state', self.state_callback, 10)
@@ -72,8 +80,55 @@ class UAVController(Node):
         self.setpoint_timer = self.create_timer(0.05, self.publish_setpoint)
         self.status_timer = self.create_timer(0.5, self.publish_status)
         
+        # Delayed auto-start (wait for EKF initialization)
+        if self.auto_arm or self.auto_takeoff:
+            self.get_logger().info(f'Auto-mission enabled. Starting in {self.startup_delay}s...')
+            self.startup_timer = self.create_timer(self.startup_delay, self.auto_startup)
+        
         self.get_logger().info('UAV Controller Started - GPS-denied mode')
     
+    def auto_startup(self):
+        """Auto arm and takeoff after startup delay."""
+        # Only run once
+        if self.startup_complete:
+            return
+        self.startup_complete = True
+        self.startup_timer.cancel()
+        
+        self.get_logger().info('Auto-startup: Beginning autonomous sequence...')
+        
+        # Check if MAVROS is connected
+        if self.mavros_state is None:
+            self.get_logger().warn('MAVROS not connected yet, retrying in 5s...')
+            self.startup_timer = self.create_timer(5.0, self.auto_startup)
+            self.startup_complete = False
+            return
+        
+        # Set GUIDED mode first
+        self.get_logger().info('Auto-startup: Setting GUIDED mode...')
+        self.set_mode('GUIDED')
+        
+        # Wait a moment then arm
+        self.create_timer(2.0, self._auto_arm_callback, callback_group=None)
+    
+    def _auto_arm_callback(self):
+        """Callback to arm after mode is set."""
+        if self.auto_arm:
+            self.get_logger().info('Auto-startup: Arming...')
+            self.arm()
+        
+        # Wait then takeoff
+        if self.auto_takeoff:
+            self.create_timer(3.0, self._auto_takeoff_callback, callback_group=None)
+    
+    def _auto_takeoff_callback(self):
+        """Callback to takeoff after arming."""
+        if self.mavros_state and self.mavros_state.armed:
+            self.get_logger().info('Auto-startup: Taking off...')
+            self.takeoff()
+        else:
+            self.get_logger().warn('Auto-startup: Not armed, cannot takeoff')
+    
     def state_callback(self, msg):
         self.mavros_state = msg
         if msg.armed and self.state == FlightState.DISARMED: