# 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

```
Mission Planner → UAV Controller → MAVROS → ArduPilot
                → UGV Controller → cmd_vel
```

| Node | Function |
|------|----------|
| `uav_controller` | GUIDED mode control, auto-arm |
| `ugv_controller` | Differential drive control |
| `mission_planner` | Multi-vehicle coordination |

#### 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/
├── vision/
│   ├── visual_odometry.py    # Feature tracking VO
│   ├── optical_flow.py       # LK optical flow
│   └── camera_processor.py   # Image processing
├── localization/
│   ├── position_estimator.py # Weighted fusion
│   └── ekf_fusion.py         # EKF fusion
├── navigation/
│   ├── local_planner.py      # Path planning
│   └── waypoint_follower.py  # Waypoint tracking
├── control/
│   ├── uav_controller.py     # UAV flight control
│   ├── ugv_controller.py     # UGV drive control
│   └── mission_planner.py    # Coordination
└── safety/
    ├── geofence_monitor.py   # GPS boundaries
    └── failsafe_handler.py   # Emergency handling
```

## 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
```