# System Architecture

## Overview

```
┌─────────────────────────────────────────────────────────────────┐
│                        Simulation                                │
├───────────────────┬─────────────────────┬───────────────────────┤
│   Gazebo Harmonic │   ArduPilot SITL    │    ROS 2 Nodes        │
│   (Physics/3D)    │   (Flight Control)  │    (Perception/Nav)   │
└───────────────────┴─────────────────────┴───────────────────────┘
         │                    │                      │
         └────────────────────┼──────────────────────┘
                              │
                    ┌─────────┴─────────┐
                    │   MAVLink/MAVROS  │
                    └───────────────────┘
```

## Components

### 1. Gazebo Harmonic

**Role:** 3D simulation, physics, sensors

- **World simulation**: Ground plane, lighting, physics
- **UAV model**: Iris quadcopter with cameras
- **UGV model**: Differential drive rover
- **Sensors**: Cameras, IMU, rangefinder

### 2. ArduPilot SITL

**Role:** Flight controller simulation

- **EKF3**: State estimation using external vision
- **Flight modes**: GUIDED, LOITER, RTL, LAND
- **Motor mixing**: Quadcopter dynamics
- **Failsafe**: Battery, geofence, communication

**Key Parameters (GPS-Denied):**
```
GPS_TYPE 0              # GPS disabled
EK3_SRC1_POSXY 6        # External nav for position
EK3_SRC1_VELXY 6        # External nav for velocity
VISO_TYPE 1             # MAVLink vision input
ARMING_CHECK 0          # Disabled for simulation
```

### 3. ROS 2 Nodes

#### Vision Pipeline

```
Camera → Visual Odometry → Position Estimator → Controller
           ↓
       Optical Flow ─────────────┘
```

| Node | Function |
|------|----------|
| `visual_odom_node` | ORB feature tracking, pose estimation |
| `optical_flow_node` | Lucas-Kanade velocity estimation |
| `position_estimator` | Weighted average sensor fusion |
| `ekf_fusion_node` | Extended Kalman Filter fusion |

#### Control Pipeline

```
Search Algorithm → UAV Controller → MAVROS → ArduPilot
                 → UGV Controller → cmd_vel
```

| Node | Function |
|------|----------|
| `uav_controller` | GUIDED mode control, auto-arm |
| `ugv_controller` | Differential drive control |
| `search` | Spiral, lawnmower, Lévy walk algorithms |

#### Safety Pipeline

```
GPS → Geofence Monitor → Failsafe Handler → Emergency Action
```

| Node | Function |
|------|----------|
| `geofence_node` | GPS-based boundary monitoring |
| `failsafe_handler` | Vision loss, battery, emergency |

## Data Flow

### Position Estimation

```
1. Camera captures frames (30 Hz)
2. ORB detects features
3. Essential matrix computed
4. Relative motion estimated
5. Position integrated
6. Published to /uav/visual_odometry/pose
```

### Control Loop

```
1. Target received (/uav/setpoint_position)
2. Current position from VO or MAVROS
3. Error computed
4. Velocity command generated
5. Sent via MAVROS to ArduPilot
6. ArduPilot executes motor commands
```

### Geofencing (GPS Only)

```
1. GPS fix received
2. Check against polygon boundary
3. Check altitude limits
4. If breach: trigger RTL/LAND
5. Navigation continues using VO (not GPS)
```

## Coordinate Frames

| Frame | Description |
|-------|-------------|
| `odom` | Local origin (takeoff point) |
| `base_link` | Vehicle body frame |
| `map` | World frame (aligned with odom) |

**NED Convention:**
- X = North (forward)
- Y = East (right)
- Z = Down (negative altitude)

## File Structure

```
src/
├── main.py                      # Entry point
├── vision/
│   ├── object_detector.py       # ArUco marker detection
│   └── camera_processor.py      # Gazebo camera feeds
├── navigation/
│   ├── search.py                # Search algorithm controller
│   ├── flight_tracker.py        # Live flight visualization
│   └── patterns/
│       ├── spiral.py            # Expanding square spiral
│       ├── lawnmower.py         # Back-and-forth lanes
│       └── levy.py              # Lévy walk random search
├── control/
│   ├── uav_controller.py        # UAV flight control (pymavlink)
│   └── ugv_controller.py        # UGV drive control (gz.transport)
├── safety/
│   └── geofence.py              # GPS boundary enforcement
└── utils/
    ├── config.py                # YAML config loader
    ├── convert.py               # Unit/coordinate conversions
    ├── helpers.py               # Distance utilities
    └── recorder.py              # Video recording & cloud upload
```

## Configuration

### ArduPilot EKF Sources

```yaml
EK3_SRC1_POSXY: 6  # External Nav
EK3_SRC1_POSZ: 1   # Barometer
EK3_SRC1_VELXY: 6  # External Nav
EK3_SRC1_YAW: 6    # External Nav
```

### Sensor Weights

```yaml
vo_weight: 0.6      # Visual odometry
optical_flow: 0.3   # Optical flow
imu: 0.1            # IMU integration
```

### Speed Limits

```yaml
WPNAV_SPEED: 150      # Horizontal speed (cm/s)
WPNAV_ACCEL: 100      # Horizontal acceleration (cm/s/s)
WPNAV_SPEED_UP: 100   # Climb speed (cm/s)
WPNAV_SPEED_DN: 75    # Descent speed (cm/s)
WPNAV_ACCEL_Z: 75     # Vertical acceleration (cm/s/s)
LOIT_SPEED: 150       # Loiter speed (cm/s)
```