Code reorganization and Drone Logic Update

README.md

@@ -1,30 +1,8 @@
-# Drone Landing Simulation (GPS-Denied)
+# GPS-Denied Drone Landing Simulation
 
-Land a drone on a moving platform using only relative sensors.
+ArduPilot + ROS 2 + Gazebo (ARG) simulation for landing on a moving platform.
 
-## Quick Start
+## Quick Start (2 Terminals)
-
-### Standalone (1 Terminal)
-
-```bash
-source activate.sh
-python standalone_simulation.py --pattern circular
-```
-
-### Gazebo + ROS 2 (2 Terminals)
-
-**Terminal 1:**
-```bash
-ros2 launch gazebo/launch/drone_landing.launch.py
-```
-
-**Terminal 2:**
-```bash
-source activate.sh
-python run_gazebo.py --pattern circular
-```
-
-### ArduPilot GPS-Denied (2 Terminals)
 
 **Terminal 1 - Gazebo:**
 ```bash
@@ -36,38 +14,53 @@ python run_gazebo.py --pattern circular
 ./scripts/run_ardupilot_controller.sh
 ```
 
+## World Options
+
+```bash
+./scripts/run_ardupilot_sim.sh runway     # Default
+./scripts/run_ardupilot_sim.sh warehouse  # Indoor
+./scripts/run_ardupilot_sim.sh custom     # Custom landing pad
+./scripts/run_ardupilot_sim.sh my_world   # gazebo/worlds/my_world.sdf
+```
+
 ## Installation
 
 ```bash
 ./setup/install_ubuntu.sh
-./setup/install_ardupilot.sh  # Optional for ArduPilot
+./setup/install_ardupilot.sh
-source activate.sh
+source ~/.bashrc
 ```
 
-## Files
+## Project Structure
 
-| File | Description |
+```
-|------|-------------|
+simulation/
-| `drone_controller.py` | **Your landing algorithm (used everywhere)** |
+├── config.py
-| `standalone_simulation.py` | PyBullet simulation |
+├── src/
-| `run_gazebo.py` | Gazebo + ROS 2 interface |
+│   └── drone_controller.py   # Your algorithm
-| `run_ardupilot.py` | ArduPilot + MAVLink interface |
+├── scripts/
-| `config.py` | Configuration settings |
+│   ├── run_ardupilot_sim.sh
-| `camera_viewer.py` | Camera feed window |
+│   └── run_ardupilot_controller.sh
+├── gazebo/
+│   ├── worlds/               # Your worlds
+│   └── models/               # Your models
+├── setup/
+└── docs/
+```
 
-## Sensors (GPS-Denied)
+## 3-Phase Mission
 
-| Sensor | Data |
+| Phase | Action |
-|--------|------|
+|-------|--------|
-| IMU | Orientation, angular velocity |
+| SEARCH | Find QR code on rover |
-| Altimeter | Altitude, vertical velocity |
+| COMMAND | Send commands to rover |
-| Camera | Downward image |
+| LAND | Land on rover |
-| Landing Pad | Relative position (when visible) |
 
 ## Documentation
 
 - [Installation](docs/installation.md)
-- [Architecture](docs/architecture.md)
 - [ArduPilot Guide](docs/ardupilot.md)
-- [Gazebo Guide](docs/gazebo.md)
+- [Drone Controller](docs/drone_guide.md)
-- [Drone Logic Guide](docs/drone_guide.md)
+- [Custom Worlds](docs/gazebo_worlds.md)
+- [Blender Models](docs/blender_models.md)
+- [Architecture](docs/architecture.md)

docs/architecture.md

@@ -1,98 +1,67 @@
-# Architecture Overview
+# Architecture
 
-## Modes
+## System Overview
-
-### 1. Standalone (1 Terminal)
-
-```bash
-python standalone_simulation.py --pattern circular
-```
 
 ```
-┌────────────────────────────────────────┐
+┌─────────────────┐     ┌─────────────────┐     ┌─────────────────┐
-│      standalone_simulation.py          │
+│ Gazebo          │◄───►│ ArduPilot SITL  │◄───►│ Controller      │
-│  PyBullet + Controllers + Camera       │
+│ (Physics)       │JSON │ (Flight Ctrl)   │MAV  │ (Your Logic)    │
-└────────────────────────────────────────┘
+└─────────────────┘     └─────────────────┘     └─────────────────┘
+        │                                               │
+        ▼                                               ▼
+  ~/ardupilot_gazebo                          src/drone_controller.py
 ```
 
-### 2. Gazebo + ROS 2 (2 Terminals)
+## Terminal Layout
 
+**Terminal 1:**
 ```
-Terminal 1                    Terminal 2
+./scripts/run_ardupilot_sim.sh
-┌───────────────────┐        ┌───────────────────┐
+    │
-│ Gazebo + Bridge   │◄──────►│ run_gazebo.py     │
+    ▼
-└───────────────────┘  ROS   └───────────────────┘
+Gazebo + ArduPilot Plugin
 ```
 
-### 3. ArduPilot (2 Terminals)
+**Terminal 2:**
-
 ```
-Terminal 1                    Terminal 2
+./scripts/run_ardupilot_controller.sh
-┌───────────────────┐        ┌────────────────────────────┐
+    │
-│ Gazebo +          │◄──────►│ run_ardupilot_controller.sh│
+    ├── ArduPilot SITL (background)
-│ ArduPilot Plugin  │  JSON  │ ┌──────────────────┐       │
+    │
-└───────────────────┘        │ │ ArduPilot SITL   │       │
+    └── run_ardupilot.py
-                             │ └─────────┬────────┘       │
+            │
-                             │           │ MAVLink        │
+            └── src/drone_controller.py
-                             │ ┌─────────▼────────┐       │
-                             │ │ run_ardupilot.py │       │
-                             │ └──────────────────┘       │
-                             └────────────────────────────┘
 ```
 
 ## Data Flow
 
-### Standalone
 ```
-Controller → PyBullet → Telemetry → Controller
+Gazebo ◄─── JSON/UDP ───► SITL ◄─── MAVLink ───► Controller
-```
+  │                         │                        │
-
+  │ Physics                 │ Flight control         │ Your logic
-### Gazebo
+  │ Sensors                 │ EKF                    │ 3-phase mission
-```
+  │ Rendering               │ Stabilization          │ QR detection
-Controller → /cmd_vel → Gazebo → /odometry → Controller
+  ▼                         ▼                        ▼
-```
+ Display              Attitude/Position          Commands
-
-### ArduPilot
-```
-Gazebo ◄─── JSON ───► SITL ◄─── MAVLink ───► Controller
 ```
 
 ## Key Files
 
 | File | Purpose |
 |------|---------|
-| `drone_controller.py` | **Your landing algorithm (used in ALL modes)** |
+| `src/drone_controller.py` | 3-phase mission logic |
-| `run_ardupilot.py` | MAVLink interface for ArduPilot |
+| `scripts/run_ardupilot.py` | MAVLink interface |
-| `run_gazebo.py` | ROS 2 interface for Gazebo |
+| `src/mavlink_bridge.py` | MAVLink utilities |
-| `standalone_simulation.py` | PyBullet simulation engine |
+| `src/gazebo_bridge.py` | Gazebo ROS bridge |
-| `config.py` | Shared configuration |
+| `config.py` | Configuration |
 
-## GPS-Denied Sensors
+## 3-Phase Mission
 
