Spaces:
Running
Running
File size: 5,037 Bytes
bd61f34 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 |
#!/usr/bin/env python3
"""
DroneKit-Python Mission Test Script
This script demonstrates how to connect to the ArduPilot SITL simulator,
create a simple mission (square pattern), and execute it.
"""
import time
import math
import sys
import argparse
from drone import compatibility_fix # Import the compatibility fix for Python 3.10+
from drone.drone_control import DroneController
from dronekit import connect, VehicleMode, LocationGlobalRelative
def get_args():
"""Parse command line arguments."""
parser = argparse.ArgumentParser(description='Test mission script for DeepDrone')
parser.add_argument('--connect',
help="Vehicle connection target string. If not specified, SITL automatically started.",
default='udp:127.0.0.1:14550')
return parser.parse_args()
def main():
# Parse connection string
args = get_args()
connection_string = args.connect
print(f"Connecting to vehicle on: {connection_string}")
try:
# Connect to the Vehicle
print("Connecting to vehicle on %s" % connection_string)
vehicle = connect(connection_string, wait_ready=True, timeout=60)
# Get some vehicle attributes (state)
print("Get some vehicle attribute values:")
print(" GPS: %s" % vehicle.gps_0)
print(" Battery: %s" % vehicle.battery)
print(" Last Heartbeat: %s" % vehicle.last_heartbeat)
print(" Is Armable?: %s" % vehicle.is_armable)
print(" System status: %s" % vehicle.system_status.state)
print(" Mode: %s" % vehicle.mode.name)
# Define the home position (current position where script is run)
home_position = vehicle.location.global_relative_frame
print("Home position: %s" % home_position)
# Define a square mission
offset = 0.0001 # Approximately 11 meters at the equator
# Create a mission with a square pattern
print("Creating mission waypoints...")
waypoints = [
# North
LocationGlobalRelative(home_position.lat + offset, home_position.lon, 20),
# Northeast
LocationGlobalRelative(home_position.lat + offset, home_position.lon + offset, 20),
# East
LocationGlobalRelative(home_position.lat, home_position.lon + offset, 20),
# Return to Launch
LocationGlobalRelative(home_position.lat, home_position.lon, 20),
]
# Arm the vehicle
print("Arming motors")
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
# Wait until armed
while not vehicle.armed:
print("Waiting for arming...")
time.sleep(1)
print("Taking off to 20 meters")
vehicle.simple_takeoff(20) # Take off to 20m
# Wait until the vehicle reaches a safe height
while True:
print("Altitude: %s" % vehicle.location.global_relative_frame.alt)
# Break and return when we reach target altitude or close to it
if vehicle.location.global_relative_frame.alt >= 19:
print("Reached target altitude")
break
time.sleep(1)
# Fly through the waypoints
for i, waypoint in enumerate(waypoints):
print(f"Flying to waypoint {i+1}")
vehicle.simple_goto(waypoint)
# Start timer for waypoint timeout
start_time = time.time()
# Wait until we reach the waypoint
while True:
# Calculate distance to waypoint
current = vehicle.location.global_relative_frame
distance_to_waypoint = math.sqrt(
(waypoint.lat - current.lat)**2 +
(waypoint.lon - current.lon)**2) * 1.113195e5
print(f"Distance to waypoint: {distance_to_waypoint:.2f} meters")
# Break if we're within 3 meters of the waypoint
if distance_to_waypoint < 3:
print(f"Reached waypoint {i+1}")
break
# Break if we've been flying to this waypoint for more than 60 seconds
if time.time() - start_time > 60:
print(f"Timed out reaching waypoint {i+1}, continuing to next waypoint")
break
time.sleep(2)
# Return to home
print("Returning to home")
vehicle.mode = VehicleMode("RTL")
# Wait for the vehicle to land
while vehicle.armed:
print("Waiting for landing and disarm...")
time.sleep(2)
print("Mission complete!")
# Close vehicle object
vehicle.close()
except Exception as e:
print(f"Error: {str(e)}")
if __name__ == "__main__":
main() |