# Architecture

System architecture for GPS-denied UAV/UGV navigation.

## Overview

The system uses vision-based navigation with GPS reserved only for geofencing.

```
Vision Sensors (Cameras)
         |
         v
Visual Odometry & Optical Flow
         |
         v
Position Estimator (EKF)
         |
         v
ArduPilot Flight Controller
         |
         v
Motor Control
```

## Components

### Perception

- **Forward Camera**: Visual odometry, feature tracking
- **Downward Camera**: Optical flow, ground plane detection
- **IMU**: Angular velocity, acceleration

### Localization

- **Visual Odometry**: Estimates motion from camera images
- **Optical Flow**: Velocity estimation from downward camera
- **EKF Fusion**: Combines all sensor inputs into position estimate

### Navigation

- **Waypoint Navigation**: Relative coordinates (meters from origin)
- **Path Planning**: Collision-free paths using local map
- **Position Hold**: Maintain position using vision

### Control

- **ArduPilot**: Flight controller firmware
- **MAVROS**: ROS interface to ArduPilot
- **Velocity/Position Control**: Low-level motor commands

### Safety

- **Geofencing**: GPS-based boundary checking (safety only)
- **Altitude Limits**: Maximum flight ceiling
- **Failsafe**: RTL on signal loss or boundary breach

## Coordinate Frames

| Frame | Description |
|-------|-------------|
| body | Attached to vehicle, X forward, Y right, Z down |
| odom | Origin at takeoff, accumulates drift |
| map | Fixed world frame (may be corrected) |

All navigation commands use local coordinates relative to takeoff point.

## Data Flow

```
Camera Images
     |
     v
Feature Detection (ORB/SIFT)
     |
     v
Feature Matching (frame to frame)
     |
     v
Motion Estimation (Essential Matrix)
     |
     v
EKF Update (fuse with IMU)
     |
     v
Position Estimate (x, y, z, roll, pitch, yaw)
     |
     v
ArduPilot (external position input)
     |
     v
PID Control -> Motor PWM
```

## ArduPilot Integration

ArduPilot receives external position via MAVLink:

1. Visual odometry → `VISION_POSITION_ESTIMATE`
2. EKF uses external source (EK3_SRC1_POSXY=6)
3. GPS disabled for navigation (GPS_TYPE=0 or EK3_SRC1_POSXY!=1)

## Geofencing

GPS is only used for safety boundaries:

1. GPS position → lat/lon
2. Check against polygon/circle boundary
3. If outside: trigger RTL or LAND

Navigation continues using vision regardless of GPS status.

## ROS 2 Topics

| Topic | Type | Description |
|-------|------|-------------|
| /uav/camera/forward/image_raw | sensor_msgs/Image | Forward camera |
| /uav/camera/downward/image_raw | sensor_msgs/Image | Downward camera |
| /mavros/local_position/pose | geometry_msgs/PoseStamped | Current position |
| /mavros/setpoint_position/local | geometry_msgs/PoseStamped | Target position |
| /mavros/imu/data | sensor_msgs/Imu | IMU data |