-The controller receives this standardized telemetry structure in all modes:
+```
-
+┌────────┐    QR Found    ┌─────────┐    Timeout    ┌──────┐
-```python
+│ SEARCH │───────────────►│ COMMAND │──────────────►│ LAND │
-telemetry = {
+└────────┘                └─────────┘               └──────┘
-    "altimeter": {
+     │                         │                        │
-        "altitude": float,           # Meters
+     ▼                         ▼                        ▼
-        "vertical_velocity": float   # m/s (positive = up)
+  Detect QR              Send to rover           Track & descend
-    },
-    "velocity": {                    # Body or Local frame
-        "x": float,
-        "y": float,
-        "z": float
-    },
-    "imu": {
-        "orientation": {
-            "roll": float,
-            "pitch": float,
-            "yaw": float
-        }
-    },
-    "landing_pad": {                 # If visible (None otherwise)
-        "relative_x": float,
-        "relative_y": float,
-        "distance": float
-    }
-}
 ```

docs/ardupilot.md

@@ -1,38 +1,7 @@
-# ArduPilot GPS-Denied Simulation
+# ArduPilot + Gazebo Simulation
-
-Realistic flight controller simulation with your drone logic.
 
 ## Quick Start (2 Terminals)
 
-**Terminal 1 - Gazebo:**
-```bash
-./scripts/run_ardupilot_sim.sh runway
-# Options: runway, warehouse, zephyr
-```
-
-**Terminal 2 - Controller + SITL:**
-```bash
-./scripts/run_ardupilot_controller.sh
-```
-
-## How It Works
-
-The `run_ardupilot_controller.sh` script starts ArduPilot SITL in the background and connects your controller to it via MAVLink.
-
-```
-┌─────────────────┐     ┌─────────────────┐     ┌─────────────────┐
-│ Gazebo          │◄───►│ ArduPilot SITL  │◄───►│ run_ardupilot.py│
-│ (Physics)       │JSON │ (Hidden)        │MAV  │ (Your Logic)    │
-└─────────────────┘     └─────────────────┘     └─────────────────┘
-                                                        │
-                                                        ▼
-                                               drone_controller.py
-```
-
-## Manual Mode (Debugging)
-
-If you need to debug with MAVProxy console (3 Terminals):
-
 **Terminal 1:**
 ```bash
 ./scripts/run_ardupilot_sim.sh runway
@@ -40,50 +9,64 @@ If you need to debug with MAVProxy console (3 Terminals):
 
 **Terminal 2:**
 ```bash
-sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
+./scripts/run_ardupilot_controller.sh
 ```
 
-**Terminal 3:**
+## Architecture
+
+```
+┌─────────────────┐     ┌─────────────────┐     ┌─────────────────┐
+│ Gazebo          │◄───►│ ArduPilot SITL  │◄───►│ run_ardupilot.py│
+│ (Physics)       │JSON │ (Flight Ctrl)   │MAV  │ (Your Logic)    │
+└─────────────────┘     └─────────────────┘     └─────────────────┘
+```
+
+## World Options
+
 ```bash
-# Debug commands in MAVProxy:
+./scripts/run_ardupilot_sim.sh runway     # Default
+./scripts/run_ardupilot_sim.sh warehouse  # Indoor
+./scripts/run_ardupilot_sim.sh zephyr     # Fixed-wing
+```
+
+## GPS-Denied Mode
+
+The simulation runs in GPS-denied mode by default.
+
+For manual debugging with MAVProxy:
+```bash
+sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
+
+# In MAVProxy:
 param set ARMING_CHECK 0
-mode guided
+mode stabilize
 arm throttle force
 ```
 
-## Installation
+## Controller Options
 
 ```bash
-./setup/install_ardupilot.sh
+./scripts/run_ardupilot_controller.sh                 # Auto takeoff
-source ~/.bashrc
+./scripts/run_ardupilot_controller.sh --no-takeoff    # Manual
+./scripts/run_ardupilot_controller.sh -a 10           # 10m altitude
 ```
 
-## Configuration
+## Files
 
-Your `drone_controller.py` receives telemetry and returns control inputs.
+| File | Purpose |
-
+|------|---------|
-The simulation translates your inputs:
+| `scripts/run_ardupilot_sim.sh` | Gazebo + GPU detection |
-- `thrust` → Vertical velocity
+| `scripts/run_ardupilot_controller.sh` | SITL + Controller |
-- `pitch/roll` → Horizontal velocity
+| `scripts/run_ardupilot.py` | MAVLink interface |
-- `yaw` → Yaw rate
+| `src/drone_controller.py` | Your algorithm |
 
 ## Troubleshooting
 
-### "SITL failed to start"
-Check if `sim_vehicle.py` is in your PATH:
-```bash
-export PATH=$PATH:~/ardupilot/Tools/autotest
-```
-
-### Drone drift
-ArduPilot in GUIDED mode requires good position estimation. Without GPS, it relies on optical flow or visual odometry (not yet implemented in default setup). The drone might drift if relying only on IMU.
-
 ### "No JSON sensor message"
-Ensure Gazebo (Terminal 1) is running before starting the controller.
+Start Gazebo (Terminal 1) before the controller.
 
-## Visualizing Camera
+### Drone doesn't respond
+Check mode is GUIDED in MAVProxy.
 
-```bash
+### Simulation laggy
-python camera_viewer.py --topic /drone/camera
+Check GPU: `glxinfo | grep "OpenGL renderer"`
-```
-(Requires bridging the topic if using ROS 2 bridge)

docs/blender_models.md

@@ -0,0 +1,168 @@
+# Blender Models in Gazebo
+
+Import 3D models from Blender into the ARG simulation.
+
+## Workflow
+
+```
+Blender (.blend) → Export COLLADA (.dae) → Gazebo Model → World
+```
+
+## Step 1: Create Model in Blender
+
+1. Create your 3D model
+2. Apply all transforms: `Ctrl+A` → All Transforms
+3. Set origin to geometry center
+
+## Step 2: Export from Blender
+
+1. File → Export → COLLADA (.dae)
+2. Settings:
+   - Selection Only (if needed)
+   - Include Armatures: OFF
+   - Include Animations: OFF
+   - Triangulate: ON
+3. Save as `model.dae`
+
+## Step 3: Create Gazebo Model
+
+```
+gazebo/models/my_model/
+├── model.config
+├── model.sdf
+├── meshes/
+│   └── model.dae
+└── materials/
+    └── textures/
+        └── texture.png
+```
+
+### model.config
+
+```xml
+<?xml version="1.0"?>
+<model>
+  <name>My Model</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <description>Custom Blender model</description>
+</model>
+```
+
+### model.sdf
+
+```xml
+<?xml version="1.0"?>
+<sdf version="1.9">
+  <model name="my_model">
+    <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <mesh>
+            <uri>meshes/model.dae</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>meshes/model.dae</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
+```
+
+## Step 4: Add to World
+
+```xml
+<include>
+  <uri>model://my_model</uri>
+  <name>my_model_instance</name>
+  <pose>5 3 0 0 0 0</pose>
+</include>
+```
+
+## Step 5: Set Model Path
+
+```bash
+export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH
+```
+
+## Common Issues
+
+### Model Not Found
+```bash
+export GZ_SIM_RESOURCE_PATH=/full/path/to/gazebo/models:$GZ_SIM_RESOURCE_PATH
+```
+
+### Scale Wrong
+In Blender, check unit settings: Properties → Scene → Units
+
+Adjust in SDF:
+```xml
+<scale>0.01 0.01 0.01</scale>
+```
+
+### Textures Not Showing
+Put textures in `materials/textures/` and reference in DAE file.
+
+Or add material in SDF:
+```xml
+<visual name="visual">
+  <geometry>
+    <mesh><uri>meshes/model.dae</uri></mesh>
+  </geometry>
+  <material>
+    <diffuse>0.8 0.2 0.2 1</diffuse>
+  </material>
+</visual>
+```
+
+### Model Orientation Wrong
+Blender uses Z-up, Gazebo uses Z-up. Should match.
+
+If rotated, fix in Blender or use pose:
+```xml
+<pose>0 0 0 1.5708 0 0</pose>
+```
+
+## Simplified Collision
+
+For complex meshes, use simple collision:
+
+```xml
+<collision name="collision">
+  <geometry>
+    <box><size>2 2 3</size></box>
+  </geometry>
+</collision>
+<visual name="visual">
+  <geometry>
+    <mesh><uri>meshes/complex_model.dae</uri></mesh>
+  </geometry>
+</visual>
+```
+
+## Template
+
+Copy the template:
+```bash
+cp -r gazebo/models/custom_object gazebo/models/my_model
+```
+
+Then:
+1. Edit `model.config` with your name
+2. Edit `model.sdf` with your model name
+3. Put your `model.dae` in `meshes/`
+
+## Test Model
+
+```bash
+gz sim -v4 gazebo/worlds/custom_landing.sdf
+```

docs/drone_guide.md

@@ -1,73 +1,79 @@
 # DroneController Guide
 
-Implement your GPS-denied landing algorithm.
+## 3-Phase Mission
 
-## Quick Start
+```
+SEARCH ──► COMMAND ──► LAND ──► COMPLETE
+```
 
-1. Edit `drone_controller.py`
+| Phase | Action |
-2. Find `calculate_landing_maneuver()`
+|-------|--------|
-3. Implement your algorithm
+| SEARCH | Find QR code on rover |
-4. Test with any simulation mode
+| COMMAND | Send commands to rover |
+| LAND | Land on rover |
 
