Code reorganization and Drone Logic Update

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`
-2. Find `calculate_landing_maneuver()`
-3. Implement your algorithm
-4. Test with any simulation mode
+| Phase | Action |
+|-------|--------|
+| SEARCH | Find QR code on rover |
+| 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
-altitude = telemetry['altimeter']['altitude']
-vertical_vel = telemetry['altimeter']['vertical_velocity']
-
-# Velocity
-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']
+telemetry = {
+    "altimeter": {"altitude": 5.0, "vertical_velocity": -0.1},
+    "velocity": {"x": 0, "y": 0, "z": 0},
+    "imu": {"orientation": {"roll": 0, "pitch": 0, "yaw": 0}},
+    "landing_pad": {"relative_x": 0.5, "relative_y": -0.2, "distance": 5.0}
+}
 ```
 
 ## 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:
-- Thrust → Z velocity
-- Pitch/Roll → X/Y velocity
+## Configuration
 
-## Example Algorithm (PD Control)
+Edit `config.py`:
 
 ```python
-def calculate_landing_maneuver(self, telemetry, rover_telemetry):
-    alt = telemetry.get('altimeter', {})
-    altitude = alt.get('altitude', 5.0)
-    vert_vel = alt.get('vertical_velocity', 0.0)
-    
-    # Altitude PD control
-    thrust = 0.5 * (target_alt - altitude) - 0.3 * vert_vel
-    
-    # Horizontal control
-    pad = telemetry.get('landing_pad')
-    if pad:
-        pitch = 0.3 * pad['relative_x']
-        roll = 0.3 * pad['relative_y']
-    else:
-        # Hover
-        pitch = 0
-        roll = 0
-    
-    return (thrust, pitch, roll, 0.0)
+CONTROLLER = {
+    "Kp_z": 0.5,
+    "Kd_z": 0.3,
+    "Kp_xy": 0.3,
+    "Kd_xy": 0.2,
+    "rate": 50,
+}
+```
+
+## Testing
+
+```bash
+./scripts/run_ardupilot_sim.sh runway
+./scripts/run_ardupilot_controller.sh
 ```