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

39
README.md
View File

@@ -22,32 +22,25 @@ ros2 launch gazebo/launch/drone_landing.launch.py
```bash
source activate.sh
python run_gazebo.py --pattern circular
python camera_viewer.py # View camera
```
### ArduPilot GPS-Denied (2 Terminals)
**Terminal 1:**
**Terminal 1 - Gazebo:**
```bash
./scripts/run_ardupilot_sim.sh runway
# Options: runway, warehouse, zephyr
```
**Terminal 2:**
**Terminal 2 - Controller:**
```bash
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
# GPS-denied mode:
param set ARMING_CHECK 0
mode stabilize
arm throttle force
./scripts/run_ardupilot_controller.sh
```
## Installation
```bash
./setup/install_ubuntu.sh
./setup/install_ardupilot.sh # Optional
./setup/install_ardupilot.sh # Optional for ArduPilot
source activate.sh
```
@@ -55,12 +48,12 @@ source activate.sh
| File | Description |
|------|-------------|
| `standalone_simulation.py` | All-in-one simulation |
| `run_gazebo.py` | Gazebo controllers |
| `scripts/run_ardupilot_sim.sh` | ArduPilot launcher (auto GPU) |
| `drone_controller.py` | **Your landing algorithm (used everywhere)** |
| `standalone_simulation.py` | PyBullet simulation |
| `run_gazebo.py` | Gazebo + ROS 2 interface |
| `run_ardupilot.py` | ArduPilot + MAVLink interface |
| `config.py` | Configuration settings |
| `camera_viewer.py` | Camera feed window |
| `drone_controller.py` | **Your landing algorithm** |
| `config.py` | Configuration |
## Sensors (GPS-Denied)
@@ -71,16 +64,10 @@ source activate.sh
| Camera | Downward image |
| Landing Pad | Relative position (when visible) |
## Options
```bash
--pattern stationary, linear, circular, square, random
--speed Speed in m/s (default: 0.5)
```
## Docs
## Documentation
- [Installation](docs/installation.md)
- [Architecture](docs/architecture.md)
- [ArduPilot](docs/ardupilot.md)
- [Gazebo](docs/gazebo.md)
- [ArduPilot Guide](docs/ardupilot.md)
- [Gazebo Guide](docs/gazebo.md)
- [Drone Logic Guide](docs/drone_guide.md)

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,
}
```

423
run_ardupilot.py
View File

@@ -1,235 +1,284 @@
#!/usr/bin/env python3
"""
ArduPilot ROS 2 Launcher - Official DDS Integration.
This script provides a convenient way to launch the ArduPilot SITL simulation
using the official ardupilot_gz packages with ROS 2 DDS support.
ArduPilot Drone Controller
==========================
Connects your drone_controller.py logic to ArduPilot SITL via MAVLink.
Usage:
python run_ardupilot.py # Launch Iris on runway
python run_ardupilot.py --world maze # Launch Iris in maze
python run_ardupilot.py --vehicle rover # Launch WildThumper rover
python run_ardupilot.py --mavproxy # Also start MAVProxy
Terminal 1: ./scripts/run_ardupilot_sim.sh runway
Terminal 2: sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
Terminal 3: python run_ardupilot.py
Prerequisites:
- ArduPilot ROS 2 packages installed (./setup/install_ardupilot.sh)
- ROS 2 Humble/Jazzy sourced
- ~/ardu_ws workspace built and sourced
This script:
1. Connects to ArduPilot SITL via MAVLink
2. Gets telemetry (position, attitude, velocity)
3. Runs your drone_controller.calculate_landing_maneuver()
4. Sends velocity commands to ArduPilot
"""
import argparse
import os
import signal
import subprocess
import sys
import time
from pathlib import Path
import math
import argparse
try:
from pymavlink import mavutil
except ImportError:
print("ERROR: pymavlink not installed")
print("Run: pip install pymavlink")
sys.exit(1)
from config import ARDUPILOT, CONTROLLER, MAVLINK
from drone_controller import DroneController
def check_ros2():
"""Check if ROS 2 is available."""
class ArduPilotInterface:
"""Interface to ArduPilot SITL via MAVLink."""
def __init__(self, connection_string=None):
self.connection_string = connection_string or MAVLINK["connection_string"]
self.mav = None
self.armed = False
self.mode = "UNKNOWN"
# Telemetry state
self.position = {"x": 0, "y": 0, "z": 0}
self.velocity = {"x": 0, "y": 0, "z": 0}
self.attitude = {"roll": 0, "pitch": 0, "yaw": 0}
self.altitude = 0
self.vertical_velocity = 0
def connect(self, timeout=30):
"""Connect to ArduPilot SITL."""
print(f"[INFO] Connecting to {self.connection_string}...")
try:
subprocess.run(['ros2', '--help'], capture_output=True, check=True)
self.mav = mavutil.mavlink_connection(self.connection_string)
self.mav.wait_heartbeat(timeout=timeout)
print(f"[OK] Connected to system {self.mav.target_system}")
return True
except (subprocess.CalledProcessError, FileNotFoundError):
except Exception as e:
print(f"[ERROR] Connection failed: {e}")
return False
def update_telemetry(self):
"""Read and update telemetry from ArduPilot."""
# Read all available messages
while True:
msg = self.mav.recv_match(blocking=False)
if msg is None:
break
def check_ardupilot_packages():
"""Check if ArduPilot ROS 2 packages are installed."""
try:
result = subprocess.run(
['ros2', 'pkg', 'list'],
capture_output=True,
text=True,
check=True
)
packages = result.stdout
return 'ardupilot_gz_bringup' in packages or 'ardupilot_sitl' in packages
except (subprocess.CalledProcessError, FileNotFoundError):
return False
msg_type = msg.get_type()
if msg_type == "HEARTBEAT":
self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
# Get mode name
if hasattr(mavutil.mavlink, 'enums'):
mode_map = mavutil.mavlink.enums.get('COPTER_MODE', {})
self.mode = mode_map.get(msg.custom_mode, {}).get('name', str(msg.custom_mode))
def source_workspace():
"""Source the ArduPilot workspace."""
ardu_ws = os.path.expanduser("~/ardu_ws")
setup_bash = os.path.join(ardu_ws, "install", "setup.bash")
elif msg_type == "LOCAL_POSITION_NED":
self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
self.velocity = {"x": msg.vx, "y": msg.vy, "z": msg.vz}
self.altitude = -msg.z # NED: down is positive
self.vertical_velocity = -msg.vz
if os.path.exists(setup_bash):
# Update environment by sourcing the workspace
# This is done by running commands in a sourced shell
return True
return False
def get_launch_command(world: str, vehicle: str) -> list:
"""Get the appropriate launch command."""
launch_files = {
# Copter configurations
'runway': ('ardupilot_gz_bringup', 'iris_runway.launch.py'),
'maze': ('ardupilot_gz_bringup', 'iris_maze.launch.py'),
'iris': ('ardupilot_gz_bringup', 'iris_runway.launch.py'),
# Rover configurations
'rover': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'),
'wildthumper': ('ardupilot_gz_bringup', 'wildthumper_playpen.launch.py'),
# SITL only (no Gazebo)
'sitl': ('ardupilot_sitl', 'sitl_dds_udp.launch.py'),
elif msg_type == "ATTITUDE":
self.attitude = {
"roll": msg.roll,
"pitch": msg.pitch,
"yaw": msg.yaw
}
if vehicle == 'rover':
key = 'rover'
else:
key = world if world in launch_files else 'runway'
def get_telemetry(self):
"""Get telemetry in the same format as standalone simulation."""
self.update_telemetry()
package, launch_file = launch_files.get(key, launch_files['runway'])
return {
"imu": {
"orientation": self.attitude,
"angular_velocity": {"x": 0, "y": 0, "z": 0} # Not available via MAVLink easily
},
"altimeter": {
"altitude": self.altitude,
"vertical_velocity": self.vertical_velocity
},
"velocity": self.velocity,
"landing_pad": None, # Not available in ArduPilot mode - use vision
"position": self.position, # Extra: actual position (GPS-like)
"armed": self.armed,
"mode": self.mode
}
cmd = ['ros2', 'launch', package, launch_file]
# Add SITL-specific parameters if using sitl_dds_udp
if launch_file == 'sitl_dds_udp.launch.py':
cmd.extend([
'transport:=udp4',
'synthetic_clock:=True',
'model:=quad' if vehicle == 'copter' else 'rover',
'speedup:=1',
])
return cmd
def launch_mavproxy(master_port: int = 14550):
"""Launch MAVProxy in a new terminal."""
mavproxy_cmd = f"mavproxy.py --console --map --master=:{master_port}"
# Try different terminal emulators
terminals = [
['gnome-terminal', '--', 'bash', '-c', mavproxy_cmd],
['xterm', '-e', mavproxy_cmd],
['konsole', '-e', mavproxy_cmd],
]
for term_cmd in terminals:
try:
subprocess.Popen(term_cmd)
return True
except FileNotFoundError:
continue
print(f"[WARN] Could not open terminal for MAVProxy")
print(f"[INFO] Run manually: {mavproxy_cmd}")
def set_mode(self, mode_name):
"""Set flight mode."""
mode_map = self.mav.mode_mapping()
if mode_name.upper() not in mode_map:
print(f"[WARN] Unknown mode: {mode_name}")
return False
mode_id = mode_map[mode_name.upper()]
self.mav.set_mode(mode_id)
print(f"[OK] Mode set to {mode_name}")
return True
def parse_args():
parser = argparse.ArgumentParser(
description='Launch ArduPilot SITL with ROS 2 and Gazebo',
formatter_class=argparse.RawDescriptionHelpFormatter,
epilog="""
Examples:
python run_ardupilot.py # Iris on runway
python run_ardupilot.py --world maze # Iris in maze
python run_ardupilot.py --vehicle rover # WildThumper rover
python run_ardupilot.py --mavproxy # With MAVProxy
def arm(self, force=True):
"""Arm motors."""
if force:
# Force arm (bypass checks)
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0, 1, 21196, 0, 0, 0, 0, 0 # 21196 = force arm
)
else:
self.mav.arducopter_arm()
Available worlds: runway, maze, sitl (no Gazebo)
Available vehicles: copter, rover
time.sleep(1)
self.update_telemetry()
if self.armed:
print("[OK] Armed")
return self.armed
def disarm(self):
"""Disarm motors."""
self.mav.arducopter_disarm()
print("[OK] Disarmed")
def takeoff(self, altitude=5.0):
"""Takeoff to specified altitude."""
self.mav.mav.command_long_send(
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, 0, 0, 0, 0, 0, 0, altitude
)
print(f"[OK] Takeoff to {altitude}m")
def send_velocity(self, vx, vy, vz, yaw_rate=0):
"""Send velocity command in body frame.
Args:
vx: Forward velocity (m/s)
vy: Right velocity (m/s)
vz: Down velocity (m/s, positive = descend)
yaw_rate: Yaw rate (rad/s)
"""
# Type mask: use velocity only
type_mask = (
mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE
)
parser.add_argument(
'--world', '-w', type=str, default='runway',
choices=['runway', 'maze', 'sitl'],
help='Simulation world (default: runway)'
self.mav.mav.set_position_target_local_ned_send(
0, # time_boot_ms
self.mav.target_system,
self.mav.target_component,
mavutil.mavlink.MAV_FRAME_BODY_NED,
type_mask,
0, 0, 0, # position (ignored)
vx, vy, vz, # velocity
0, 0, 0, # acceleration (ignored)
0, yaw_rate # yaw, yaw_rate
)
parser.add_argument(
'--vehicle', '-v', type=str, default='copter',
choices=['copter', 'rover'],
help='Vehicle type (default: copter)'
)
parser.add_argument(
'--mavproxy', '-m', action='store_true',
help='Also launch MAVProxy in a new terminal'
)
parser.add_argument(
'--mavproxy-port', type=int, default=14550,
help='MAVProxy master port (default: 14550)'
)
return parser.parse_args()
def main():
args = parse_args()
parser = argparse.ArgumentParser(description="ArduPilot Drone Controller")
parser.add_argument("--connection", "-c", default=None,
help="MAVLink connection string (default: from config.py)")
parser.add_argument("--takeoff-alt", "-a", type=float, default=5.0,
help="Takeoff altitude (meters)")
parser.add_argument("--no-takeoff", action="store_true",
help="Don't auto takeoff, just control")
args = parser.parse_args()
print("=" * 60)
print(" ArduPilot SITL + Gazebo (Official ROS 2 DDS)")
print("=" * 60)
print()
# Create ArduPilot interface
ardupilot = ArduPilotInterface(args.connection)
# Check ROS 2
if not check_ros2():
print("[ERROR] ROS 2 not found!")
print("Please source ROS 2:")
print(" source /opt/ros/humble/setup.bash")
return 1
# Connect
if not ardupilot.connect():
print("[ERROR] Could not connect to ArduPilot")
print("Make sure sim_vehicle.py is running")
sys.exit(1)
print("[OK] ROS 2 available")
# Create drone controller
controller = DroneController()
# Check ArduPilot packages
if not check_ardupilot_packages():
print("[ERROR] ArduPilot ROS 2 packages not found!")
print()
print("Please install ArduPilot ROS 2:")
print(" ./setup/install_ardupilot.sh")
print()
print("Then source the workspace:")
print(" source ~/ardu_ws/install/setup.bash")
return 1
print("\n" + "=" * 50)
print(" ArduPilot GPS-Denied Controller")
print("=" * 50)
print(f"Mode: {ardupilot.mode}")
print(f"Armed: {ardupilot.armed}")
print("")
print("[OK] ArduPilot ROS 2 packages found")
if not args.no_takeoff:
# Set GUIDED mode and arm
print("[INFO] Setting up for takeoff...")
ardupilot.set_mode("GUIDED")
time.sleep(1)
# Get launch command
launch_cmd = get_launch_command(args.world, args.vehicle)
ardupilot.arm(force=True)
time.sleep(1)
print()
print(f"World: {args.world}")
print(f"Vehicle: {args.vehicle}")
print(f"Launch: {' '.join(launch_cmd)}")
print()
if ardupilot.armed:
ardupilot.takeoff(args.takeoff_alt)
print(f"[INFO] Taking off to {args.takeoff_alt}m...")
# Launch MAVProxy if requested
if args.mavproxy:
print("[INFO] Starting MAVProxy...")
# Delay to let SITL start first
time.sleep(2)
launch_mavproxy(args.mavproxy_port)
# Wait for takeoff
for _ in range(50): # 5 seconds
telemetry = ardupilot.get_telemetry()
if telemetry["altimeter"]["altitude"] > args.takeoff_alt * 0.9:
print("[OK] Reached target altitude")
break
time.sleep(0.1)
print("Starting simulation...")
print("Press Ctrl+C to stop.")
print()
print("-" * 60)
print("\n[INFO] Starting controller loop...")
print("[INFO] Press Ctrl+C to stop\n")
# Handle Ctrl+C gracefully
def signal_handler(sig, frame):
print("\nShutting down...")
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# Run the launch command
rate = 20 # Hz
try:
subprocess.run(launch_cmd, check=True)
except subprocess.CalledProcessError as e:
print(f"[ERROR] Launch failed: {e}")
return 1
while True:
# Get telemetry
telemetry = ardupilot.get_telemetry()
# Run your controller
thrust, pitch, roll, yaw = controller.calculate_landing_maneuver(
telemetry,
rover_telemetry=None # No rover in ArduPilot mode yet
)
# Convert control outputs to velocities
# thrust -> vertical velocity (negative = up)
# pitch -> forward velocity
# roll -> right velocity
vz = -thrust * 2.0 # Scale factor
vx = pitch * 2.0
vy = roll * 2.0
yaw_rate = yaw * 1.0
# Send velocity command
ardupilot.send_velocity(vx, vy, vz, yaw_rate)
# Print status
alt = telemetry["altimeter"]["altitude"]
mode = telemetry.get("mode", "?")
print(f"\rAlt: {alt:5.1f}m | Mode: {mode:10} | Cmd: vx={vx:+.2f} vy={vy:+.2f} vz={vz:+.2f}", end="")
time.sleep(1.0 / rate)
except KeyboardInterrupt:
print("\nShutdown complete.")
return 0
print("\n\n[INFO] Stopping...")
ardupilot.set_mode("LAND")
print("[OK] Landing mode set")
if __name__ == '__main__':
sys.exit(main())
if __name__ == "__main__":
main()

61
scripts/run_ardupilot_controller.sh Executable file
View File

@@ -0,0 +1,61 @@
#!/bin/bash
# =============================================================================
# ArduPilot Controller Launcher
# =============================================================================
# Starts ArduPilot SITL and your drone controller together.
#
# Usage:
# Terminal 1: ./scripts/run_ardupilot_sim.sh runway
# Terminal 2: ./scripts/run_ardupilot_controller.sh
# =============================================================================
set -e
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
echo "=============================================="
echo " ArduPilot Controller"
echo "=============================================="
echo ""
# Check dependencies
if ! command -v sim_vehicle.py &> /dev/null; then
echo "[ERROR] sim_vehicle.py not found"
echo "Add to PATH: export PATH=\$PATH:~/ardupilot/Tools/autotest"
exit 1
fi
# Activate virtual environment if exists
if [ -f "$PROJECT_DIR/venv/bin/activate" ]; then
source "$PROJECT_DIR/venv/bin/activate"
fi
# Start SITL in background
echo "[INFO] Starting ArduPilot SITL..."
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy &
SITL_PID=$!
# Wait for SITL to start
sleep 5
# Check if SITL started
if ! kill -0 $SITL_PID 2>/dev/null; then
echo "[ERROR] SITL failed to start"
exit 1
fi
echo "[OK] SITL started (PID: $SITL_PID)"
echo ""
# Trap to kill SITL on exit
trap "echo ''; echo '[INFO] Stopping SITL...'; kill $SITL_PID 2>/dev/null; exit 0" INT TERM
# Run controller
echo "[INFO] Starting drone controller..."
echo ""
cd "$PROJECT_DIR"
python run_ardupilot.py "$@"
# Cleanup
kill $SITL_PID 2>/dev/null