def rtl(self):
self.vehicle.mode = dronekit.VehicleMode("RTL")
print("returned to land!!")
except exceptions.OSError as e:
print 'No serial exists!'
except dronekit.APIException:
print 'Timeout!'
except:
print 'Some other error!'
# Pre-arm checks
while not markut.is_armable:
print "Waiting for Markut to initialize.."
time.sleep(1)
print "Arming Motors"
markut.mode = dronekit.VehicleMode("GUIDED")
markut.armed = True
while not markut.mode.name == "GUIDED" and not markut.armed:
print "Getting ready to take off.."
time.sleep(1)
print "Taking off!"
markut.simple_takeoff(args.altitude)
while True:
markut_current_altitude = markut.location.global_relative_frame.alt
print " Altitude: ", markut_current_altitude
if markut_current_altitude >= args.altitude * 0.95:
print "Reached target altitude."
break
def rtl(self):
self.trace("rtl")
self.vehicle.mode = dronekit.VehicleMode("RTL")
def do_VTOL_land(vehicle, land_position):
if vehicle.mode == dronekit.VehicleMode('GUIDED'):
return True
else:
return False
baudrate = 57600
port = '/dev/ttyS%s'%conn_number #If using hardware serial. HW serial has issues relating to GPU throttling (google it), use with caution
port = '/dev/ttyACM%s'%conn_number #If using USB. Safe but physically clunky.
port = 'udp:localhost:14551'
print 'Connecting to %s at %s baud'%(port, baudrate)
vehicle = dk.connect(port, wait_ready=True, baud=baudrate)
print('Connected!')
print('Battery voltage is %f V'%vehicle.battery.voltage)
fly_sm = sm.Turn(vehicle)#sm.Takeoff(vehicle)
cam_sm = sm.CameraHandler()
main_state = 0
main_timer = 0
l_q = Logger(filename=None, folder='quad_coords')
l_c = Logger(filename=None, folder='car_coords')
vehicle.mode = dk.VehicleMode('POSHOLD')
vehicle.armed = True
while vehicle.armed != True:
print 'waiting to arm\r',
print vehicle.home_location
u.set_home(vehicle)
while (main_state != 1000) & (cam_sm.state != -1):
cam_sm.run()
positions = u.get_relative_target_coords(vehicle, cam_sm.centers)
global_pos = u.get_global_target_coords(vehicle, cam_sm.centers)
N = vehicle.location.local_frame.north
E = vehicle.location.local_frame.east
D = vehicle.location.local_frame.down
def issue(self,
connection: dronekit.Vehicle,
*,
timeout: float = 30.0) -> None:
"""Issues this mission to a given vehicle.
Parameters
----------
connection: dronekit.Vehicle
A connection to the vehicle.
timeout: float
A timeout that is enforced on the process of preparing the vehicle
and issuing this mission.
Raises
------
TimeoutError
If the timeout occurs before the mission has been issued to the
vehicle.
"""
timer = Stopwatch()
timer.start()
def time_left() -> float:
return max(0.0, timeout - timer.duration)
logger.debug("waiting for vehicle to be armable")
while not connection.is_armable:
if timer.duration > timeout:
raise TimeoutError
time.sleep(0.05)
# set home location
logger.debug("waiting for home location")
while not connection.home_location:
if timer.duration > timeout:
raise TimeoutError
vcmds = connection.commands
vcmds.download()
try:
vcmds.wait_ready(timeout=time_left())
except dronekit.TimeoutError:
raise TimeoutError
time.sleep(0.1)
logger.debug("determined home location: %s", connection.home_location)
logger.debug("attempting to arm vehicle")
connection.armed = True
while not connection.armed:
if timer.duration > timeout:
raise TimeoutError
time.sleep(0.05)
logger.debug("armed vehicle")
# upload mission
vcmds = connection.commands
vcmds.clear()
for command in self:
vcmds.add(command)
vcmds.upload()
try:
vcmds.wait_ready(timeout=time_left())
except dronekit.TimeoutError:
raise TimeoutError
# start mission
# FIXME check that mode was changed!
logger.debug("switching to AUTO mode")
connection.mode = dronekit.VehicleMode('AUTO')
logger.debug("switched to AUTO mode")
message = connection.message_factory.command_long_encode(
0, 0, 300, 0, 1,
len(self) + 1, 0, 0, 0, 0, 4)
connection.send_mavlink(message)
def main(path_to_config, ardupath=None):
if ardupath is not None:
global ARDUPATH
ARDUPATH = ardupath
global DO_CONT
DO_CONT = True
config = load_json(path_to_config)
dronology = util.Connection()
dronology.start()
# A list of sitl instances.
sitls = []
# A list of drones. (dronekit.Vehicle)
vehicles = []
# A list of lists of lists (i.e., [ [ [lat0, lon0, alt0], ...] ...]
# These are the waypoints each drone must go to!
routes = []
# Example:
# vehicle0 = vehicles[0]
# waypoints_for_vehicle0 = routes[0]
# for waypoint in waypoints_for_vehicle0:
# lat, lon, alt = waypoint
# vehicle0.simple_goto(lat, lon, alt)
# The above example obviously won't work... you'll need to write some code to figure out when the current waypoint
# has been reached and it's time to go to the next waypoint.
# Define the shutdown behavior
def stop(*args):
global DO_CONT
DO_CONT = False
w0.join()
for v, sitl in zip(vehicles, sitls):
v.close()
sitl.stop()
dronology.stop()
signal.signal(signal.SIGINT, stop)
signal.signal(signal.SIGTERM, stop)
# Start up all the drones specified in the json configuration file
for i, v_config in enumerate(config):
home = v_config['start']
vehicle, sitl = connect_vehicle(i, home)
handshake = util.DroneHandshakeMessage.from_vehicle(
vehicle, get_vehicle_id(i))
dronology.send(str(handshake))
sitls.append(sitl)
vehicles.append(vehicle)
routes.append(v_config['waypoints'])
# Create a thread for sending the state of drones back to Dronology
w0 = threading.Thread(target=state_out_work, args=(dronology, vehicles))
# Start the thread.
w0.start()
# At this point, all of the "behind the scenes stuff" has been set up.
# It's time to write some code that:
# 1. Starts up the drones (set the mode to guided, arm, takeoff)
for vehicle in vehicles:
print("Starting vehicle {}".format(vehicle))
starting_altitude = 20 # unsure about this number
print("Basic pre-arm checks")
while not vehicle.is_armable:
print(" Waiting for vehicle to initialise...")
time.sleep(1)
print("Arming motors")
# Copter should arm in GUIDED mode
vehicle.mode = dronekit.VehicleMode("GUIDED")
vehicle.armed = True
# Confirm vehicle armed before attempting to take off
while not vehicle.armed:
print(" Waiting for arming...")
time.sleep(1)
print("Taking off!")
vehicle.simple_takeoff(
starting_altitude) # Take off to target altitude
# 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 >= starting_altitude * 0.95:
print("Reached target altitude")
break
time.sleep(1)
# end the loop above (for v in vehicles)
# For each set of waypoints
for waypointindex in range(0, 5):
sort_by_height(vehicles, routes, waypointindex)
# Send each drone to it next waypoint
for vindex in range(len(vehicles)):
lat, lon, alt = routes[vindex][waypointindex]
waypoint = LocationGlobalRelative(
lat, lon, vehicles[vindex].location.global_relative_frame.alt)
vehicles[vindex].simple_goto(waypoint, groundspeed=10)
# Check if collisions occur
for i, vehicle in enumerate(vehicles):
v1_curr = (vehicle.location.global_relative_frame.lat,
vehicle.location.global_relative_frame.lon)
v1_goto = (routes[i][waypointindex][0],
routes[i][waypointindex][1])
v1 = (v1_curr, v1_goto)
target_altitude = routes[i][waypointindex][2]
for j, colliding_vehicle in enumerate(vehicles):
v2_curr = (
colliding_vehicle.location.global_relative_frame.lat,
colliding_vehicle.location.global_relative_frame.lon)
v2_goto = (routes[j][waypointindex][0],
routes[j][waypointindex][1])
v2 = (v2_curr, v2_goto)
if not will_collide(v1, v2):
print "Descending / ascending to target alt"
go_to_altitude(target_altitude, vehicle)
else:
if get_distance_meters(v1_curr[0], v1_curr[1], v2_curr[0],
v2_curr[1]) > 20:
print "Descending / ascending if not too close"
go_to_altitude(target_altitude, vehicle)
else:
print "Waiting for drone to pass"
time.sleep(20.0)
go_to_altitude(target_altitude, vehicle)
# wait until ctrl c to exit
while DO_CONT:
time.sleep(5.0)
def land(self):
self._executor.cancel()
self.vehicle.mode = dronekit.VehicleMode("Land")
def rtl(self):
self._executor.cancel()
self.vehicle.mode = dronekit.VehicleMode("RTL")
def run(self):
t = time.time()
if self.state == 0:
print 'Waiting to arm...'
self.vehicle.mode = dk.VehicleMode(self.mode)
print 'Mode: %s' % self.vehicle.mode.name
self.vehicle.armed = True
self.state = 1
elif self.state == 1:
#print 'Mode: %s, Armed = %s\r'%(self.vehicle.mode.name, self.vehicle.armed)
if (self.vehicle.armed == True
and self.vehicle.mode.name == self.mode):
print 'Armed!'
self.state = 100
self.start_timer = t
elif self.state == 100:
if t - self.start_timer > 3: #Let drone arm for 3 seconds
self.state = 2 #Then go to next state
elif self.state == 2:
print 'running simple takeoff'
self.vehicle.simple_takeoff(1.4)
self.start_timer = t
self.state = 3
elif self.state == 3:
if t - self.start_timer > 3: #Let drone take off for 3 seconds
self.state = 4 #Then go to next state
elif self.state == 4:
self.log = Logger() # Init logger
self.log.write_line('N\tE\tD\tY\n')
self.start_timer = t
self.state = 5
elif self.state == 5:
yaw = self.vehicle.attitude.yaw
loc = self.vehicle.location.local_frame
h = self.vehicle.rangefinder.distance
#print("Coordinates: %.3f N, %.3f E, %.3f D Yaw: %3f\r"%(loc.north, loc.east, h ,yaw))
self.log.write_line('%s\t%s\t%s\t%s\n' %
(loc.north, loc.east, h, yaw))
if t - self.start_timer > 10: #Fly for x secs
self.state = 6
self.log.close()
elif self.state == 6:
#Mode is set after the check in order to guarantee that there is
# one cycle between setting and checking.
print 'Landing!'
if self.vehicle.mode == dk.VehicleMode('LAND'):
self.state = 7
self.start_timer = t
self.vehicle.mode = dk.VehicleMode('LAND')
elif self.state == 7:
if t - self.start_timer > 5:
if self.vehicle.mode == dk.VehicleMode(self.mode):
print 'Reset to mode: %s\r' % self.vehicle.mode.name
self.state = 8
self.vehicle.mode = dk.VehicleMode(self.mode)
elif self.state == 8:
print 'Disarming\r'
self.vehicle.armed = False
if self.vehicle.armed == False:
self.state = 1000
print 'Done.'
elif self.state == 1000:
print 'Program finished!'
vehicle.close()
return self.state
def client_handler(inbound_socket, addr, vehicle):
"""
Handle a single client connection.
Basically a big loop that polls the connection supplied for single bytes
and responds to them.
Inputs:
inbound_socket: socket returned by the servers accept() method.
addr: IP to client. Currently unused.
vehicle: dronekit vehicle refering to the drone.
Returns:
None. This method is just a loop, which excepts out when disconnected.
"""
print(inbound_socket)
client_connection = inbound_socket.makefile('rwb')
RC_utils.set_home(vehicle, 57.704996, 11.963963, 0)
while True:
command = RC_utils.get_command(client_connection)
if command != b'':
print(command)
sys.stdout.flush()
if command == b'w':
RC_utils.override_interval(vehicle, 'pitch', 1450, 0.1)
if command == b's':
RC_utils.override_interval(vehicle, 'pitch', 1550, 0.1)
if command == b'a':
RC_utils.override_interval(vehicle, 'roll', 1450, 0.1)
if command == b'd':
RC_utils.override_interval(vehicle, 'roll', 1550, 0.1)
elif command == b'b':
vehicle.armed = True
elif command == b'n':
vehicle.armed = False
elif command == b'm':
mode = RC_utils.get_data(client_connection, 's')[0]
vehicle.mode = dk.VehicleMode(mode)
elif command == b'p':
pos = RC_utils.get_data(client_connection, 'f')
RC_utils.send_ned_target(vehicle, n=pos[0], e=pos[1])
elif command == b't':
height = RC_utils.get_data(client_connection, 'f')[0]
print('Takeoff to %s m' % height)
sys.stdout.flush()
vehicle.simple_takeoff(height)
elif command == b'y':
ang = RC_utils.get_data(client_connection, 'f')[0]
RC_utils.simple_yaw_rel(vehicle, ang)
elif command == b'x':
val, dur = RC_utils.get_data(client_connection, 'f')
print('Got yaw val %s and yaw time %s' % (val, dur))
RC_utils.override_interval(vehicle, 'yaw', 1500 + val, dur)
elif command == b'v':
val, dur = RC_utils.get_data(client_connection, 'f')
print('Got pitch val %s and pitch time %s' % (val, dur))
RC_utils.override_interval(vehicle, 'pitch', 1500 + val, dur)
elif command == b'r':
RC_utils.simple_yaw_rel(vehicle, 10)
elif command == b'l':
RC_utils.simple_yaw_rel(vehicle, -10)
elif command == b'o':
pos = (vehicle.location.local_frame.north,
vehicle.location.local_frame.east,
vehicle.location.local_frame.down)
RC_utils.send_data(client_connection, pos)
elif command == b'c':
att = (vehicle.attitude.roll, vehicle.attitude.pitch,
vehicle.attitude.yaw)
RC_utils.send_data(client_connection, att)
elif command == b'q':
freq, dur = RC_utils.get_data(client_connection, 'f')
beep(dur, freq)
'''except:
print('Error: %s'%sys.exc_info()[0])
print('Error: %s'%sys.exc_info()[1])
print('Error: %s'%sys.exc_info()[2])
client_connection.close()
inbound_socket.close()'''
return 0
def run(self):
t = time.time()
if self.state == 0:
print 'Waiting to arm...'
self.vehicle.mode = dk.VehicleMode(self.mode)
print 'Mode: %s' % self.vehicle.mode.name
self.vehicle.armed = True
self.state = 1
elif self.state == 1:
#print 'Mode: %s, Armed = %s\r'%(self.vehicle.mode.name, self.vehicle.armed)
if (self.vehicle.armed == True
and self.vehicle.mode.name == self.mode):
print 'Armed!'
self.state = 100
self.start_timer = t
elif self.state == 100:
if t - self.start_timer > 3: #Let drone arm for 3 seconds
self.state = 2 #Then go to next state
elif self.state == 2:
print 'running simple takeoff'
self.vehicle.simple_takeoff(1)
self.start_timer = t
self.state = 3
elif self.state == 3:
if t - self.start_timer > 3: #Let drone take off for 3 seconds
self.state = 4 #Then go to next state
self.vehicle.mode = dk.VehicleMode('GUIDED')
elif self.state == 4:
if self.vehicle.mode.name == 'GUIDED':
self.log = Logger() # Init logger
self.log.write_line('N\tE\tD\tY\n')
self.start_timer = t
print 'Sent rotate command!'
u.condition_yaw(self.vehicle,
heading=-90,
rate=20,
relative=False)
self.state = 401
elif self.state == 401:
print dir(self.vehicle.location)
yaw = self.vehicle.attitude.yaw
loc = self.vehicle.location.local_frame
h = self.vehicle.rangefinder.distance
self.log.write_line('%s\t%s\t%s\t%s\n' %
(loc.north, loc.east, h, yaw))
if t - self.start_timer > 5: #rotate for 5 secs
self.state = 402
elif self.state == 402:
if self.vehicle.mode.name == 'GUIDED':
self.log = Logger() # Init logger
self.log.write_line('N\tE\tD\tY\n')
self.start_timer = t
print 'Sent rotate command!'
u.condition_yaw(self.vehicle,
heading=90,
rate=20,
relative=False)
self.state = 403
elif self.state == 403:
yaw = self.vehicle.attitude.yaw
loc = self.vehicle.location.local_frame
h = self.vehicle.rangefinder.distance
self.log.write_line('%s\t%s\t%s\t%s\n' %
(loc.north, loc.east, h, yaw))
if t - self.start_timer > 10: #rotate for 10 secs
self.state = 6
self.log.close()
elif self.state == 6:
#Mode is set after the check in order to guarantee that there is
# one cycle between setting and checking.
print 'Landing!'
if self.vehicle.mode == dk.VehicleMode('LAND'):
self.state = 7
self.start_timer = t
self.vehicle.mode = dk.VehicleMode('LAND')
elif self.state == 7:
if t - self.start_timer > 5:
if self.vehicle.mode == dk.VehicleMode(self.mode):
print 'Reset to mode: %s\r' % self.vehicle.mode.name
self.state = 8
self.vehicle.mode = dk.VehicleMode(self.mode)
elif self.state == 8:
print 'Disarming\r'
self.vehicle.armed = False
if self.vehicle.armed == False:
self.state = 1000
print 'Done.'
elif self.state == 1000:
print 'Program finished!'
vehicle.close()
return self.state
rover_location.get_lon())
print("Latitude from Drop: " + str(lat_from_drop))
print("Longitude from Drop: " + str(lon_from_drop) + "\n")
if (rover_location.get_alt() < 10 and rover_location.get_alt() > -1
and lat_from_drop < 0.00035 and lon_from_drop < 0.00035):
return True
else:
return False
def airdrop_countdown(wait):
print("Rover Has Reached AirDrop")
for x in range(wait):
print("Rover Arming in: " + str(wait - x) + " seconds")
sleep(1)
rover = connect_to_rover()
landed = False
while not landed:
if (has_rover_reached_airdrop(rover)):
landed = True
airdrop_countdown(120) #Change wait period (In seconds) here
rover.armed = True
print("Rover is Armed")
rover.mode = dronekit.VehicleMode("AUTO")
print("Rover Mode: AUTO")
def main():
target = "udpin:0.0.0.0:14550"
vehicle = dronekit.connect(target)
vehicle.mode = dronekit.VehicleMode("LOITER")
vehicle.close()
if counter == 0:
detected = 1
print("FIRE")
if cv2.countNonZero(gray) == 0:
counter = 10
cv2.imshow('ROI', frame)
# cv2.imshow('mask', mask)
# cv2.imshow('res', res)
if cv2.waitKey(1) & 0xFF == ord('q'):
print("Exiting...")
break
cv2.waitKey(int((1 / int(fps)) * 1000))
# STOP Flying --------------------------------
send_ned_velocity(0, 0, 0) # stop the vehicle
#sleepNrecord(2)
time.sleep(3) #for 3 seconds
# READ CURRENT COORDINATES FROM PIXHAWK-------------------
lat = vehicle.location.global_relative_frame.lat # get the current latitude
lon = vehicle.location.global_relative_frame.lon # get the current longitude
coords = str(lat) + " " + str(lon)
# TRANSMIT CURRENT COORDINATES TO RESCUE DR --------------
ser.write(coords.encode())
# RETURN HOME CODE ----------------------------
vehicle.mode = VehicleMode("RTL")
#time.sleep(20)
# ---------------------------------------------
vehicle.mode = dk.VehicleMode("LAND")
vehicle.flush()
def set_mode(self, mode):
self._vehicle.mode = dronekit.VehicleMode(mode)
while not self._vehicle.mode.name == mode:
logger.debug('Waiting for mode change to %s...', mode)
time.sleep(1)
def set_mode(self, mode):
self.conn.mode = dronekit.VehicleMode(mode)
time.sleep(1)
return self.conn.mode.name
def main(path_to_config, ardupath=None):
if ardupath is not None:
global ARDUPATH
ARDUPATH = ardupath
global DO_CONT
DO_CONT = True
config = load_json(path_to_config)
dronology = util.Connection()
dronology.start()
# A list of sitl instances.
sitls = []
# A list of drones. (dronekit.Vehicle)
vehicles = []
# A list of lists of lists (i.e., [ [ [lat0, lon0, alt0], ...] ...]
# These are the waypoints each drone must go to!
routes = []
# Example:
# vehicle0 = vehicles[0]
# waypoints_for_vehicle0 = routes[0]
# for waypoint in waypoints_for_vehicle0:
# lat, lon, alt = waypoint
# vehicle0.simple_goto(lat, lon, alt)
# The above example obviously won't work... you'll need to write some code to figure out when the current waypoint
# has been reached and it's time to go to the next waypoint.
# Define the shutdown behavior
def stop(*args):
global DO_CONT
DO_CONT = False
w0.join()
for v, sitl in zip(vehicles, sitls):
v.close()
sitl.stop()
dronology.stop()
signal.signal(signal.SIGINT, stop)
signal.signal(signal.SIGTERM, stop)
# Start up all the drones specified in the json configuration file
for i, v_config in enumerate(config):
home = v_config['start']
vehicle, sitl = connect_vehicle(i, home)
handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, get_vehicle_id(i))
dronology.send(str(handshake))
sitls.append(sitl)
vehicles.append(vehicle)
routes.append(v_config['waypoints'])
# Create a thread for sending the state of drones back to Dronology
w0 = threading.Thread(target=state_out_work, args=(dronology, vehicles))
# Start the thread.
w0.start()
server_addr = '127.0.0.1'
port = '5000'
# handle lead drone
print("Starting Lead Drone")
print("Pre-arm checks")
while not vehicles[0].is_armable:
print("Waiting for drone to initialize...")
time.sleep(1)
'''
print("Arming motors")
vehicles[0].mode = dronekit.VehicleMode("GUIDED")
vehicles[0].armed = True
while not vehicle.armed:
print("Waiting to arm...")
time.sleep(1)
'''
# Connect lead drone to server
print("Connecting to server as Lead")
wypts = routes[0]
wypts = {'waypoints': wypts}
r = requests.post('http://' + server_addr + ':' + port + '/createLeadDrone', data=json.dumps(wypts))
if r.status_code == 200:
resp = json.loads(r.content)
else:
print('Error: {} Response'.format(r.status_code))
# handle second+ drones
# random starting point from an example file
home = [
41.714867454724,
-86.242300802635,
0]
vehicle, sitl = connect_vehicle(1, home)
handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, get_vehicle_id(1))
dronology.send(str(handshake))
sitls.append(sitl)
vehicles.append(vehicle)
print("Starting Auxillary Drone 1")
print("Pre-arm checks")
while not vehicles[1].is_armable:
print("Waiting for drone to initialize...")
time.sleep(1)
print("Arming motors 0")
vehicles[0].mode = dronekit.VehicleMode("GUIDED")
vehicles[0].armed = True
while not vehicles[0].armed:
print("Waiting to arm...")
time.sleep(1)
print("Arming motors 1")
print("Inside a loop")
vehicles[1].mode = dronekit.VehicleMode("GUIDED")
vehicles[1].armed = True
while not vehicles[1].armed:
print("Waiting to arm...")
time.sleep(1)
print("Connecting as aux drone 1")
r = requests.get('http://' + server_addr + ':' + port + '/computeStraightLinePath')
if r.status_code == 200:
resp = json.loads(r.content)
wypts1 = resp['waypoints']#dict(resp['waypoints'])
routes.append(wypts1)
else:
print('Error: {} Response'.format(r.status_code))
# setup should be complete...
# takeoff
counter = 0
for vehicle in vehicles:
starting_altitude = 10
vehicle.simple_takeoff(starting_altitude)
# check to make sure we're at a safe height before raising the other ones
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 >= starting_altitude * 0.95:
print("Reached target altitude for vehicle {}".format(counter))
break
time.sleep(1)
counter += 1
# lots of hardcoded stuff, need to change
# base it off lead drone waypoints
for i, wypts0 in enumerate(routes[0]):
print routes[1]
loc_0 = dronekit.LocationGlobal(wypts0[0], wypts0[1], 10)
if i > 4:
break
loc_1 = dronekit.LocationGlobal(routes[1][i*6][0], routes[1][i*6][1], 10)
vehicles[0].simple_goto(loc_0, groundspeed=10)
vehicles[1].simple_goto(loc_1, groundspeed=10)
for j in range(1,5):
location = dronekit.LocationGlobal(routes[1][j+i*6][0], routes[1][j+i*6][1], 10)
vehicles[1].simple_goto(location, groundspeed=10)
# wait until ctrl c to exit
while DO_CONT:
time.sleep(5.0)
print("Slave connecté: "+str(slave.version))
print("Connection au master....")
master = dronekit.connect("/dev/ttyACM0",baud=56700) #Le master est relié directement en USB
time.sleep(3)
print("Master connecté: "+str(master.version))
print("On attend un peu pour tout récupérer.")
time.sleep(2)
rep = input("Armer le slave (Yes/No)")
if(rep=="Y" or rep=="y"):
slave.arm()
slave.mode = dronekit.VehicleMode("HOLD")
print("Slave armé et en mode HOLD")
rep = input("SimpleTakeOff? (Alt or N)")
if(rep=="N" or rep =="n"):
pass
else:
slave.mode = dronekit.VehicleMode("GUIDED")
slave.simple_takeoff(int(rep))
time.sleep(5)
rep = input("Armer le master (Yes/No)")
if(rep=="Y" or rep=="y"):
master.arm()
master.mode = dronekit.VehicleMode("MANUAL")
print("Master armé et en mode HOLD")
rep = input("SimpleTakeOff? (Alt or N)")
import dronekit
import time
import math
master = dronekit.connect("/dev/ttyACM0")
print("wait 2s")
time.sleep(2)
print(master.version)
master.mode = dronekit.VehicleMode("GUIDED")
chk1 = dronekit.LocationGlobalRelative(48.5791376, 7.7657461, 30)
chk2 = dronekit.LocationGlobalRelative(48.5790596, 7.7665722, 30)
print("wait 5s")
time.sleep(5)
master.simple_goto(chk1, groundspeed=2)
print("go at 2m/s to ", chk1)
for i in range(0, 50):
print(master.velocity)
print(
math.sqrt(
math.pow(master.velocity[0], 2) + math.pow(master.velocity[1], 2) +
math.pow(master.velocity[2], 2)), str("m/s"))
time.sleep(1)
print("gone")
master.simple_goto(chk2, groundspeed=4.0)
print("go at 4m/s to ", chk2)
time.sleep(50)
master.simple_goto(chk1, groundspeed=0.75)
print("go at 0.75m/s to ", chk1)
time.sleep(50)
print("fin de chantier")
vehicle.add_attribute_listener('attitude', attitude_callback) #-- message type, callback function
time.sleep(5)
#--- Now we print the attitude from the callback for 15 seconds, then we remove the callback
vehicle.remove_attribute_listener('attitude', attitude_callback) #(.remove)
while not vehicle.is_armable:
print("waiting to be armable")
time.sleep(1)
# Set vehicle mode
desired_mode = 'ALT_HOLD'
while vehicle.mode != desired_mode:
vehicle.mode = dronekit.VehicleMode(desired_mode)
time.sleep(0.5)
while not vehicle.armed:
print("Arming motors")
vehicle.armed = True
time.sleep(0.5)
timer = 0
while timer <= 10:
vehicle.channels.overrides[3] = 1000
print " Ch3 override: %s" % vehicle.channels.overrides[3]
time.sleep(0.5)
timer += 0.5
def control_loop(self):
TARGET_TAKEOFF_ALT = 3.0
last_loop = 0
# Regular loop.
while self.should_run:
last_loop = self.phased_loop(10, last_loop)
new_state = None
current_state = self.get_state()
# Log any change in state.
# self.print_debug("Drone state: " + current_state)
if current_state != "STANDBY" and self.check_velocity_control():
self.set_state("FAILSAFE")
current_state = "FAILSAFE"
# Drone state machine.
if current_state == "STANDBY":
pass
elif current_state == "ARM":
self.set_state("ARM WAIT FOR INIT")
elif current_state == "ARM WAIT FOR INIT":
if self.vehicle.is_armable:
self.set_state("ARMING")
elif current_state == "ARMING":
self.vehicle.mode = dronekit.VehicleMode("GUIDED")
self.vehicle.armed = True
self.set_state("ARMED CHECK")
elif current_state == "ARMED CHECK":
if self.vehicle.armed:
self.set_state("ARMED")
elif current_state == "ARMED":
# Wait for other drones to also be armed.
self.vehicle.mode = dronekit.VehicleMode("GUIDED")
self.vehicle.armed = True
pass
elif current_state == "TAKEOFF":
self.vehicle.mode = dronekit.VehicleMode("GUIDED")
self.armed = True
self.set_state("TAKEOFF SEND COMMAND")
elif current_state == "TAKEOFF SEND COMMAND":
self.vehicle.mode = dronekit.VehicleMode("GUIDED")
self.armed = True
self.vehicle.simple_takeoff(TARGET_TAKEOFF_ALT)
self.set_state("TAKEOFF WAIT FOR TARGET ALTITUDE")
elif current_state == "TAKEOFF WAIT FOR TARGET ALTITUDE":
self.vehicle.mode = dronekit.VehicleMode("GUIDED")
self.armed = True
if self.vehicle.location.global_relative_frame.alt \
>= TARGET_TAKEOFF_ALT * 0.85:
self.set_state("TAKEN OFF")
else:
pass
#self.print_debug("Altitude: " +
# str(self.vehicle.location.global_relative_frame.alt))
elif current_state == "TAKEN OFF":
# Wait for other drones to take off.
self.vehicle.mode = dronekit.VehicleMode("GUIDED")
self.armed = True
pass
elif current_state == "VELOCITY_CONTROL":
self.vehicle.mode = dronekit.VehicleMode("GUIDED")
self.vehicle.armed = True
self.send_velocity()
elif current_state == "LAND":
# Command vehicle to land.
self.vehicle.mode = dronekit.VehicleMode("LAND")
self.set_state("LANDING")
elif current_state == "LANDING":
# Wait until vehicle disarms after landing.
if self.vehicle.armed:
pass
#self.print_debug("Altitude: " +
# str(self.vehicle.location.global_relative_frame.alt))
else:
self.set_state("LANDED")
elif current_state == "LANDED":
# Wait for all other drones to land.
pass
elif current_state == "FAILSAFE":
# Exit control loop and go into failsafe mode.
break
else:
self.print_debug("UNKNOWN CONTROLLER STATE: " + current_state)
self.set_state("FAILSAFE")
# Failsafe mode (cannot escape without restarting drone_interface).
self.print_debug("FAILSAFE")
while True:
# If we go into failsafe mode, continuously tell the flight
# controller to land.
self.vehicle.mode = dronekit.VehicleMode("LAND")
time.sleep(0.1)
def go_to(vehicle, location):
if vehicle.mode == dronekit.VehicleMode('GUIDED'):
vehicle.simple_goto(location)
return True
else:
return False
def start(self,
binary_name: str,
model_name: str,
param_file: str,
verbose: bool = True) -> None:
"""
Launches the SITL inside this sandbox, and establishes a connection to
the vehicle running inside the simulation. Blocks until SITL is
launched and a connection is established.
Raises:
NoConnectionError: if a connection cannot be established.
ConnectionLostError: if the connecton is lost before the vehicle
is ready to receive commands.
PostConnectionSetupFailed: if the post-connection setup phase
failed.
VehicleNotReadyError: if a timeout occurred before the vehicle was
ready to accept commands.
"""
stopwatch = Stopwatch()
speedup = self.configuration.speedup
timeout_set_mode = (15 / speedup + 2) + 30
timeout_3d_fix = (10 / speedup + 2) + 30
timeout_state = (90 / speedup + 2) + 30
timeout_mavlink = 60
bzc = self._bugzoo.containers
args = (binary_name, model_name, param_file, verbose)
self.__sitl_thread = threading.Thread(target=self._launch_sitl,
args=args)
self.__sitl_thread.daemon = True
self.__sitl_thread.start()
# establish connection
protocol = 'tcp'
port = 5760
ip = str(bzc.ip_address(self.container))
url = "{}:{}:{}".format(protocol, ip, port)
logger.debug("connecting to SITL at %s", url)
try:
self.__connection = MAVLinkConnection(url, {'update': self.update},
timeout=timeout_mavlink)
except dronekit.APIException:
raise NoConnectionError
# wait for longitude and latitude to match their expected values, and
# for the system to match the expected `armable` state.
initial_lon = self.state_initial['longitude']
initial_lat = self.state_initial['latitude']
initial_armable = self.state_initial['armable']
v = self.state_initial.__class__.variables
# FIXME wait for 3D fix
time.sleep(timeout_3d_fix)
stopwatch.reset()
stopwatch.start()
while True:
ready_lon = v['longitude'].eq(initial_lon, self.state['longitude'])
ready_lat = v['latitude'].eq(initial_lat, self.state['latitude'])
ready_armable = self.state['armable'] == initial_armable
if ready_lon and ready_lat and ready_armable:
break
if stopwatch.duration > timeout_state:
logger.error("latitude should be [%f] but was [%f]",
initial_lat, self.state['latitude'])
logger.error("longitude should be [%f] but was [%f]",
initial_lon, self.state['longitude'])
logger.error("armable should be [%s] but was [%s]",
initial_armable, self.state['armable'])
raise VehicleNotReadyError
time.sleep(0.05)
if not self._on_connected():
raise PostConnectionSetupFailed
# wait until the vehicle is in GUIDED mode
guided_mode = dronekit.VehicleMode('GUIDED')
self.vehicle.mode = guided_mode
stopwatch.reset()
stopwatch.start()
while self.vehicle.mode != guided_mode:
if stopwatch.duration > timeout_set_mode:
logger.error('vehicle is not in guided mode')
raise VehicleNotReadyError
time.sleep(0.05)
def land(self):
self.vehicle.mode = dronekit.VehicleMode("LAND")
def run_and_trace(self,
commands: Sequence[Command],
collect_coverage: bool = False) -> 'MissionTrace':
"""
Executes a mission, represented as a sequence of commands, and
returns a description of the outcome.
Parameters:
commands: the list of commands to be sent to the robot as
a mission.
collect_coverage: indicates whether or not coverage information
should be incorporated into the trace. If True (i.e., coverage
collection is enabled), this function expects the sandbox to be
properly instrumented.
Returns:
a trace describing the execution of a sequence of commands.
"""
config = self.configuration
env = self.environment
speedup = config.speedup
timeout_command = 300 / speedup + 5
timeout_arm = 10 / speedup + 5
timeout_mission_upload = 20
# the number of seconds for the delay added after DO commands
do_delay = max(4, int(20 / speedup))
with self.__lock:
outcomes = [] # type: List[CommandOutcome]
passed = True
connection_lost = threading.Event()
# FIXME The initial command should be based on initial state
initial = dronekit.Command(0, 0, 0, 0, 16, 0, 0, 0.0, 0.0, 0.0,
0.0, -35.3632607, 149.1652351, 584)
# delay to allow the robot to reach its stable state
delay = dronekit.Command(0, 0, 0, 3, 93, 0, 0, do_delay, -1, -1,
-1, 0, 0, 0)
# converting from Houston commands to dronekit commands
dronekitcmd_to_cmd_mapping = {}
cmds = [initial]
for i, cmd in enumerate(commands):
dronekitcmd_to_cmd_mapping[len(cmds)] = i
cmds.append(cmd.to_message().to_dronekit_command())
# DO commands trigger some action and return.
# we add a delay after them to see how they affect the state.
if 'MAV_CMD_DO_' in cmd.__class__.uid:
dronekitcmd_to_cmd_mapping[len(cmds)] = i
cmds.append(delay)
logger.debug("Final mission commands len: %d, mapping: %s",
len(cmds), dronekitcmd_to_cmd_mapping)
# uploading the mission to the vehicle
vcmds = self.vehicle.commands
vcmds.clear()
for cmd in cmds:
vcmds.add(cmd)
vcmds.upload(timeout=timeout_mission_upload)
logger.debug("Mission uploaded")
vcmds.wait_ready()
# maps each wp to the final state and time when wp was reached
wp_to_state = {} # Dict[int, Tuple[State, float]]
# [wp that has last been reached, wp running at the moment]
last_wp = [0, 0]
# used to synchronize rw to last_wp
wp_lock = threading.Lock()
# is set whenever a command in mission is done
wp_event = threading.Event()
# NOTE dronekit connection must not use its own heartbeat checking
def heartbeat_listener(_, __, value):
if value > TIME_LOST_CONNECTION:
connection_lost.set()
wp_event.set()
self.vehicle.add_attribute_listener('last_heartbeat',
heartbeat_listener)
def check_for_reached(m):
name = m.name
message = m.message
if name == 'MISSION_ITEM_REACHED':
logger.debug("**MISSION_ITEM_REACHED: %d", message.seq)
if message.seq == len(cmds) - 1:
logger.info("Last item reached")
with wp_lock:
last_wp[1] = int(message.seq) + 1
wp_event.set()
elif name == 'MISSION_CURRENT':
logger.debug("**MISSION_CURRENT: %d", message.seq)
logger.debug("STATE: {}".format(self.state))
if message.seq > last_wp[1]:
with wp_lock:
if message.seq > last_wp[0]:
last_wp[1] = message.seq
logger.debug("SET EVENT")
wp_event.set()
elif name == 'MISSION_ACK':
logger.debug("**MISSION_ACK: %s", message.type)
self.connection.add_hooks({'check_for_reached': check_for_reached})
stopwatch = Stopwatch()
stopwatch.start()
self.vehicle.armed = True
while not self.vehicle.armed:
if stopwatch.duration >= timeout_arm:
raise VehicleNotReadyError
logger.debug("waiting for the rover to be armed...")
self.vehicle.armed = True
time.sleep(0.1)
# starting the mission
self.vehicle.mode = dronekit.VehicleMode("AUTO")
initial_state = self.state
start_message = CommandLong(0, 0, 300, 0, 1,
len(cmds) + 1, 0, 0, 0, 0, 4)
self.connection.send(start_message)
logger.debug("sent mission start message to vehicle")
time_start = timer()
wp_to_traces = {}
with self.record() as recorder:
while last_wp[0] <= len(cmds) - 1:
logger.debug("waiting for command")
not_reached_timeout = wp_event.wait(timeout_command)
logger.debug("Event set %s", last_wp)
if not not_reached_timeout:
logger.error("Timeout occured %d", last_wp[0])
break
if connection_lost.is_set():
logger.error("Connection to vehicle was lost.")
raise ConnectionLostError
with wp_lock:
# self.observe()
logger.info("last_wp: %s len: %d", str(last_wp),
len(cmds))
logger.debug("STATE: {}".format(self.state))
current_time = timer()
time_passed = current_time - time_start
time_start = current_time
states, messages = recorder.flush()
if last_wp[0] > 0:
cmd_index = dronekitcmd_to_cmd_mapping[last_wp[0]]
wp_to_state[cmd_index] = (self.state, time_passed)
cmd = commands[cmd_index]
trace = CommandTrace(cmd, states)
wp_to_traces[cmd_index] = trace
last_wp[0] = last_wp[1]
wp_event.clear()
self.connection.remove_hook('check_for_reached')
logger.debug("Removed hook")
coverage = None
if collect_coverage:
# if appropriate, store coverage files
self.__flush_coverage()
coverage = self.__get_coverage()
traces = [wp_to_traces[k] for k in sorted(wp_to_traces.keys())]
return MissionTrace(tuple(traces), coverage)
def land(self):
self.trace("land")
# http://python.dronekit.io/examples/guided-set-speed-yaw-demo.html
self.vehicle.groundspeed = 0
self.vehicle.mode = dronekit.VehicleMode("LAND")
return True
try:
conn_number = int(sys.argv[1])
except:
conn_number = 0
baudrate = 57600
port = '/dev/ttyS%s' % conn_number #If using hardware serial. HW serial has issues relating to GPU throttling (google it), use with caution
port = '/dev/ttyACM%s' % conn_number #If using USB. Safe but physically clunky.
print 'Connecting to %s at %s baud' % (port, baudrate)
vehicle = dk.connect(port, wait_ready=True, baud=baudrate)
print('Connected!')
print('Battery voltage is %f V' % vehicle.battery.voltage)
print 'Waiting to arm...'
mode = 'POSHOLD'
vehicle.mode = dk.VehicleMode(mode)
print 'Mode: %s' % vehicle.mode.name
vehicle.armed = True
time.sleep(0.1)
while (vehicle.armed == False and vehicle.mode.name != mode):
print 'Mode: %s, Armed = %s\r' % (vehicle.mode.name, vehicle.armed)
time.sleep(0.2)
print 'Armed!'
time.sleep(2)
print 'running simple takeoff'
vehicle.simple_takeoff(1)
log = Logger()
log.write_line('N\tE\tD\tY\n')
0, # body pitch rate in radian
yaw_rate, # body yaw rate in radian
thrust) # thrust
copter.send_mavlink(msg)
def to_quaternion(self, roll=0.0, pitch=0.0, yaw=0.0):
"""Convert degrees to quaternions."""
t0 = math.cos(yaw * 0.5)
t1 = math.sin(yaw * 0.5)
t2 = math.cos(roll * 0.5)
t3 = math.sin(roll * 0.5)
t4 = math.cos(pitch * 0.5)
t5 = math.sin(pitch * 0.5)
w = t0 * t2 * t4 + t1 * t3 * t5
x = t0 * t3 * t4 - t1 * t2 * t5
y = t0 * t2 * t5 + t1 * t3 * t4
z = t1 * t2 * t4 - t0 * t3 * t5
return [w, x, y, z]
while True:
if(copter and copter.mode == dronekit.VehicleMode("GUIDED")):
send_attitude_target(0, # roll
0, # pitch
0, # yaw_angle
0, # yaw_rate
0.5) # thrust
print("attitude command sent")
time.sleep(0.1)
dronekit_logger = logging.getLogger("autopilot")
dronekit_logger.disabled = True
# connect to vehicle
vehicle = dronekit.connect('/dev/ttyACM0', wait_ready=True, baud=921600)
print_log("Connection established.")
time.sleep(1)
print_status(vehicle)
try:
# arm the vehicle
print_log("Trying to arm vehicle...")
vehicle.arm(timeout=5)
# set to guided mode first
vehicle.mode = dronekit.VehicleMode("GUIDED")
# take off
take_off(vehicle, 20)
# set velocity
# fly_to_location(vehicle, 31.0246941285, 121.4266920090)
send_ned_velocity(vehicle, (5, 0, 0), 2)
send_ned_velocity(vehicle, (-5, 0, 0), 2)
send_ned_velocity(vehicle, (0, 5, 0), 2)
send_ned_velocity(vehicle, (0, -5, 0), 2)
send_ned_velocity(vehicle, (0, 0, 5), 2)
send_ned_velocity(vehicle, (0, 0, -5), 2)
finally:
# wait for 2 seconds and then disarm the vehicle
