Code reorganization and Drone Logic Update
This commit is contained in:
287
scripts/run_ardupilot.py
Normal file
287
scripts/run_ardupilot.py
Normal file
@@ -0,0 +1,287 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
ArduPilot Drone Controller
|
||||
==========================
|
||||
Connects your drone_controller.py logic to ArduPilot SITL via MAVLink.
|
||||
|
||||
Usage:
|
||||
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
|
||||
|
||||
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 sys
|
||||
import os
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
import time
|
||||
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 src.drone_controller import DroneController
|
||||
|
||||
|
||||
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:
|
||||
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 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
|
||||
|
||||
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))
|
||||
|
||||
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
|
||||
|
||||
elif msg_type == "ATTITUDE":
|
||||
self.attitude = {
|
||||
"roll": msg.roll,
|
||||
"pitch": msg.pitch,
|
||||
"yaw": msg.yaw
|
||||
}
|
||||
|
||||
def get_telemetry(self):
|
||||
"""Get telemetry in the same format as standalone simulation."""
|
||||
self.update_telemetry()
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
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 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()
|
||||
|
||||
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
|
||||
)
|
||||
|
||||
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
|
||||
)
|
||||
|
||||
|
||||
def main():
|
||||
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()
|
||||
|
||||
# Create ArduPilot interface
|
||||
ardupilot = ArduPilotInterface(args.connection)
|
||||
|
||||
# Connect
|
||||
if not ardupilot.connect():
|
||||
print("[ERROR] Could not connect to ArduPilot")
|
||||
print("Make sure sim_vehicle.py is running")
|
||||
sys.exit(1)
|
||||
|
||||
# Create drone controller
|
||||
controller = DroneController()
|
||||
|
||||
print("\n" + "=" * 50)
|
||||
print(" ArduPilot GPS-Denied Controller")
|
||||
print("=" * 50)
|
||||
print(f"Mode: {ardupilot.mode}")
|
||||
print(f"Armed: {ardupilot.armed}")
|
||||
print("")
|
||||
|
||||
if not args.no_takeoff:
|
||||
# Set GUIDED mode and arm
|
||||
print("[INFO] Setting up for takeoff...")
|
||||
ardupilot.set_mode("GUIDED")
|
||||
time.sleep(1)
|
||||
|
||||
ardupilot.arm(force=True)
|
||||
time.sleep(1)
|
||||
|
||||
if ardupilot.armed:
|
||||
ardupilot.takeoff(args.takeoff_alt)
|
||||
print(f"[INFO] Taking off to {args.takeoff_alt}m...")
|
||||
|
||||
# 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("\n[INFO] Starting controller loop...")
|
||||
print("[INFO] Press Ctrl+C to stop\n")
|
||||
|
||||
rate = 20 # Hz
|
||||
try:
|
||||
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("\n\n[INFO] Stopping...")
|
||||
ardupilot.set_mode("LAND")
|
||||
print("[OK] Landing mode set")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -113,7 +113,6 @@ fi
|
||||
# =============================================================================
|
||||
WORLD_ARG="${1:-runway}"
|
||||
|
||||
# Map friendly names to actual world files
|
||||
case "$WORLD_ARG" in
|
||||
runway|iris|default)
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf"
|
||||
@@ -130,10 +129,16 @@ case "$WORLD_ARG" in
|
||||
parachute)
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_parachute.sdf"
|
||||
;;
|
||||
custom)
|
||||
WORLD="${PROJECT_DIR}/gazebo/worlds/custom_landing.sdf"
|
||||
;;
|
||||
*)
|
||||
# Try as full path or filename
|
||||
if [ -f "$WORLD_ARG" ]; then
|
||||
WORLD="$WORLD_ARG"
|
||||
elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}" ]; then
|
||||
WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}"
|
||||
elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf" ]; then
|
||||
WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf"
|
||||
elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}" ]; then
|
||||
WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}"
|
||||
elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf" ]; then
|
||||
@@ -147,12 +152,13 @@ esac
|
||||
if [ -z "$WORLD" ] || [ ! -f "$WORLD" ]; then
|
||||
echo "[ERROR] World not found: $WORLD_ARG"
|
||||
echo ""
|
||||
echo "Available options:"
|
||||
echo " runway - Iris drone on runway (default)"
|
||||
echo "Built-in worlds:"
|
||||
echo " runway - Iris on runway (default)"
|
||||
echo " warehouse - Iris in warehouse"
|
||||
echo " gimbal - Gimbal test"
|
||||
echo " zephyr - Zephyr plane"
|
||||
echo " parachute - Parachute test"
|
||||
echo " custom - Custom landing pad"
|
||||
echo ""
|
||||
echo "Local worlds in gazebo/worlds/:"
|
||||
ls -1 "${PROJECT_DIR}/gazebo/worlds/"*.sdf 2>/dev/null | xargs -n1 basename || echo " (none)"
|
||||
echo ""
|
||||
echo "Or specify full path to .sdf file"
|
||||
exit 1
|
||||
|
||||
Reference in New Issue
Block a user