def main(connection_string, target_altitude): """Takes the drone up and then lands.""" # Setup logging logger = logging.getLogger('control') logger.setLevel(logging.DEBUG) filehandler = logging.FileHandler('simpletakeoff.log') filehandler.setLevel(logging.DEBUG) console = logging.StreamHandler() console.setLevel(logging.DEBUG) formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s') filehandler.setFormatter(formatter) console.setFormatter(formatter) logger.addHandler(filehandler) logger.addHandler(console) # Connect to flight controller logger.debug("Starting program, attempting connection to flight controller.") vehicle_control = Controller(connection_string, baud=57600) if not SIMULATION: time.sleep(60) # Sleep to avoid immediate takeoff on boot # Arm and takeoff logger.debug("Arming...") vehicle_control.arm() vehicle_control.takeoff(target_altitude) # Give time for user controller to takeover (if you desire to test controller takeover) logger.debug("Hovering for 30 seconds...") for second in range(30): vehicle_control.check_geofence(10, target_altitude + 10) if vehicle_control.vehicle.mode.name != "GUIDED": break time.sleep(1) # Land if still in guided mode (i.e. no user takeover, no flight controller failure) if vehicle_control.vehicle.mode.name == "GUIDED": logger.debug("Landing...") vehicle_control.land() # Always keep the programming running and logging until the vehicle is disarmed while vehicle_control.vehicle.armed: vehicle_control.log_flight_info() time.sleep(3) logger.debug("Finshed program.") sys.exit(0)
def main(target_altitude, radius): """Takes the drone up and then lands.""" # Setup logging logger = logging.getLogger('control') logger.setLevel(logging.DEBUG) filehandler = logging.FileHandler('simpleflight.log') filehandler.setLevel(logging.DEBUG) console = logging.StreamHandler() console.setLevel(logging.DEBUG) formatter = logging.Formatter( '%(asctime)s - %(name)s - %(levelname)s - %(message)s') filehandler.setFormatter(formatter) console.setFormatter(formatter) logger.addHandler(filehandler) logger.addHandler(console) # Connect to the drone logger.debug( "Starting program, attempting connection to flight controller.") vehicle_control = Controller(CONNECTION_STRING, baud=57600) # Sleep to avoid immediate takeoff on boot if not SIMULATION: time.sleep(60) # Arm and takeoff logger.debug("Arming...") vehicle_control.arm() vehicle_control.takeoff(target_altitude) # Create points home_gps = location_global_relative_to_gps_reading(vehicle_control.home) north_gps = get_location_offset(home_gps, radius, 0) south_gps = get_location_offset(home_gps, -1 * radius, 0) east_gps = get_location_offset(home_gps, 0, radius) west_gps = get_location_offset(home_gps, 0, -1 * radius) points = [north_gps, south_gps, east_gps, west_gps, home_gps] # Transform GpsReading to LocationGlobalRelative for index, point in enumerate(points): points[index] = gps_reading_to_location_global(point) logger.debug("Destination {}: {}".format(index, points[index])) # Go to the points if vehicle_control.vehicle.mode.name == "GUIDED": logger.debug("Flying to points...") for point in points: if vehicle_control.vehicle.mode.name != "GUIDED": break vehicle_control.goto(point) # Wait for vehicle to reach destination before updating the point for sleep_time in range(10): if vehicle_control.vehicle.mode.name != "GUIDED": break vehicle_control.log_flight_info(point) # Don't let the vehicle go too far (could be stricter if get_distance # improved and if gps was more accurate. Also note that altitude # is looser here to avoid false landings (since gps altitude not # accurate at all). vehicle_control.check_geofence(radius * 2, target_altitude + 20) current = vehicle_control.vehicle.location.global_relative_frame current_reading = location_global_relative_to_gps_reading( current) point_reading = location_global_relative_to_gps_reading(point) distance = get_distance(current_reading, point_reading) if distance < 1: logger.debug('Destination Reached') time.sleep(3) break time.sleep(3) # Land if still in guided mode (i.e. no user takeover, no flight controller failure) if vehicle_control.vehicle.mode.name == "GUIDED": logger.debug("Landing...") vehicle_control.land() # Always keep the programming running and logging until the vehicle is disarmed while vehicle_control.vehicle.armed: vehicle_control.log_flight_info() time.sleep(3) logger.debug("Finshed program.") sys.exit(0)
def main(): """Takes the drone up and then lands.""" # Setup logging logger = logging.getLogger('control') logger.setLevel(logging.DEBUG) filehandler = logging.FileHandler('main.log') filehandler.setLevel(logging.DEBUG) console = logging.StreamHandler() console.setLevel(logging.DEBUG) formatter = logging.Formatter( '%(asctime)s - %(name)s - %(levelname)s - %(message)s') filehandler.setFormatter(formatter) console.setFormatter(formatter) logger.addHandler(filehandler) logger.addHandler(console) # Connect to xBee com = Communication(COM_CONNECTION_STRING, 0.1) logger.debug("Connected to wireless communication receiver") com.send(u"Connected to wireless communication receiver") # Connect to i2c data server data_client = I2cDataClient("tcp://localhost:5555") if not data_client.read(): logger.critical("Can't connect to zmq data server") com.send(u"Can't connect to zmq data server") sys.exit(1) # Connect to the drone logger.debug( "Starting program, attempting connection to flight controller.") com.send(u"Starting program, attempting connection to flight controller") vehicle_control = None try: vehicle_control = Controller(PIXHAWK_CONNECTION_STRING, baud=57600, com=com) logger.debug("Connected to flight controller") com.send(u"Connected to flight controller") except dronekit.APIException as err: logger.critical("dronekit.APIException: {}".format(err)) logger.critical("Could not connect to flight controller.") com.send(u"Could not connect to flight controller.") sys.exit(1) # Wait until the waypoints flight path is received from GCS logger.debug("Waiting to receive flight path from GCS") com.send(u"Waiting to receive flight path from GCS") waypoints = com.receive() while not waypoints: waypoints = com.receive() time.sleep(1) # Create points start_location = vehicle_control.vehicle.location.global_relative_frame points = create_waypoints(logger, com, start_location, waypoints) if not points: logger.critical("Invalid points received from GCS") com.send(u"Invalid points received from GCS") sys.exit(1) # Arm and takeoff vehicle_control.arm() vehicle_control.takeoff(10) points.append(vehicle_control.home) # Log points for index, point in enumerate(points): logger.debug("Destination {}: {}".format(index, point)) # Go to the points flight_start_time = time.time() if vehicle_control.vehicle.mode.name == "GUIDED": logger.debug("Flying to points...") for point in points: com.send(u"Destination: {}".format(point)) if vehicle_control.vehicle.mode.name != "GUIDED": com.send(u"Mode no longer guided") break vehicle_control.goto(point) # Wait for vehicle to reach destination before updating the point for sleep_time in range(60): if vehicle_control.vehicle.mode.name != "GUIDED": com.send(u"Mode no longer guided") break vehicle_control.log_flight_info(point) data_for_gcs = package_data( vehicle_control.home, vehicle_control.vehicle.location.global_relative_frame, data_client, time.time() - flight_start_time) com.send(data_for_gcs) # Don't let the vehicle go too far (could be stricter if get_distance # improved and if gps was more accurate. Also note that altitude # is looser here to avoid false landings (since gps altitude not # accurate at all). vehicle_control.check_geofence(MAX_RADIUS + 10, MAX_ALTITUDE + 10) if is_destination_reached(logger, com, vehicle_control, point): break time.sleep(1) # Land if still in guided mode (i.e. no user takeover, no flight controller failure) if vehicle_control.vehicle.mode.name == "GUIDED": vehicle_control.land() # Always keep the programming running and logging until the vehicle is disarmed while vehicle_control.vehicle.armed: vehicle_control.log_flight_info() time.sleep(1) # Program end logger.debug("Finished program.") com.send("Finished program.") sys.exit(0)