SirBlob/RDC_Simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ardupilot Controller Script

Browse Source
This commit is contained in:
default
2026-01-04 02:31:31 +00:00
parent 4b514f1dd9
commit c5b208c91a
6 changed files with 438 additions and 349 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
docs/architecture.md
View File

@@ -24,14 +24,20 @@ Terminal 1 Terminal 2
└───────────────────┘ ROS └───────────────────┘
```
### 3. ArduPilot GPS-Denied (2 Terminals)
### 3. ArduPilot (2 Terminals)
```
Terminal 1 Terminal 2
┌───────────────────┐ ┌───────────────────┐
│ Gazebo + │◄──────►│ ArduPilot SITL │
│ ArduPilot Plugin │ JSON │ + MAVProxy │
└───────────────────┘ └───────────────────┘
┌───────────────────┐ ┌────────────────────────────┐
│ Gazebo + │◄──────►│ run_ardupilot_controller.sh│
│ ArduPilot Plugin │ JSON │ ┌──────────────────┐ │
└───────────────────┘ │ │ ArduPilot SITL │ │
│ └─────────┬────────┘ │
│ │ MAVLink │
│ ┌─────────▼────────┐ │
│ │ run_ardupilot.py │ │
│ └──────────────────┘ │
└────────────────────────────┘
```
## Data Flow
@@ -48,31 +54,45 @@ Controller → /cmd_vel → Gazebo → /odometry → Controller
### ArduPilot
```
Gazebo ◄─── JSON/UDP ───► SITL ◄─── MAVLink ───► MAVProxy
Gazebo ◄─── JSON ───► SITL ◄─── MAVLink ───► Controller
```
## Key Files
| File | Purpose |
|------|---------|
| `drone_controller.py` | Your landing algorithm |
| `gazebo_bridge.py` | Gazebo ↔ ROS bridge |
| `camera_viewer.py` | Camera display |
| `config.py` | Configuration |
| `drone_controller.py` | **Your landing algorithm (used in ALL modes)** |
| `run_ardupilot.py` | MAVLink interface for ArduPilot |
| `run_gazebo.py` | ROS 2 interface for Gazebo |
| `standalone_simulation.py` | PyBullet simulation engine |
| `config.py` | Shared configuration |
## GPS-Denied Sensors
| Sensor | Data |
|--------|------|
| IMU | Orientation, angular velocity |
| Altimeter | Altitude, vertical velocity |
| Camera | Downward image |
| Landing Pad | Relative position |
The controller receives this standardized telemetry structure in all modes:
## Topics
| Topic | Direction |
|-------|-----------|
| `/cmd_vel` | → Drone commands |
| `/drone/telemetry` | ← Sensor data |
| `/drone/camera` | ← Camera images |
```python
telemetry = {
"altimeter": {
"altitude": float, # Meters
"vertical_velocity": float # m/s (positive = up)
},
"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
}
}
```

111
docs/ardupilot.md
View File

@@ -1,9 +1,38 @@
# ArduPilot GPS-Denied Simulation
Realistic flight controller simulation without GPS.
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
@@ -12,10 +41,13 @@ Realistic flight controller simulation without GPS.
**Terminal 2:**
```bash
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
```
# GPS-denied mode:
**Terminal 3:**
```bash
# Debug commands in MAVProxy:
param set ARMING_CHECK 0
mode stabilize
mode guided
arm throttle force
```
@@ -26,67 +58,32 @@ arm throttle force
source ~/.bashrc
```
## World Options
## Configuration
```bash
./scripts/run_ardupilot_sim.sh runway # Iris on runway (default)
./scripts/run_ardupilot_sim.sh warehouse # Iris in warehouse
./scripts/run_ardupilot_sim.sh zephyr # Zephyr plane
./scripts/run_ardupilot_sim.sh gimbal # Gimbal test
./scripts/run_ardupilot_sim.sh parachute # Parachute test
```
Your `drone_controller.py` receives telemetry and returns control inputs.
## GPS-Denied Configuration
In MAVProxy console:
```bash
param set ARMING_CHECK 0 # Disable pre-arm checks
mode stabilize # Manual with stabilization
arm throttle force # Force arm
```
## GPU Support
Auto-detects: NVIDIA > Intel > AMD > Software
Check your GPU:
```bash
glxinfo | grep "OpenGL renderer"
```
## MAVProxy Commands
```bash
mode stabilize # Manual mode
mode alt_hold # Altitude hold
arm throttle force # Arm motors
takeoff 5 # Takeoff 5m
mode land # Land
```
The simulation translates your inputs:
- `thrust` → Vertical velocity
- `pitch/roll` → Horizontal velocity
- `yaw` → Yaw rate
## 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.
Gazebo isn't sending data to SITL. Check:
1. Start Gazebo FIRST, then SITL
2. Plugin path is set:
```bash
echo $GZ_SIM_SYSTEM_PLUGIN_PATH | grep ardupilot
```
### Simulation laggy
Check GPU is being used (not software):
```bash
glxinfo | grep "OpenGL renderer"
# Should NOT show "llvmpipe"
```
### Can't arm
## Visualizing Camera
```bash
param set ARMING_CHECK 0
arm throttle force
python camera_viewer.py --topic /drone/camera
```
(Requires bridging the topic if using ROS 2 bridge)

55
docs/drone_guide.md
View File

@@ -7,12 +7,13 @@ Implement your GPS-denied landing algorithm.
1. Edit `drone_controller.py`
2. Find `calculate_landing_maneuver()`
3. Implement your algorithm
4. Test: `python standalone_simulation.py`
4. Test with any simulation mode
## Function to Implement
```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry):
# Your logic here
return (thrust, pitch, roll, yaw)
```
@@ -38,12 +39,16 @@ if landing_pad:
| Value | Range | Effect |
|-------|-------|--------|
| thrust | ±1.0 | Up/down |
| thrust | ±1.0 | Up/down (positive = up) |
| pitch | ±0.5 | Forward/back |
| roll | ±0.5 | Left/right |
| yaw | ±0.5 | Rotation |
## Example Algorithm
Note: In ArduPilot mode, these are scaled to velocities:
- Thrust → Z velocity
- Pitch/Roll → X/Y velocity
## Example Algorithm (PD Control)
```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry):
@@ -51,48 +56,18 @@ def calculate_landing_maneuver(self, telemetry, rover_telemetry):
altitude = alt.get('altitude', 5.0)
vert_vel = alt.get('vertical_velocity', 0.0)
vel = telemetry.get('velocity', {})
vel_x = vel.get('x', 0.0)
vel_y = vel.get('y', 0.0)
pad = telemetry.get('landing_pad')
# Altitude PD control
thrust = 0.5 * (0 - altitude) - 0.3 * vert_vel
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'] - 0.2 * vel_x
roll = 0.3 * pad['relative_y'] - 0.2 * vel_y
pitch = 0.3 * pad['relative_x']
roll = 0.3 * pad['relative_y']
else:
pitch = -0.2 * vel_x
roll = -0.2 * vel_y
# Hover
pitch = 0
roll = 0
return (thrust, pitch, roll, 0.0)
```
## Testing
```bash
# Easy
python standalone_simulation.py --pattern stationary
# Medium
python standalone_simulation.py --pattern circular --speed 0.3
# Hard
python standalone_simulation.py --pattern random --speed 0.5
```
## Configuration
Edit `config.py`:
```python
CONTROLLER = {
"Kp_z": 0.5,
"Kd_z": 0.3,
"Kp_xy": 0.3,
"Kd_xy": 0.2,
}
```