-## Function to Implement
+## Your Code
 
+Edit `src/drone_controller.py`:
+
+### Search Phase
+```python
+def calculate_search_maneuver(self, telemetry):
+    return (thrust, pitch, roll, yaw)
+
+def detect_qr_code(self):
+    return {'data': 'qr_content', 'position': {...}} or None
+```
+
+### Command Phase
+```python
+def generate_rover_command(self, qr_data):
+    return {'type': 'move', 'x': 0, 'y': 0}
+```
+
+### Land Phase
 ```python
 def calculate_landing_maneuver(self, telemetry, rover_telemetry):
-    # Your logic here
     return (thrust, pitch, roll, yaw)
 ```
 
-## Sensors (GPS-Denied)
+## Telemetry
 
 ```python
-# Altitude
+telemetry = {
-altitude = telemetry['altimeter']['altitude']
+    "altimeter": {"altitude": 5.0, "vertical_velocity": -0.1},
-vertical_vel = telemetry['altimeter']['vertical_velocity']
+    "velocity": {"x": 0, "y": 0, "z": 0},
-
+    "imu": {"orientation": {"roll": 0, "pitch": 0, "yaw": 0}},
-# Velocity
+    "landing_pad": {"relative_x": 0.5, "relative_y": -0.2, "distance": 5.0}
-vel_x = telemetry['velocity']['x']
+}
-vel_y = telemetry['velocity']['y']
-
-# Landing Pad (may be None!)
-landing_pad = telemetry.get('landing_pad')
-if landing_pad:
-    relative_x = landing_pad['relative_x']
-    relative_y = landing_pad['relative_y']
 ```
 
 ## Control Output
 
 | Value | Range | Effect |
 |-------|-------|--------|
-| thrust | ±1.0 | Up/down (positive = up) |
+| thrust | ±1.0 | Vertical velocity |
 | pitch | ±0.5 | Forward/back |
 | roll | ±0.5 | Left/right |
 | yaw | ±0.5 | Rotation |
 
-Note: In ArduPilot mode, these are scaled to velocities:
+## Configuration
-- Thrust → Z velocity
-- Pitch/Roll → X/Y velocity
 
-## Example Algorithm (PD Control)
+Edit `config.py`:
 
 ```python
-def calculate_landing_maneuver(self, telemetry, rover_telemetry):
+CONTROLLER = {
-    alt = telemetry.get('altimeter', {})
+    "Kp_z": 0.5,
-    altitude = alt.get('altitude', 5.0)
+    "Kd_z": 0.3,
-    vert_vel = alt.get('vertical_velocity', 0.0)
+    "Kp_xy": 0.3,
-    
+    "Kd_xy": 0.2,
-    # Altitude PD control
+    "rate": 50,
-    thrust = 0.5 * (target_alt - altitude) - 0.3 * vert_vel
+}
-    
+```
-    # Horizontal control
+
-    pad = telemetry.get('landing_pad')
+## Testing
-    if pad:
+
-        pitch = 0.3 * pad['relative_x']
+```bash
-        roll = 0.3 * pad['relative_y']
+./scripts/run_ardupilot_sim.sh runway
-    else:
+./scripts/run_ardupilot_controller.sh
-        # Hover
-        pitch = 0
-        roll = 0
-    
-    return (thrust, pitch, roll, 0.0)
 ```

docs/gazebo.md

@@ -1,66 +0,0 @@
-# Gazebo Simulation Guide
-
-## Quick Start (2 Terminals)
-
-**Terminal 1:**
-```bash
-ros2 launch gazebo/launch/drone_landing.launch.py
-```
-
-**Terminal 2:**
-```bash
-source activate.sh
-python run_gazebo.py --pattern circular
-```
-
-## Camera Feed
-
-```bash
-python camera_viewer.py --topic /drone/camera
-```
-
-## Options
-
-```bash
---pattern   stationary, linear, circular, square, random
---speed     Rover speed in m/s (default: 0.5)
---no-rover  Disable rover movement
-```
-
-## Sensors
-
-| Sensor | Source |
-|--------|--------|
-| IMU | Gazebo odometry |
-| Altimeter | Z position |
-| Camera | Camera sensor |
-| Landing Pad | Relative position |
-
-## ROS 2 Topics
-
-| Topic | Direction |
-|-------|-----------|
-| `/cmd_vel` | Your commands → Drone |
-| `/drone/telemetry` | Sensors → You |
-| `/drone/camera` | Camera images |
-
-## Headless Mode (WSL2)
-
-```bash
-ign gazebo -s gazebo/worlds/drone_landing.sdf
-```
-
-## Troubleshooting
-
-**Drone falls:**
-Check `run_gazebo.py` is running
-
-**No camera:**
-```bash
-python camera_viewer.py --list  # Find topics
-```
-
-**Model not found:**
-```bash
-export GZ_SIM_RESOURCE_PATH=$PWD/gazebo/models:$GZ_SIM_RESOURCE_PATH
-```

docs/gazebo_worlds.md

@@ -0,0 +1,167 @@
+# Custom Gazebo Worlds
+
+Create custom environments for the ARG simulation.
+
+## Quick Start
+
+1. Copy template world:
+```bash
+cp gazebo/worlds/custom_landing.sdf gazebo/worlds/my_world.sdf
+```
+
+2. Edit the world file
+
+3. Run:
+```bash
+./scripts/run_ardupilot_sim.sh gazebo/worlds/my_world.sdf
+```
+
+## World Structure
+
+```xml
+<?xml version="1.0" ?>
+<sdf version="1.9">
+  <world name="my_world">
+    
+    <!-- Required plugins -->
+    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
+    <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors"/>
+    <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>
+    <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/>
+    <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
+    
+    <!-- Physics -->
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    
+    <!-- Lighting -->
+    <light type="directional" name="sun">
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+    </light>
+    
+    <!-- Ground -->
+    <model name="ground">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry><plane><normal>0 0 1</normal></plane></geometry>
+        </collision>
+        <visual name="visual">
+          <geometry><plane><normal>0 0 1</normal><size>100 100</size></plane></geometry>
+        </visual>
+      </link>
+    </model>
+    
+    <!-- Your models here -->
+    
+    <!-- ArduPilot drone (required) -->
+    <include>
+      <uri>model://iris_with_ardupilot</uri>
+      <name>iris</name>
+      <pose>0 0 0.195 0 0 0</pose>
+    </include>
+    
+  </world>
+</sdf>
+```
+
+## Adding Objects
+
+### Basic Shapes
+
+```xml
+<model name="box">
+  <static>true</static>
+  <pose>5 0 0.5 0 0 0</pose>
+  <link name="link">
+    <collision name="collision">
+      <geometry><box><size>1 1 1</size></box></geometry>
+    </collision>
+    <visual name="visual">
+      <geometry><box><size>1 1 1</size></box></geometry>
+      <material>
+        <ambient>0.8 0.2 0.2 1</ambient>
+      </material>
+    </visual>
+  </link>
+</model>
+```
+
+### Cylinder
+
+```xml
+<model name="cylinder">
+  <static>true</static>
+  <pose>0 5 1 0 0 0</pose>
+  <link name="link">
+    <collision name="collision">
+      <geometry><cylinder><radius>0.5</radius><length>2</length></cylinder></geometry>
+    </collision>
+    <visual name="visual">
+      <geometry><cylinder><radius>0.5</radius><length>2</length></cylinder></geometry>
+    </visual>
+  </link>
+</model>
+```
+
+### Include Model
+
+```xml
+<include>
+  <uri>model://my_custom_model</uri>
+  <name>obstacle_1</name>
+  <pose>3 4 0 0 0 0.5</pose>
+</include>
+```
+
+## Landing Pad with Rover
+
+```xml
+<model name="landing_pad">
+  <static>false</static>
+  <pose>0 0 0.05 0 0 0</pose>
+  <link name="base">
+    <inertial><mass>10</mass></inertial>
+    <collision name="collision">
+      <geometry><cylinder><radius>0.75</radius><length>0.1</length></cylinder></geometry>
+    </collision>
+    <visual name="visual">
+      <geometry><cylinder><radius>0.75</radius><length>0.1</length></cylinder></geometry>
+      <material><diffuse>0.2 0.8 0.2 1</diffuse></material>
+    </visual>
+  </link>
+  <plugin filename="gz-sim-velocity-control-system" name="gz::sim::systems::VelocityControl">
+    <topic>/landing_pad/cmd_vel</topic>
+  </plugin>
+</model>
+```
+
+## Camera Sensor
+
+Add to drone or create camera model:
+
+```xml
+<sensor name="camera" type="camera">
+  <pose>0 0 -0.1 0 1.5708 0</pose>
+  <always_on>true</always_on>
+  <update_rate>30</update_rate>
+  <camera>
+    <horizontal_fov>1.047</horizontal_fov>
+    <image>
+      <width>640</width>
+      <height>480</height>
+    </image>
+  </camera>
+  <topic>/drone/camera</topic>
+</sensor>
+```
+
+## Tips
+
+- Use `<static>true</static>` for non-moving objects
+- Pose format: `x y z roll pitch yaw`
+- Angles are in radians
+- Colors are RGBA (0-1 range)

