FlyingFall2020/Ashley_flying_test.py at main · SAREC-Lab/FlyingFall2020 · GitHub

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
137
138
139
140
141
142
143
144
145
146
147
# Import DroneKit-Python
from dronekit import connect, VehicleMode, time
import math

#Setup option parsing to get connection string
import argparse
parser = argparse.ArgumentParser(description='Print out vehicle state information')
parser.add_argument('--connect',help="vehicle connection target string.")
args=parser.parse_args()

connection_string = args.connect

# Connect to the Vehicle.
print "\nConnecting to vehicle on: %s" % connection_string
vehicle = connect(connection_string, wait_ready=False)
vehicle.wait_ready(timeout=120)

#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.system_status.state
print "Mode: %s" % vehicle.mode.name # settable

def arm_and_takeoff(aTargetAltitude):
    """
    Arms vehicle and fly to aTargetAltitude.
    """

    print("Basic pre-arm checks")
    print("Arming motors")
    vehicle.mode = VehicleMode("GUIDED")
    vehicle.armed = True
    print("Vehicle armed!")

    #Takeoff
    print("Taking off!")
    vehicle.simple_takeoff(aTargetAltitude)  # Take off to target altitude
    lat = vehicle.location.global_relative_frame.lat
    lon = vehicle.location.global_relative_frame.lon
    alt = vehicle.location.global_relative_frame.alt
    print('Current location after takeoff is: {0},{1},{2}'.format(lat,lon,alt))

    # Wait until the vehicle reaches a safe height before processing the goto
    #  (otherwise the command after Vehicle.simple_takeoff will execute immediately).
    while True:
        print(" Altitude: ", vehicle.location.global_relative_frame.alt)
        # Break and return from function just below target altitude.
        if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
            print("Reached target altitude")
            break
        time.sleep(1)

arm_and_takeoff(10)

####################################
# YOUR CODE GOES HERE
####################################

# Check that you are using proper GOTO commands that are
# executed in a loop with the following logic:
#def my_goto_function(target):
#    print the target coordinates so we know what is happening
#    simple goto command
#    while loop must include while vehicle.mode="GUIDED"
#        check distance to target
#        if close to target
#            break
#
# You can call your function for each waypoint
# SANITY CHECK that you don't go farther than 20 meters from your starting waypoint.
# I'm somehow against anyone trying to fly to Australia during our flight tests!

def newLoc_meters(loc, ndist, edist):
    R = 6378137.0 #Radius of earth
    addLat = ndist/R
    addLon = edist/(R*math.cos(math.pi*loc.lat/180))
    newLat = loc.lat + (addLat * 180/math.pi)
    newLon = loc.lon + (addLon * 180/math.pi)
    return LocationGlobalRelative(newLat, newLon, loc.alt)

def goto(ndist, edist):
    targetLoc = newLoc_meters(vehicle.location.global_relative_frame, ndist, edist)
    vehicle.simple_goto(targetLoc)
    return targetLoc

def get_distance_meters(loc1, loc2):
    diffLat = loc2.lat - loc1.lat
    diffLon = loc2.lon - loc2.lon
    return math.sqrt((diffLat*diffLat) + (diffLon*diffLon)) * 1.1131915e5

def travel(targetLoc, currLoc):
    while vehicle.mode.name == 'GUIDED':
        if get_distance_meters(targetLoc, currLoc) < 1:
            break
        time.sleep(1)
    currLat = currLoc.lat
    currLon = currLoc.lon
    print("   Current Lat: " + str(currLat) + ", Lon: " + str(currLon))

print("Set default/target airspeed to 3")
vehicle.airspeed=3

initLoc = vehicle.location.global_relative_frame

print("Hover for 5 sec")
time.sleep(5)
currLat = vehicle.location.global_relative_frame.lat
currLon = vehicle.location.global_relative_frame.lon
print("  Current Lat: " + str(currLat) + ", Lon: " + str(currLon))

print("Fly NW for 20 meters")
targetLoc = goto(20/math.sqrt(2), -20/math.sqrt(2)) #gives diagonal NW of 20m
travel(targetLoc, vehicle.location.global_relative_frame)

print("Increase altitude to 15 meters")
point = LocationGlobalRelative(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, 15)
vehicle.simple_goto(point)
travel(point, vehicle.location.global_relative_frame)

print("Fly East for 20 meters")
targetLoc = goto(0, 20) # straight E 20 m
travel(targetLoc, vehicle.locaiton.global_relative_frame)

print("Fly back to original position")
point = LocationGlobalRelative(initLoc.lat, initLoc.lon, 15)
vehicle.simple_goto(point)
travel(point, vehicle.location.global_relative_frame)


####################################
# Add this at the end
####################################
print("Returning to Launch")
vehicle.mode = VehicleMode("LAND")  # Note we replace RTL with LAND

# Close vehicle object before exiting script
print("Close vehicle object")
vehicle.close()

# Close vehicle object before exiting script
vehicle.close()

time.sleep(5)

print("Completed")