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