docs/installation.md

@@ -1,110 +1,62 @@
-# Installation Guide
+# Installation
 
 ## Quick Install
 
 ```bash
 ./setup/install_ubuntu.sh
-source activate.sh
-python standalone_simulation.py
-```
-
-## Scripts
-
-| Platform | Command |
-|----------|---------|
-| Ubuntu/Debian | `./setup/install_ubuntu.sh` |
-| ArduPilot SITL | `./setup/install_ardupilot.sh` |
-| macOS | `./setup/install_macos.sh` |
-| Windows | `.\setup\install_windows.ps1` |
-
-## Platform Support
-
-| Mode | Ubuntu | macOS | Windows |
-|------|--------|-------|---------|
-| Standalone | ✅ | ✅ | ✅ |
-| Gazebo | ✅ | ❌ | WSL2 |
-| ArduPilot | ✅ | ❌ | WSL2 |
-
----
-
-## Ubuntu/Debian
-
-```bash
-./setup/install_ubuntu.sh
-source activate.sh
-```
-
-Installs: ROS 2, Gazebo, PyBullet, OpenCV, pymavlink
-
----
-
-## ArduPilot SITL
-
-```bash
 ./setup/install_ardupilot.sh
 source ~/.bashrc
 ```
 
-Installs: ArduPilot SITL, ardupilot_gazebo, MAVProxy
+## What Gets Installed
 
-**Run:**
+| Component | Location |
-```bash
+|-----------|----------|
-./scripts/run_ardupilot_sim.sh camera
+| ArduPilot SITL | `~/ardupilot` |
-```
+| ardupilot_gazebo | `~/ardupilot_gazebo` |
+| Gazebo Harmonic | System |
+| ROS 2 | System |
+| MAVProxy | `~/.local/bin` |
 
----
+## Dependencies
-
-## GPU Support
-
-The simulation auto-detects GPU:
-
-| Priority | GPU Type | Notes |
-|----------|----------|-------|
-| 1 | NVIDIA | Best performance |
-| 2 | Intel integrated | Good for laptops |
-| 3 | AMD | Good performance |
-| 4 | Software (llvmpipe) | Slow fallback |
-
-Check your GPU:
-```bash
-glxinfo | grep "OpenGL renderer"
-```
-
----
-
-## Manual Install
 
 ```bash
-python3 -m venv venv
-source venv/bin/activate
 pip install -r requirements.txt
-python standalone_simulation.py
 ```
 
----
+- pybullet
+- numpy
+- pillow
+- opencv-python
+- pymavlink
+- pexpect
+
+## Verify Installation
+
+```bash
+sim_vehicle.py --help
+gz sim --help
+```
 
 ## Troubleshooting
 
-### Simulation is laggy
-
-```bash
-# Check GPU (should NOT show "llvmpipe")
-glxinfo | grep "OpenGL renderer"
-
-# Install GPU drivers
-sudo apt install mesa-utils  # Intel/AMD
-sudo apt install nvidia-driver-535  # NVIDIA
-```
-
-### MAVProxy not found
-
-```bash
-pip3 install --user mavproxy
-export PATH=$PATH:~/.local/bin
-```
-
 ### sim_vehicle.py not found
-
 ```bash
 export PATH=$PATH:~/ardupilot/Tools/autotest
 ```
+
+### mavproxy.py not found
+```bash
+export PATH=$PATH:~/.local/bin
+```
+
+### pexpect error
+```bash
+pip install pexpect
+```
+
+### Gazebo slow
+```bash
+glxinfo | grep "OpenGL renderer"
+```
+Should show GPU, not "llvmpipe".

docs/protocol.md

@@ -1,71 +0,0 @@
-# Communication Protocol
-
-Message formats for drone operation.
-
-## Commands
-
-```json
-{
-    "thrust": 0.5,
-    "pitch": 0.1,
-    "roll": -0.2,
-    "yaw": 0.0
-}
-```
-
-| Field | Range | Effect |
-|-------|-------|--------|
-| thrust | ±1.0 | Up/down |
-| pitch | ±0.5 | Forward/back |
-| roll | ±0.5 | Left/right |
-| yaw | ±0.5 | Rotation |
-
-## Telemetry
-
-```json
-{
-    "imu": {
-        "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0},
-        "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}
-    },
-    "altimeter": {
-        "altitude": 5.0,
-        "vertical_velocity": -0.1
-    },
-    "velocity": {"x": 0.0, "y": 0.0, "z": -0.1},
-    "landing_pad": {
-        "relative_x": 0.5,
-        "relative_y": -0.2,
-        "distance": 4.5,
-        "confidence": 0.85
-    },
-    "camera": {
-        "width": 320,
-        "height": 240,
-        "image": "<base64 JPEG>"
-    }
-}
-```
-
-## Sensors
-
-| Sensor | Fields |
-|--------|--------|
-| IMU | orientation (roll, pitch, yaw), angular_velocity |
-| Altimeter | altitude, vertical_velocity |
-| Velocity | x, y, z (m/s) |
-| Landing Pad | relative_x, relative_y, distance, confidence |
-| Camera | Base64 JPEG image |
-
-## Decoding Camera
-
-```python
-import base64
-import cv2
-import numpy as np
-
-image_b64 = telemetry['camera']['image']
-image_bytes = base64.b64decode(image_b64)
-nparr = np.frombuffer(image_bytes, np.uint8)
-image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
-```

docs/pybullet.md

@@ -1,68 +0,0 @@
-# PyBullet Simulation Guide
-
-## Standalone Mode (1 Terminal)
-
-No ROS 2 required. Works on Windows, macOS, Linux:
-
-```bash
-source activate.sh
-python standalone_simulation.py --pattern circular
-```
-
-## ROS 2 Mode (2 Terminals)
-
-**Terminal 1 - Simulator:**
-```bash
-python simulation_host.py
-```
-
-**Terminal 2 - Controllers:**
-```bash
-python run_bridge.py --pattern circular
-```
-
-## Options
-
-```bash
---pattern, -p   stationary, linear, circular, square, random
---speed, -s     Rover speed in m/s (default: 0.5)
---amplitude, -a Movement radius (default: 2.0)
-```
-
-## Remote Setup
-
-**Machine 1:** `python simulation_host.py`
-**Machine 2:** `python run_bridge.py --host <IP>`
-
-## Sensors
-
-| Sensor | Description |
-|--------|-------------|
-| IMU | Orientation, angular velocity |
-| Altimeter | Altitude, vertical velocity |
-| Velocity | Estimated velocity (x, y, z) |
-| Camera | 320x240 downward JPEG |
-| Landing Pad | Relative position |
-
-## Configuration
-
-Edit `config.py`:
-
-```python
-CONTROLLER = {
-    "Kp_z": 0.5,
-    "Kd_z": 0.3,
-    "Kp_xy": 0.3,
-    "Kd_xy": 0.2,
-}
-```
-
-## Troubleshooting
-
-**"Cannot connect to X server":**
-```bash
-xvfb-run python standalone_simulation.py
-```
-
-**Drone flies erratically:**
-Reduce gains in `config.py`

docs/rover_controller.md

