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)
