200 lines
4.3 KiB
Markdown
200 lines
4.3 KiB
Markdown
# ArduPilot + Gazebo Guide
|
|
|
|
## Architecture
|
|
|
|
```
|
|
┌─────────────────┐ UDP 9002 ┌─────────────────┐ TCP 5760 ┌─────────────────┐
|
|
│ │ JSON sensor data │ │ MAVLink cmds │ │
|
|
│ Gazebo │◄─────────────────────►│ ArduPilot SITL │◄───────────────────►│ Controller │
|
|
│ (Physics) │ motor commands │ (ArduCopter) │ telemetry │ (run_ardupilot) │
|
|
│ │ │ │ │ │
|
|
└─────────────────┘ └─────────────────┘ └─────────────────┘
|
|
```
|
|
|
|
## Quick Start (3 Terminals)
|
|
|
|
### Terminal 1: Start Gazebo FIRST
|
|
|
|
```bash
|
|
cd ~/RDC_Simulation
|
|
./scripts/run_ardupilot_sim.sh runway
|
|
```
|
|
|
|
Wait until the drone appears in the 3D view!
|
|
|
|
### Terminal 2: Start ArduCopter SITL
|
|
|
|
```bash
|
|
source ~/.ardupilot_env
|
|
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
|
|
```
|
|
|
|
You should see:
|
|
- "JSON received: timestamp, imu, position..." ✅
|
|
- NOT "No JSON sensor message received" ❌
|
|
|
|
### Terminal 3: Run Controller
|
|
|
|
```bash
|
|
cd ~/RDC_Simulation
|
|
source activate.sh
|
|
python scripts/run_ardupilot.py --pattern square
|
|
```
|
|
|
|
## Flight Patterns
|
|
|
|
```bash
|
|
# Square pattern (5m sides, 5m altitude)
|
|
python scripts/run_ardupilot.py --pattern square --size 5 --altitude 5
|
|
|
|
# Circle pattern (5m radius)
|
|
python scripts/run_ardupilot.py --pattern circle --size 5
|
|
|
|
# Hover in place
|
|
python scripts/run_ardupilot.py --pattern hover
|
|
|
|
# GPS mode (instead of GPS-denied)
|
|
python scripts/run_ardupilot.py --pattern square --gps
|
|
```
|
|
|
|
## Controller Code
|
|
|
|
```python
|
|
from src.drone_controller import DroneController
|
|
|
|
drone = DroneController()
|
|
drone.connect()
|
|
|
|
# GPS-denied setup
|
|
drone.setup_gps_denied()
|
|
drone.set_mode("GUIDED_NOGPS")
|
|
|
|
# Or GPS mode
|
|
# drone.set_mode("GUIDED")
|
|
|
|
drone.arm()
|
|
drone.takeoff(5.0)
|
|
drone.fly_square(size=5, altitude=5)
|
|
drone.land()
|
|
```
|
|
|
|
## ArduCopter Flight Modes
|
|
|
|
| Mode | ID | Description |
|
|
|------|-----|-------------|
|
|
| STABILIZE | 0 | Manual, self-leveling |
|
|
| ALT_HOLD | 2 | Hold altitude |
|
|
| GUIDED | 4 | Waypoint commands (GPS required) |
|
|
| LOITER | 5 | Hold position (GPS required) |
|
|
| RTL | 6 | Return to launch |
|
|
| LAND | 9 | Automatic landing |
|
|
| **GUIDED_NOGPS** | 20 | Commands without GPS |
|
|
|
|
## MAVProxy Commands
|
|
|
|
In the SITL console, you can type:
|
|
|
|
```bash
|
|
# Check status
|
|
status
|
|
|
|
# Change mode
|
|
mode GUIDED
|
|
mode GUIDED_NOGPS
|
|
|
|
# Arm
|
|
arm throttle force
|
|
|
|
# Takeoff (in GUIDED mode)
|
|
takeoff 5
|
|
|
|
# Land
|
|
mode LAND
|
|
|
|
# Set parameter
|
|
param set ARMING_CHECK 0
|
|
```
|
|
|
|
## Gazebo Worlds
|
|
|
|
```bash
|
|
# Outdoor runway (default)
|
|
./scripts/run_ardupilot_sim.sh runway
|
|
|
|
# Indoor warehouse
|
|
./scripts/run_ardupilot_sim.sh warehouse
|
|
```
|
|
|
|
## MAVLink Connection
|
|
|
|
Default: `tcp:127.0.0.1:5760`
|
|
|
|
Port map:
|
|
- **5760**: Primary MAVLink (SERIAL0)
|
|
- **5762**: Secondary (SERIAL1)
|
|
- **5763**: Tertiary (SERIAL2)
|
|
- **14550**: MAVProxy GCS output (UDP)
|
|
|
|
## Troubleshooting
|
|
|
|
### "No JSON sensor message received"
|
|
|
|
Gazebo isn't sending data.
|
|
|
|
**Fix:**
|
|
1. Make sure Gazebo is running FIRST
|
|
2. Wait for world to fully load
|
|
3. Then start SITL
|
|
|
|
### Drone won't arm
|
|
|
|
**Fix:**
|
|
```bash
|
|
# In MAVProxy console
|
|
arm throttle force
|
|
|
|
# Or set in your code
|
|
drone.set_param("ARMING_CHECK", 0)
|
|
```
|
|
|
|
### Wrong vehicle type (ArduPlane)
|
|
|
|
Mode list shows FBWA, FBWB, QSTABILIZE instead of GUIDED_NOGPS, ALT_HOLD.
|
|
|
|
**Fix:**
|
|
```bash
|
|
pkill -9 -f sim_vehicle
|
|
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --wipe-eeprom
|
|
```
|
|
|
|
### Mode not changing
|
|
|
|
The controller hardcodes mode IDs for ArduCopter. If using ArduPlane, modes will differ.
|
|
|
|
### Gazebo plugin not loading
|
|
|
|
```bash
|
|
# Check plugin exists
|
|
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
|
|
|
|
# Check environment
|
|
echo $GZ_SIM_SYSTEM_PLUGIN_PATH
|
|
# Should include: ~/ardupilot_gazebo/build
|
|
```
|
|
|
|
## Useful Debug Commands
|
|
|
|
```bash
|
|
# List Gazebo topics
|
|
gz topic -l
|
|
|
|
# Echo a topic
|
|
gz topic -e -t /model/iris_with_gimbal/pose
|
|
|
|
# Check SITL processes
|
|
ps aux | grep ardupilot
|
|
|
|
# Check port usage
|
|
lsof -i :5760
|
|
```
|