@@ -1,61 +0,0 @@
-# Rover Controller
-
-The RoverController creates a moving landing pad target.
-
-## Usage
-
-```bash
-# Stationary (default)
-python standalone_simulation.py --pattern stationary
-
-# Moving
-python standalone_simulation.py --pattern circular --speed 0.3
-```
-
-## Options
-
-| Option | Default | Description |
-|--------|---------|-------------|
-| `--pattern, -p` | stationary | Movement pattern |
-| `--speed, -s` | 0.5 | Speed in m/s |
-| `--amplitude, -a` | 2.0 | Radius in meters |
-
-## Patterns
-
-| Pattern | Description |
-|---------|-------------|
-| stationary | Stays at origin |
-| linear | Oscillates along X-axis |
-| circular | Circular path |
-| square | Square with sharp turns |
-| random | Random positions |
-
-## Difficulty Levels
-
-| Level | Pattern | Speed |
-|-------|---------|-------|
-| Beginner | stationary | 0.0 |
-| Easy | linear | 0.2 |
-| Medium | circular | 0.3 |
-| Hard | random | 0.3 |
-| Expert | square | 0.5 |
-
-## Progressive Testing
-
-```bash
-# 1. Static target
-python standalone_simulation.py --pattern stationary
-
-# 2. Slow circular
-python standalone_simulation.py --pattern circular --speed 0.2
-
-# 3. Faster circular
-python standalone_simulation.py --pattern circular --speed 0.4
-
-# 4. Random
-python standalone_simulation.py --pattern random --speed 0.3
-```
-
-## Note
-
-The drone cannot access rover position directly (GPS-denied). It must detect the landing pad visually via the camera.

gazebo/models/custom_object/model.config

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<model>
+  <name>Custom Object</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Your Name</name>
+  </author>
+  <description>Template for custom Gazebo model</description>
+</model>

gazebo/models/custom_object/model.sdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<sdf version="1.9">
+  <model name="custom_object">
+    <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <mesh>
+            <uri>meshes/model.dae</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <mesh>
+            <uri>meshes/model.dae</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>

gazebo/worlds/custom_landing.sdf

@@ -0,0 +1,126 @@
+<?xml version="1.0" ?>
+<sdf version="1.9">
+  <world name="custom_landing">
+    
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    
+    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics"/>
+    <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"/>
+    <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"/>
+    <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
+    
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.8 0.9 1</background>
+      <shadows>true</shadows>
+    </scene>
+    
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.3 0.3 0.3 1</ambient>
+            <diffuse>0.5 0.5 0.5 1</diffuse>
+          </material>
+        </visual>
+      </link>
+    </model>
+    
+    <model name="landing_pad">
+      <static>false</static>
+      <pose>0 0 0.05 0 0 0</pose>
+      <link name="base">
+        <inertial>
+          <mass>10</mass>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <cylinder>
+              <radius>0.75</radius>
+              <length>0.1</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <visual name="pad">
+          <geometry>
+            <cylinder>
+              <radius>0.75</radius>
+              <length>0.1</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.1 0.6 0.1 1</ambient>
+            <diffuse>0.2 0.8 0.2 1</diffuse>
+          </material>
+        </visual>
+        <visual name="h_bar">
+          <pose>0 0 0.06 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.1 0.5 0.02</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 1 1 1</ambient>
+            <diffuse>1 1 1 1</diffuse>
+          </material>
+        </visual>
+        <visual name="h_cross">
+          <pose>0 0 0.06 0 0 1.5708</pose>
+          <geometry>
+            <box>
+              <size>0.1 0.25 0.02</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 1 1 1</ambient>
+            <diffuse>1 1 1 1</diffuse>
+          </material>
+        </visual>
+      </link>
+      <plugin filename="gz-sim-velocity-control-system" name="gz::sim::systems::VelocityControl">
+        <topic>/landing_pad/cmd_vel</topic>
+      </plugin>
+      <plugin filename="gz-sim-odometry-publisher-system" name="gz::sim::systems::OdometryPublisher">
+        <odom_topic>/landing_pad/odom</odom_topic>
+      </plugin>
+    </model>
+    
+    <include>
+      <uri>model://iris_with_ardupilot</uri>
+      <name>iris</name>
+      <pose>0 0 0.195 0 0 0</pose>
+    </include>
+    
+  </world>
+</sdf>

requirements.txt

@@ -1,16 +1,7 @@
-# Drone Simulation Requirements
-# Install with: pip install -r requirements.txt
-
-# Core simulation
 pybullet>=3.2.5
 numpy>=1.20.0
 pillow>=9.0.0
-
-# Camera viewer
 opencv-python>=4.5.0
-
-# ArduPilot/MAVLink (optional - for ArduPilot mode)
 pymavlink>=2.4.0
-
+pexpect>=4.8.0
-# Build executables (optional)
 pyinstaller>=6.0.0

scripts/run_ardupilot.py

@@ -15,8 +15,11 @@ This script:
 3. Runs your drone_controller.calculate_landing_maneuver()
 4. Sends velocity commands to ArduPilot
 """
-
 import sys
+import os
+
+sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
 import time
 import math
 import argparse
@@ -29,7 +32,7 @@ except ImportError:
     sys.exit(1)
 
 from config import ARDUPILOT, CONTROLLER, MAVLINK
-from drone_controller import DroneController
+from src.drone_controller import DroneController
 
 
 class ArduPilotInterface:

scripts/run_ardupilot_sim.sh

@@ -113,7 +113,6 @@ fi
 # =============================================================================
 WORLD_ARG="${1:-runway}"
 
-# Map friendly names to actual world files
 case "$WORLD_ARG" in
     runway|iris|default)
         WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf"
@@ -130,10 +129,16 @@ case "$WORLD_ARG" in
     parachute)
         WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_parachute.sdf"
         ;;
+    custom)
+        WORLD="${PROJECT_DIR}/gazebo/worlds/custom_landing.sdf"
+        ;;
     *)
-        # Try as full path or filename
         if [ -f "$WORLD_ARG" ]; then
             WORLD="$WORLD_ARG"
+        elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}" ]; then
+            WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}"
+        elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf" ]; then
+            WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf"
         elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}" ]; then
             WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}"
         elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf" ]; then
@@ -147,12 +152,13 @@ esac
 if [ -z "$WORLD" ] || [ ! -f "$WORLD" ]; then
     echo "[ERROR] World not found: $WORLD_ARG"
     echo ""
-    echo "Available options:"
+    echo "Built-in worlds:"
-    echo "  runway     - Iris drone on runway (default)"
+    echo "  runway     - Iris on runway (default)"
     echo "  warehouse  - Iris in warehouse"
-    echo "  gimbal     - Gimbal test"
+    echo "  custom     - Custom landing pad"
-    echo "  zephyr     - Zephyr plane"
+    echo ""
-    echo "  parachute  - Parachute test"
+    echo "Local worlds in gazebo/worlds/:"
+    ls -1 "${PROJECT_DIR}/gazebo/worlds/"*.sdf 2>/dev/null | xargs -n1 basename || echo "  (none)"
     echo ""
     echo "Or specify full path to .sdf file"
     exit 1

setup/install_ardupilot.sh

@@ -131,8 +131,8 @@ else
     echo "[OK] Environment already configured"
 fi
 
-# Install MAVProxy
+# Install MAVProxy and Python dependencies
-pip3 install --user pymavlink mavproxy
+pip3 install --user pymavlink mavproxy pexpect
 
 # -----------------------------------------------------------------------------
 # Verification

setup/install_ubuntu.sh

@@ -151,7 +151,7 @@ pip install --upgrade pip
 if [ -f "$PROJECT_ROOT/requirements.txt" ]; then
     pip install -r "$PROJECT_ROOT/requirements.txt"
 else
-    pip install pybullet numpy pillow opencv-python pymavlink
+    pip install pybullet numpy pillow opencv-python pymavlink pexpect
 fi
 
 echo "[INFO] Python packages installed"

src/drone_controller.py

@@ -1,27 +1,26 @@
 #!/usr/bin/env python3
 """
-DroneController - Template for GPS-denied landing logic.
+DroneController - GPS-denied landing with 3-phase state machine.
 
-Supports multiple telemetry sources:
+Phases:
-  1. Custom JSON telemetry (/drone/telemetry) - PyBullet/Gazebo bridge
+    1. SEARCH - Find QR code on rover using camera
-  2. ArduPilot ROS 2 DDS topics (/ap/*) - Official ArduPilot integration
+    2. COMMAND - Send commands to rover
-
+    3. LAND - Land on the rover
-Implement your algorithm in calculate_landing_maneuver().
-Uses sensors: IMU, altimeter, camera, and landing pad detection.
 """
 
 import json
 import math
+import base64
+from enum import Enum, auto
 from typing import Dict, Any, Optional
 
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
 from geometry_msgs.msg import Twist, PoseStamped, TwistStamped
-from sensor_msgs.msg import Imu, BatteryState, NavSatFix
+from sensor_msgs.msg import Imu, BatteryState, NavSatFix, Image
 from std_msgs.msg import String
 
-# Load configuration
 try:
     from config import CONTROLLER, DRONE, LANDING
     CONFIG_LOADED = True
@@ -31,22 +30,32 @@ except ImportError:
     DRONE = {"max_thrust": 1.0, "max_pitch": 0.5, "max_roll": 0.5}
     LANDING = {"height_threshold": 0.1, "success_velocity": 0.1}
 
+try:
+    import cv2
+    import numpy as np
+    CV2_AVAILABLE = True
+except ImportError:
+    CV2_AVAILABLE = False
+
+
+class Phase(Enum):
+    SEARCH = auto()
+    COMMAND = auto()
+    LAND = auto()
+    COMPLETE = auto()
+
 
 def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple:
-    """Convert quaternion to Euler angles (roll, pitch, yaw)."""
-    # Roll (x-axis rotation)
     sinr_cosp = 2.0 * (w * x + y * z)
     cosr_cosp = 1.0 - 2.0 * (x * x + y * y)
     roll = math.atan2(sinr_cosp, cosr_cosp)
     
-    # Pitch (y-axis rotation)
     sinp = 2.0 * (w * y - z * x)
     if abs(sinp) >= 1:
         pitch = math.copysign(math.pi / 2, sinp)
     else:
         pitch = math.asin(sinp)
     
-    # Yaw (z-axis rotation)
     siny_cosp = 2.0 * (w * z + x * y)
     cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
     yaw = math.atan2(siny_cosp, cosy_cosp)
@@ -55,18 +64,9 @@ def quaternion_to_euler(x: float, y: float, z: float, w: float) -> tuple:
 
 
 class DroneController(Node):
-    """
-    Drone controller for GPS-denied landing.
-    
-    Supports multiple telemetry sources:
-      - Legacy: /drone/telemetry (JSON String)
-      - ArduPilot DDS: /ap/pose/filtered, /ap/imu/filtered, etc.
-    """
-
     def __init__(self, use_ardupilot_topics: bool = True):
         super().__init__('drone_controller')
         
-        # Load from config
         self._control_rate = CONTROLLER.get("rate", 50.0)
         self._Kp_z = CONTROLLER.get("Kp_z", 0.5)
         self._Kd_z = CONTROLLER.get("Kd_z", 0.3)
@@ -78,29 +78,37 @@ class DroneController(Node):
         self._landing_height = LANDING.get("height_threshold", 0.1)
         self._landing_velocity = LANDING.get("success_velocity", 0.1)
         
-        # Mode selection
         self._use_ardupilot = use_ardupilot_topics
         
-        self.get_logger().info('=' * 50)
+        self._phase = Phase.SEARCH
-        self.get_logger().info('Drone Controller Starting (GPS-Denied)...')
+        self._phase_start_time = self.get_clock().now()
-        if CONFIG_LOADED:
+        
-            self.get_logger().info('  Configuration loaded from config.py')
+        self._qr_detected = False
-        self.get_logger().info(f'  Mode: {"ArduPilot DDS" if use_ardupilot_topics else "Legacy JSON"}')
+        self._qr_data: Optional[str] = None
-        self.get_logger().info('=' * 50)
+        self._qr_position: Optional[Dict[str, float]] = None
+        self._search_pattern_angle = 0.0
+        
+        self._commands_sent = False
+        self._command_acknowledged = False
         
-        # State variables
-        self._latest_telemetry: Optional[Dict[str, Any]] = None
-        self._rover_telemetry: Optional[Dict[str, Any]] = None
-        self._telemetry_received = False
         self._landing_complete = False
         
-        # ArduPilot state (built from DDS topics)
+        self._latest_telemetry: Optional[Dict[str, Any]] = None
+        self._rover_telemetry: Optional[Dict[str, Any]] = None
+        self._latest_camera_image: Optional[bytes] = None
+        self._telemetry_received = False
+        
         self._ap_pose: Optional[PoseStamped] = None
         self._ap_twist: Optional[TwistStamped] = None
         self._ap_imu: Optional[Imu] = None
         self._ap_battery: Optional[BatteryState] = None
         
-        # QoS for sensor topics
+        self.get_logger().info('=' * 50)
+        self.get_logger().info('Drone Controller - 3 Phase GPS-Denied Landing')
+        self.get_logger().info(f'  Phase 1: SEARCH | Phase 2: COMMAND | Phase 3: LAND')
+        self.get_logger().info(f'  Mode: {"ArduPilot DDS" if use_ardupilot_topics else "Legacy JSON"}')
+        self.get_logger().info('=' * 50)
+        
         sensor_qos = QoSProfile(
             reliability=ReliabilityPolicy.BEST_EFFORT,
             history=HistoryPolicy.KEEP_LAST,
@@ -112,59 +120,33 @@ class DroneController(Node):
         else:
             self._setup_legacy_subscriptions()
         
-        # Always subscribe to rover telemetry (for moving landing pad)
         self._rover_telemetry_sub = self.create_subscription(
-            String, '/rover/telemetry', self._rover_telemetry_callback, 10
+            String, '/rover/telemetry', self._rover_telemetry_callback, 10)
-        )
+        
-        self.get_logger().info('  Subscribed to: /rover/telemetry')
+        self._camera_sub = self.create_subscription(
+            Image, '/drone/camera', self._camera_callback, sensor_qos)
         
-        # Command publisher
         self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
-        self.get_logger().info('  Publishing to: /cmd_vel')
+        self._rover_cmd_pub = self.create_publisher(String, '/rover/command', 10)
         
-        # Control loop
+        self._control_timer = self.create_timer(1.0 / self._control_rate, self._control_loop)
-        control_period = 1.0 / self._control_rate
-        self._control_timer = self.create_timer(control_period, self._control_loop)
-        self.get_logger().info(f'  Control loop: {self._control_rate} Hz')
         
-        self.get_logger().info('Drone Controller Ready!')
+        self.get_logger().info(f'Current Phase: {self._phase.name}')
-        self.get_logger().info('Sensors: IMU, Altimeter, Velocity, Landing Pad Detection')
     
     def _setup_ardupilot_subscriptions(self, qos: QoSProfile):
-        """Set up subscriptions to official ArduPilot ROS 2 topics."""
-        
-        # Pose (position + orientation)
         self._ap_pose_sub = self.create_subscription(
-            PoseStamped, '/ap/pose/filtered', self._ap_pose_callback, qos
+            PoseStamped, '/ap/pose/filtered', self._ap_pose_callback, qos)
-        )
-        self.get_logger().info('  Subscribed to: /ap/pose/filtered')
-        
-        # Twist (velocity)
         self._ap_twist_sub = self.create_subscription(
-            TwistStamped, '/ap/twist/filtered', self._ap_twist_callback, qos
+            TwistStamped, '/ap/twist/filtered', self._ap_twist_callback, qos)
-        )
-        self.get_logger().info('  Subscribed to: /ap/twist/filtered')
-        
-        # IMU
         self._ap_imu_sub = self.create_subscription(
-            Imu, '/ap/imu/filtered', self._ap_imu_callback, qos
+            Imu, '/ap/imu/filtered', self._ap_imu_callback, qos)
-        )
-        self.get_logger().info('  Subscribed to: /ap/imu/filtered')
-        
-        # Battery
         self._ap_battery_sub = self.create_subscription(
-            BatteryState, '/ap/battery', self._ap_battery_callback, qos
+            BatteryState, '/ap/battery', self._ap_battery_callback, qos)
-        )
-        self.get_logger().info('  Subscribed to: /ap/battery')
     
     def _setup_legacy_subscriptions(self):
-        """Set up subscriptions to legacy JSON telemetry."""
         self._telemetry_sub = self.create_subscription(
-            String, '/drone/telemetry', self._telemetry_callback, 10
+            String, '/drone/telemetry', self._telemetry_callback, 10)
-        )
-        self.get_logger().info('  Subscribed to: /drone/telemetry')
     
-    # ArduPilot topic callbacks
     def _ap_pose_callback(self, msg: PoseStamped):
         self._ap_pose = msg
         if not self._telemetry_received:
@@ -185,82 +167,6 @@ class DroneController(Node):
         self._ap_battery = msg
         self._build_telemetry_from_ardupilot()
     
-    def _build_telemetry_from_ardupilot(self):
-        """Build telemetry dict from ArduPilot topics."""
-        telemetry = {}
-        
-        # Position and orientation from pose
-        if self._ap_pose:
-            pos = self._ap_pose.pose.position
-            ori = self._ap_pose.pose.orientation
-            roll, pitch, yaw = quaternion_to_euler(ori.x, ori.y, ori.z, ori.w)
-            
-            telemetry['position'] = {
-                'x': pos.x,
-                'y': pos.y,
-                'z': pos.z
-            }
-            telemetry['altimeter'] = {
-                'altitude': pos.z,
-                'vertical_velocity': self._ap_twist.twist.linear.z if self._ap_twist else 0.0
-            }
-            telemetry['imu'] = {
-                'orientation': {'roll': roll, 'pitch': pitch, 'yaw': yaw},
-                'angular_velocity': {'x': 0, 'y': 0, 'z': 0}
-            }
-        
-        # Velocity from twist
-        if self._ap_twist:
-            twist = self._ap_twist.twist
-            telemetry['velocity'] = {
-                'x': twist.linear.x,
-                'y': twist.linear.y,
-                'z': twist.linear.z
-            }
-            if 'altimeter' in telemetry:
-                telemetry['altimeter']['vertical_velocity'] = twist.linear.z
-        
-        # Angular velocity from IMU
-        if self._ap_imu:
-            if 'imu' not in telemetry:
-                telemetry['imu'] = {}
-            telemetry['imu']['angular_velocity'] = {
-                'x': self._ap_imu.angular_velocity.x,
-                'y': self._ap_imu.angular_velocity.y,
-                'z': self._ap_imu.angular_velocity.z
-            }
-        
-        # Battery
-        if self._ap_battery:
-            telemetry['battery'] = {
-                'voltage': self._ap_battery.voltage,
-                'remaining': self._ap_battery.percentage * 100
-            }
-        
-        # Landing pad detection (placeholder - would need vision processing)
-        # For now, calculate from rover telemetry if available
-        if self._rover_telemetry and self._ap_pose:
-            rover_pos = self._rover_telemetry.get('position', {})
-            rx = rover_pos.get('x', 0)
-            ry = rover_pos.get('y', 0)
-            dx = self._ap_pose.pose.position.x
-            dy = self._ap_pose.pose.position.y
-            dz = self._ap_pose.pose.position.z
-            
-            rel_x = rx - dx
-            rel_y = ry - dy
-            distance = math.sqrt(rel_x**2 + rel_y**2 + dz**2)
-            
-            telemetry['landing_pad'] = {
-                'relative_x': rel_x,
-                'relative_y': rel_y,
-                'distance': distance,
-                'confidence': 1.0 if distance < 10.0 else 0.0
-            }
-        
-        self._latest_telemetry = telemetry
-    
-    # Legacy callbacks
     def _telemetry_callback(self, msg: String) -> None:
         try:
             self._latest_telemetry = json.loads(msg.data)
@@ -277,32 +183,96 @@ class DroneController(Node):
         except json.JSONDecodeError:
             pass
     
+    def _camera_callback(self, msg: Image) -> None:
+        try:
+            self._latest_camera_image = bytes(msg.data)
+            self._camera_width = msg.width
+            self._camera_height = msg.height
+            self._camera_encoding = msg.encoding
+        except Exception as e:
+            self.get_logger().warning(f'Camera callback error: {e}')
+    
+    def _build_telemetry_from_ardupilot(self):
+        telemetry = {}
+        
+        if self._ap_pose:
+            pos = self._ap_pose.pose.position
+            ori = self._ap_pose.pose.orientation
+            roll, pitch, yaw = quaternion_to_euler(ori.x, ori.y, ori.z, ori.w)
+            
+            telemetry['position'] = {'x': pos.x, 'y': pos.y, 'z': pos.z}
+            telemetry['altimeter'] = {
+                'altitude': pos.z,
+                'vertical_velocity': self._ap_twist.twist.linear.z if self._ap_twist else 0.0
+            }
+            telemetry['imu'] = {
+                'orientation': {'roll': roll, 'pitch': pitch, 'yaw': yaw},
+                'angular_velocity': {'x': 0, 'y': 0, 'z': 0}
+            }
+        
+        if self._ap_twist:
+            twist = self._ap_twist.twist
+            telemetry['velocity'] = {
+                'x': twist.linear.x, 'y': twist.linear.y, 'z': twist.linear.z
+            }
+            if 'altimeter' in telemetry:
+                telemetry['altimeter']['vertical_velocity'] = twist.linear.z
+        
+        if self._ap_imu:
+            if 'imu' not in telemetry:
+                telemetry['imu'] = {}
+            telemetry['imu']['angular_velocity'] = {
+                'x': self._ap_imu.angular_velocity.x,
+                'y': self._ap_imu.angular_velocity.y,
+                'z': self._ap_imu.angular_velocity.z
+            }
+        
+        if self._ap_battery:
+            telemetry['battery'] = {
+                'voltage': self._ap_battery.voltage,
+                'remaining': self._ap_battery.percentage * 100
+            }
+        
+        if self._qr_position:
+            telemetry['landing_pad'] = self._qr_position
+        elif self._rover_telemetry and self._ap_pose:
+            rover_pos = self._rover_telemetry.get('position', {})
+            rx, ry = rover_pos.get('x', 0), rover_pos.get('y', 0)
+            dx = self._ap_pose.pose.position.x
+            dy = self._ap_pose.pose.position.y
+            dz = self._ap_pose.pose.position.z
+            
+            rel_x, rel_y = rx - dx, ry - dy
+            distance = math.sqrt(rel_x**2 + rel_y**2 + dz**2)
+            
+            telemetry['landing_pad'] = {
+                'relative_x': rel_x, 'relative_y': rel_y,
+                'distance': distance, 'confidence': 1.0 if distance < 10.0 else 0.0
+            }
+        
+        self._latest_telemetry = telemetry
+    
     def _control_loop(self) -> None:
         if self._latest_telemetry is None:
             return
         
-        if self._landing_complete:
+        if self._phase == Phase.COMPLETE:
             return
         
-        # Warmup period - wait for stable telemetry before checking landing
         if not hasattr(self, '_warmup_count'):
             self._warmup_count = 0
         self._warmup_count += 1
-        if self._warmup_count < 100:  # Wait ~2 seconds at 50Hz
+        if self._warmup_count < 50:
-            # Still fly with controller, just don't check for landing yet
-            pass
-        elif self._check_landing_complete():
-            self._landing_complete = True
-            self.get_logger().info('=' * 50)
-            self.get_logger().info('LANDING COMPLETE!')
-            self.get_logger().info('=' * 50)
-            self._publish_command(0.0, 0.0, 0.0, 0.0)
             return
         
-        thrust, pitch, roll, yaw = self.calculate_landing_maneuver(
+        if self._phase == Phase.SEARCH:
-            self._latest_telemetry,
+            thrust, pitch, roll, yaw = self._execute_search_phase()
-            self._rover_telemetry
+        elif self._phase == Phase.COMMAND:
-        )
+            thrust, pitch, roll, yaw = self._execute_command_phase()
+        elif self._phase == Phase.LAND:
+            thrust, pitch, roll, yaw = self._execute_land_phase()
+        else:
+            thrust, pitch, roll, yaw = 0.0, 0.0, 0.0, 0.0
         
         thrust = max(min(thrust, self._max_thrust), -self._max_thrust)
         pitch = max(min(pitch, self._max_pitch), -self._max_pitch)
@@ -311,47 +281,139 @@ class DroneController(Node):
         
         self._publish_command(thrust, pitch, roll, yaw)
     
-    def _check_landing_complete(self) -> bool:
+    def _execute_search_phase(self) -> tuple:
-        if self._latest_telemetry is None:
+        qr_result = self.detect_qr_code()
-            return False
+        
+        if qr_result is not None:
+            self._qr_detected = True
+            self._qr_data = qr_result.get('data')
+            self._qr_position = qr_result.get('position')
+            
+            self.get_logger().info(f'QR CODE DETECTED: {self._qr_data}')
+            self._transition_to_phase(Phase.COMMAND)
+            return (0.0, 0.0, 0.0, 0.0)
+        
+        return self.calculate_search_maneuver(self._latest_telemetry)
+    
+    def detect_qr_code(self) -> Optional[Dict[str, Any]]:
+        if not CV2_AVAILABLE or self._latest_camera_image is None:
+            return None
+        
         try:
-            altimeter = self._latest_telemetry.get('altimeter', {})
+            if self._camera_encoding == 'rgb8':
-            altitude = altimeter.get('altitude', float('inf'))
+                img = np.frombuffer(self._latest_camera_image, dtype=np.uint8)
-            vertical_velocity = abs(altimeter.get('vertical_velocity', float('inf')))
+                img = img.reshape((self._camera_height, self._camera_width, 3))
-            return altitude < self._landing_height and vertical_velocity < self._landing_velocity
+                img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
-        except (KeyError, TypeError):
+            elif self._camera_encoding == 'bgr8':
-            return False
+                img = np.frombuffer(self._latest_camera_image, dtype=np.uint8)
+                img = img.reshape((self._camera_height, self._camera_width, 3))
+            else:
+                return None
+            
+            detector = cv2.QRCodeDetector()
+            data, points, _ = detector.detectAndDecode(img)
+            
+            if data and points is not None:
+                cx = np.mean(points[0][:, 0])
+                cy = np.mean(points[0][:, 1])
+                
+                rel_x = (cx - self._camera_width / 2) / (self._camera_width / 2)
+                rel_y = (cy - self._camera_height / 2) / (self._camera_height / 2)
+                
+                qr_size = np.linalg.norm(points[0][0] - points[0][2])
+                altitude = self._latest_telemetry.get('altimeter', {}).get('altitude', 5.0)
+                
+                return {
+                    'data': data,
+                    'position': {
+                        'relative_x': rel_x * altitude * 0.5,
+                        'relative_y': rel_y * altitude * 0.5,
+                        'distance': altitude,
+                        'confidence': min(qr_size / 100, 1.0)
+                    }
+                }
+        except Exception as e:
+            self.get_logger().debug(f'QR detection error: {e}')
+        
+        return None
+    
+    def calculate_search_maneuver(self, telemetry: Dict[str, Any]) -> tuple:
+        altimeter = telemetry.get('altimeter', {})
+        altitude = altimeter.get('altitude', 5.0)
+        vertical_vel = altimeter.get('vertical_velocity', 0.0)
+        
+        target_altitude = 5.0
+        altitude_error = target_altitude - altitude
+        thrust = self._Kp_z * altitude_error - self._Kd_z * vertical_vel
+        
+        self._search_pattern_angle += 0.01
+        yaw = 0.1
+        
+        velocity = telemetry.get('velocity', {})
+        pitch = -self._Kd_xy * velocity.get('x', 0)
+        roll = -self._Kd_xy * velocity.get('y', 0)
+        
+        return (thrust, pitch, roll, yaw)
+    
+    def _execute_command_phase(self) -> tuple:
+        if not self._commands_sent:
+            command = self.generate_rover_command(self._qr_data)
+            self._send_rover_command(command)
+            self._commands_sent = True
+            self._command_time = self.get_clock().now()
+        
+        elapsed = (self.get_clock().now() - self._command_time).nanoseconds / 1e9
+        
+        if self._command_acknowledged or elapsed > 5.0:
+            self.get_logger().info('Command phase complete, transitioning to LAND')
+            self._transition_to_phase(Phase.LAND)
+        
+        return self.calculate_hover_maneuver(self._latest_telemetry)
+    
+    def generate_rover_command(self, qr_data: Optional[str]) -> Dict[str, Any]:
+        return {
+            'type': 'prepare_landing',
+            'qr_data': qr_data,
+            'drone_position': self._latest_telemetry.get('position', {})
+        }
+    
+    def _send_rover_command(self, command: Dict[str, Any]) -> None:
+        msg = String()
+        msg.data = json.dumps(command)
+        self._rover_cmd_pub.publish(msg)
+        self.get_logger().info(f'Sent rover command: {command.get("type")}')
+    
+    def calculate_hover_maneuver(self, telemetry: Dict[str, Any]) -> tuple:
+        altimeter = telemetry.get('altimeter', {})
+        altitude = altimeter.get('altitude', 5.0)
+        vertical_vel = altimeter.get('vertical_velocity', 0.0)
+        
+        altitude_error = 0
+        thrust = self._Kp_z * altitude_error - self._Kd_z * vertical_vel
+        
+        velocity = telemetry.get('velocity', {})
+        pitch = -self._Kd_xy * velocity.get('x', 0)
+        roll = -self._Kd_xy * velocity.get('y', 0)
+        
+        return (thrust, pitch, roll, 0.0)
+    
+    def _execute_land_phase(self) -> tuple:
+        if self._check_landing_complete():
+            self._landing_complete = True
+            self.get_logger().info('LANDING COMPLETE!')
+            self._transition_to_phase(Phase.COMPLETE)
+            return (0.0, 0.0, 0.0, 0.0)
+        
+        return self.calculate_landing_maneuver(
+            self._latest_telemetry,
+            self._rover_telemetry
+        )
     
     def calculate_landing_maneuver(
         self,
         telemetry: Dict[str, Any],
         rover_telemetry: Optional[Dict[str, Any]] = None
-    ) -> tuple[float, float, float, float]:
+    ) -> tuple:
-        """
-        Calculate control commands for GPS-denied landing.
-        
-        Available sensors in telemetry:
-            - imu.orientation: {roll, pitch, yaw} in radians
-            - imu.angular_velocity: {x, y, z} in rad/s
-            - altimeter.altitude: height in meters
-            - altimeter.vertical_velocity: vertical speed in m/s
-            - velocity: {x, y, z} estimated velocity in m/s
-            - landing_pad: relative position to pad (may be None)
-                - relative_x, relative_y: offset in body frame
-                - distance: vertical distance to pad
-                - confidence: detection confidence (0-1)
-            - camera: {width, height, fov, image}
-                - image: base64 encoded JPEG (or None)
-            - landed: bool
-        
-        Args:
-            telemetry: Sensor data from drone
-            rover_telemetry: Rover state (for moving target) - may be None
-        
-        Returns:
-            Tuple of (thrust, pitch, roll, yaw) control values (-1.0 to 1.0)
-        """
-        # Extract sensor data
         altimeter = telemetry.get('altimeter', {})
         altitude = altimeter.get('altitude', 5.0)
         vertical_vel = altimeter.get('vertical_velocity', 0.0)
@@ -362,10 +424,6 @@ class DroneController(Node):
         
         landing_pad = telemetry.get('landing_pad', None)
         
-        # camera = telemetry.get('camera', None)
-
-        
-        # Descent control
         if altitude > 1.0:
             target_descent_rate = -0.5
         else:
@@ -374,7 +432,6 @@ class DroneController(Node):
         descent_error = target_descent_rate - vertical_vel
         thrust = self._Kp_z * descent_error
         
-        # Horizontal control
         if landing_pad is not None:
             target_x = landing_pad.get('relative_x', 0.0)
             target_y = landing_pad.get('relative_y', 0.0)
@@ -389,6 +446,23 @@ class DroneController(Node):
         
         return (thrust, pitch, roll, yaw)
     
+    def _transition_to_phase(self, new_phase: Phase) -> None:
+        old_phase = self._phase
+        self._phase = new_phase
+        self._phase_start_time = self.get_clock().now()
+        self.get_logger().info(f'Phase: {old_phase.name} -> {new_phase.name}')
+    
+    def _check_landing_complete(self) -> bool:
+        if self._latest_telemetry is None:
+            return False
+        try:
+            altimeter = self._latest_telemetry.get('altimeter', {})
+            altitude = altimeter.get('altitude', float('inf'))
+            vertical_velocity = abs(altimeter.get('vertical_velocity', float('inf')))
+            return altitude < self._landing_height and vertical_velocity < self._landing_velocity
+        except (KeyError, TypeError):
+            return False
+
     def _publish_command(self, thrust: float, pitch: float, roll: float, yaw: float) -> None:
         msg = Twist()
         msg.linear.z = thrust
@@ -397,14 +471,14 @@ class DroneController(Node):
         msg.angular.z = yaw
         self._cmd_vel_pub.publish(msg)
     
+    def get_current_phase(self) -> Phase:
+        return self._phase
+
 
 def main(args=None):
     import sys
     
-    # Check for --ardupilot flag
     use_ardupilot = '--ardupilot' in sys.argv or '-a' in sys.argv
-    
-    # Remove our custom args before passing to rclpy
     filtered_args = [a for a in (args or sys.argv) if a not in ['--ardupilot', '-a']]
     
     rclpy.init(args=filtered_args)