From aeb0f7a72511494b7f3d7f2a979d29e7a003d564 Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Sun, 11 Sep 2016 12:58:54 +0200 Subject: [PATCH 1/9] Added attribute listener on 'commands.next', so you can get notified when a waypoint is reached and the drone starts heading for the next one. --- dronekit/__init__.py | 5 +- .../mission_basic.py => mission_basic.py | 440 +++++++++--------- 2 files changed, 228 insertions(+), 217 deletions(-) rename examples/mission_basic/mission_basic.py => mission_basic.py (87%) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 9e6f77b1d..ce08b1be1 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1148,7 +1148,10 @@ def listener(self, name, m): @self.on_message(['WAYPOINT_CURRENT', 'MISSION_CURRENT']) def listener(self, name, m): - self._current_waypoint = m.seq + if m.seq != self._current_waypoint: + self._current_waypoint = m.seq + self.notify_attribute_listeners('commands.next',self._current_waypoint) + self._ekf_poshorizabs = False self._ekf_constposmode = False diff --git a/examples/mission_basic/mission_basic.py b/mission_basic.py similarity index 87% rename from examples/mission_basic/mission_basic.py rename to mission_basic.py index 4255ec25c..3d81c599b 100644 --- a/examples/mission_basic/mission_basic.py +++ b/mission_basic.py @@ -1,216 +1,224 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -""" -© Copyright 2015-2016, 3D Robotics. -mission_basic.py: Example demonstrating basic mission operations including creating, clearing and monitoring missions. - -Full documentation is provided at http://python.dronekit.io/examples/mission_basic.html -""" - -from dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal, Command -import time -import math -from pymavlink import mavutil - - -#Set up option parsing to get connection string -import argparse -parser = argparse.ArgumentParser(description='Demonstrates basic mission operations.') -parser.add_argument('--connect', - help="vehicle connection target string. If not specified, SITL automatically started and used.") -args = parser.parse_args() - -connection_string = args.connect -sitl = None - - -#Start SITL if no connection string specified -if not connection_string: - import dronekit_sitl - sitl = dronekit_sitl.start_default() - connection_string = sitl.connection_string() - - -# Connect to the Vehicle -print 'Connecting to vehicle on: %s' % connection_string -vehicle = connect(connection_string, wait_ready=True) - - -def get_location_metres(original_location, dNorth, dEast): - """ - Returns a LocationGlobal object containing the latitude/longitude `dNorth` and `dEast` metres from the - specified `original_location`. The returned Location has the same `alt` value - as `original_location`. - - The function is useful when you want to move the vehicle around specifying locations relative to - the current vehicle position. - The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles. - For more information see: - http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters - """ - earth_radius=6378137.0 #Radius of "spherical" earth - #Coordinate offsets in radians - dLat = dNorth/earth_radius - dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180)) - - #New position in decimal degrees - newlat = original_location.lat + (dLat * 180/math.pi) - newlon = original_location.lon + (dLon * 180/math.pi) - return LocationGlobal(newlat, newlon,original_location.alt) - - -def get_distance_metres(aLocation1, aLocation2): - """ - Returns the ground distance in metres between two LocationGlobal objects. - - This method is an approximation, and will not be accurate over large distances and close to the - earth's poles. It comes from the ArduPilot test code: - https://github.com/diydrones/ardupilot/blob/master/Tools/autotest/common.py - """ - dlat = aLocation2.lat - aLocation1.lat - dlong = aLocation2.lon - aLocation1.lon - return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5 - - - -def distance_to_current_waypoint(): - """ - Gets distance in metres to the current waypoint. - It returns None for the first waypoint (Home location). - """ - nextwaypoint = vehicle.commands.next - if nextwaypoint==0: - return None - missionitem=vehicle.commands[nextwaypoint-1] #commands are zero indexed - lat = missionitem.x - lon = missionitem.y - alt = missionitem.z - targetWaypointLocation = LocationGlobalRelative(lat,lon,alt) - distancetopoint = get_distance_metres(vehicle.location.global_frame, targetWaypointLocation) - return distancetopoint - - -def download_mission(): - """ - Download the current mission from the vehicle. - """ - cmds = vehicle.commands - cmds.download() - cmds.wait_ready() # wait until download is complete. - - - -def adds_square_mission(aLocation, aSize): - """ - Adds a takeoff command and four waypoint commands to the current mission. - The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation). - - The function assumes vehicle.commands matches the vehicle mission state - (you must have called download at least once in the session and after clearing the mission) - """ - - cmds = vehicle.commands - - print " Clear any existing commands" - cmds.clear() - - print " Define/add new commands." - # Add new commands. The meaning/order of the parameters is documented in the Command class. - - #Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air. - cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10)) - - #Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands - point1 = get_location_metres(aLocation, aSize, -aSize) - point2 = get_location_metres(aLocation, aSize, aSize) - point3 = get_location_metres(aLocation, -aSize, aSize) - point4 = get_location_metres(aLocation, -aSize, -aSize) - cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point1.lat, point1.lon, 11)) - cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point2.lat, point2.lon, 12)) - cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point3.lat, point3.lon, 13)) - cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point4.lat, point4.lon, 14)) - #add dummy waypoint "5" at point 4 (lets us know when have reached destination) - cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point4.lat, point4.lon, 14)) - - print " Upload new commands to vehicle" - cmds.upload() - - -def arm_and_takeoff(aTargetAltitude): - """ - Arms vehicle and fly to aTargetAltitude. - """ - - print "Basic pre-arm checks" - # Don't let the user try to arm until autopilot is ready - 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 = VehicleMode("GUIDED") - vehicle.armed = True - - while not vehicle.armed: - print " Waiting for arming..." - time.sleep(1) - - print "Taking off!" - vehicle.simple_takeoff(aTargetAltitude) # 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 - if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95: #Trigger just below target alt. - print "Reached target altitude" - break - time.sleep(1) - - -print 'Create a new mission (for current location)' -adds_square_mission(vehicle.location.global_frame,50) - - -# From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently). -arm_and_takeoff(10) - -print "Starting mission" -# Reset mission set to first (0) waypoint -vehicle.commands.next=0 - -# Set mode to AUTO to start mission -vehicle.mode = VehicleMode("AUTO") - - -# Monitor mission. -# Demonstrates getting and setting the command number -# Uses distance_to_current_waypoint(), a convenience function for finding the -# distance to the next waypoint. - -while True: - nextwaypoint=vehicle.commands.next - print 'Distance to waypoint (%s): %s' % (nextwaypoint, distance_to_current_waypoint()) - - if nextwaypoint==3: #Skip to next waypoint - print 'Skipping to Waypoint 5 when reach waypoint 3' - vehicle.commands.next = 5 - if nextwaypoint==5: #Dummy waypoint - as soon as we reach waypoint 4 this is true and we exit. - print "Exit 'standard' mission when start heading to final waypoint (5)" - break; - time.sleep(1) - -print 'Return to launch' -vehicle.mode = VehicleMode("RTL") - - -#Close vehicle object before exiting script -print "Close vehicle object" -vehicle.close() - -# Shut down simulator if it was started. -if sitl is not None: - sitl.stop() +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +mission_basic.py: Example demonstrating basic mission operations including creating, clearing and monitoring missions. + +Full documentation is provided at http://python.dronekit.io/examples/mission_basic.html +""" + +from ..dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal, Command +import time +import math +from pymavlink import mavutil +from urllib2 import urlopen + +#Set up option parsing to get connection string +import argparse +parser = argparse.ArgumentParser(description='Demonstrates basic mission operations.') +parser.add_argument('--connect', + help="vehicle connection target string. If not specified, SITL automatically started and used.") +args = parser.parse_args() + +connection_string = args.connect +sitl = None + + +#Start SITL if no connection string specified +if not connection_string: + import dronekit_sitl + sitl = dronekit_sitl.start_default() + connection_string = sitl.connection_string() + + +# Connect to the Vehicle +print 'Connecting to vehicle on: %s' % connection_string +vehicle = connect(connection_string, wait_ready=True) + + +def get_location_metres(original_location, dNorth, dEast): + """ + Returns a LocationGlobal object containing the latitude/longitude `dNorth` and `dEast` metres from the + specified `original_location`. The returned Location has the same `alt` value + as `original_location`. + + The function is useful when you want to move the vehicle around specifying locations relative to + the current vehicle position. + The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles. + For more information see: + http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters + """ + earth_radius=6378137.0 #Radius of "spherical" earth + #Coordinate offsets in radians + dLat = dNorth/earth_radius + dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180)) + + #New position in decimal degrees + newlat = original_location.lat + (dLat * 180/math.pi) + newlon = original_location.lon + (dLon * 180/math.pi) + return LocationGlobal(newlat, newlon,original_location.alt) + + +def get_distance_metres(aLocation1, aLocation2): + """ + Returns the ground distance in metres between two LocationGlobal objects. + + This method is an approximation, and will not be accurate over large distances and close to the + earth's poles. It comes from the ArduPilot test code: + https://github.com/diydrones/ardupilot/blob/master/Tools/autotest/common.py + """ + dlat = aLocation2.lat - aLocation1.lat + dlong = aLocation2.lon - aLocation1.lon + return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5 + + + +def distance_to_current_waypoint(): + """ + Gets distance in metres to the current waypoint. + It returns None for the first waypoint (Home location). + """ + nextwaypoint = vehicle.commands.next + if nextwaypoint==0: + return None + missionitem=vehicle.commands[nextwaypoint-1] #commands are zero indexed + lat = missionitem.x + lon = missionitem.y + alt = missionitem.z + targetWaypointLocation = LocationGlobalRelative(lat,lon,alt) + distancetopoint = get_distance_metres(vehicle.location.global_frame, targetWaypointLocation) + return distancetopoint + + +def download_mission(): + """ + Download the current mission from the vehicle. + """ + cmds = vehicle.commands + cmds.download() + cmds.wait_ready() # wait until download is complete. + + + +def adds_square_mission(aLocation, aSize): + """ + Adds a takeoff command and four waypoint commands to the current mission. + The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation). + + The function assumes vehicle.commands matches the vehicle mission state + (you must have called download at least once in the session and after clearing the mission) + """ + + cmds = vehicle.commands + + print " Clear any existing commands" + cmds.clear() + + print " Define/add new commands." + # Add new commands. The meaning/order of the parameters is documented in the Command class. + + #Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air. + cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10)) + + #Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands + point1 = get_location_metres(aLocation, aSize, -aSize) + point2 = get_location_metres(aLocation, aSize, aSize) + point3 = get_location_metres(aLocation, -aSize, aSize) + point4 = get_location_metres(aLocation, -aSize, -aSize) + cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point1.lat, point1.lon, 11)) + cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point2.lat, point2.lon, 12)) + cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point3.lat, point3.lon, 13)) + cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point4.lat, point4.lon, 14)) + #add dummy waypoint "5" at point 4 (lets us know when have reached destination) + cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point4.lat, point4.lon, 14)) + + print " Upload new commands to vehicle" + cmds.upload() + + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + + print "Basic pre-arm checks" + # Don't let the user try to arm until autopilot is ready + 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 = VehicleMode("GUIDED") + vehicle.armed = True + + while not vehicle.armed: + print " Waiting for arming..." + time.sleep(1) + + print "Taking off!" + vehicle.simple_takeoff(aTargetAltitude) # 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 + if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95: #Trigger just below target alt. + print "Reached target altitude" + break + time.sleep(1) + +def waypoint_callback(self, name, msg): + print "Callback! ", msg.seq + +print "\nAdd `commands.next` attribute callback/observer on `vehicle`" +vehicle.add_message_listener('MISSION_CURRENT', waypoint_callback) + +print "\nAdd `commands.next` attribute callback/observer on `vehicle`" +vehicle.add_message_listener('WAYPOINT_CURRENT', waypoint_callback) + +print 'Create a new mission (for current location)' +adds_square_mission(vehicle.location.global_frame,50) + + +# From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently). +arm_and_takeoff(10) + +print "Starting mission" +# Reset mission set to first (0) waypoint +vehicle.commands.next=0 + +# Set mode to AUTO to start mission +vehicle.mode = VehicleMode("AUTO") + + +# Monitor mission. +# Demonstrates getting and setting the command number +# Uses distance_to_current_waypoint(), a convenience function for finding the +# distance to the next waypoint. + +while True: + nextwaypoint=vehicle.commands.next + print 'Distance to waypoint (%s): %s' % (nextwaypoint, distance_to_current_waypoint()) + + if nextwaypoint==3: #Skip to next waypoint + print 'Skipping to Waypoint 5 when reach waypoint 3' + vehicle.commands.next = 5 + if nextwaypoint==5: #Dummy waypoint - as soon as we reach waypoint 4 this is true and we exit. + print "Exit 'standard' mission when start heading to final waypoint (5)" + break; + time.sleep(1) + +print 'Return to launch' +vehicle.mode = VehicleMode("RTL") + + +#Close vehicle object before exiting script +print "Close vehicle object" +vehicle.close() + +# Shut down simulator if it was started. +if sitl is not None: + sitl.stop() From 6ac775a005eaa25ce0ed84e65171cb02ec290f4c Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Sun, 11 Sep 2016 13:28:52 +0200 Subject: [PATCH 2/9] Demonstrate waypoint change callback --- mission_basic.py | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/mission_basic.py b/mission_basic.py index 3d81c599b..da27eda11 100644 --- a/mission_basic.py +++ b/mission_basic.py @@ -8,11 +8,11 @@ Full documentation is provided at http://python.dronekit.io/examples/mission_basic.html """ -from ..dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal, Command +from dronekit import connect, VehicleMode, LocationGlobalRelative, LocationGlobal, Command import time import math from pymavlink import mavutil -from urllib2 import urlopen + #Set up option parsing to get connection string import argparse @@ -170,18 +170,15 @@ def arm_and_takeoff(aTargetAltitude): break time.sleep(1) -def waypoint_callback(self, name, msg): - print "Callback! ", msg.seq - -print "\nAdd `commands.next` attribute callback/observer on `vehicle`" -vehicle.add_message_listener('MISSION_CURRENT', waypoint_callback) - -print "\nAdd `commands.next` attribute callback/observer on `vehicle`" -vehicle.add_message_listener('WAYPOINT_CURRENT', waypoint_callback) print 'Create a new mission (for current location)' adds_square_mission(vehicle.location.global_frame,50) +def waypoint_callback(self, attribute, value): + print "A new waypoint! It's: ", value + +print "\nAdd `commands.next` attribute callback/observer on `vehicle`" +vehicle.add_attribute_listener('commands.next', waypoint_callback) # From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently). arm_and_takeoff(10) From 053ac6e578db00596ffdad14819ad3b7be6ffe40 Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Sun, 11 Sep 2016 13:54:40 +0200 Subject: [PATCH 3/9] Added servo_output_raw attribute and listener to check for servo outputs from the AUV. --- dronekit/__init__.py | 102 +++++++++++++++++++++++++++++++++++++++++++ mission_basic.py | 3 ++ 2 files changed, 105 insertions(+) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index ce08b1be1..5be9d4b6a 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1120,6 +1120,45 @@ def set_rc(chnum, v): set_rc(8, m.chan8_raw) self.notify_attribute_listeners('channels', self.channels) + # Servo outputs + self._servo_output_raw = ServoOutputRaw() + + # Create a message listener using the decorator. + @self.on_message('SERVO_OUTPUT_RAW') + def listener(self, name, message): + """ + The listener is called for messages that contain the string specified in the decorator, + passing the vehicle, message name, and the message. + + The listener writes the message to the (newly attached) ``vehicle.raw_imu`` object + and notifies observers. + """ + self._servo_output_raw.time_boot_us = message.time_usec + self._servo_output_raw.port = message.port + self._servo_output_raw.servo1_raw = message.servo1_raw + self._servo_output_raw.servo2_raw = message.servo2_raw + self._servo_output_raw.servo3_raw = message.servo3_raw + self._servo_output_raw.servo4_raw = message.servo4_raw + self._servo_output_raw.servo5_raw = message.servo5_raw + self._servo_output_raw.servo6_raw = message.servo6_raw + self._servo_output_raw.servo7_raw = message.servo7_raw + self._servo_output_raw.servo8_raw = message.servo8_raw + + if hasattr(message,'servo9_raw'): #Some ArduPilot implementations only send 8 channels over mavlink + self._servo_output_raw.servo9_raw = message.servo9_raw + self._servo_output_raw.servo10_raw = message.servo10_raw + self._servo_output_raw.servo11_raw = message.servo11_raw + self._servo_output_raw.servo12_raw = message.servo12_raw + self._servo_output_raw.servo13_raw = message.servo13_raw + self._servo_output_raw.servo14_raw = message.servo14_raw + self._servo_output_raw.servo15_raw = message.servo15_raw + self._servo_output_raw.servo16_raw = message.servo16_raw + + # Notify all observers of new message (with new value) + # Note that argument `cache=False` by default so listeners + # are updated with every new message + self.notify_attribute_listeners('servo_output_raw', self._servo_output_raw) + self._voltage = None self._current = None self._level = None @@ -1524,6 +1563,10 @@ def flush(self): # Private sugar methods # + @property + def servo_output_raw(self): + return self._servo_output_raw + @property def _mode_mapping(self): return self._master.mode_mapping() @@ -2751,6 +2794,65 @@ def __setitem__(self, index, value): self._vehicle._wpts_dirty = True +class ServoOutputRaw(object): + """ + The RAW servo readings from the OUTPUT of the autopilot + This contains the true raw values without any scaling to allow data capture and system debugging. + + The message definition is here: https://pixhawk.ethz.ch/mavlink/#SERVO_OUTPUT_RAW + + + """ + + def __init__(self, + time_boot_us=None, # Timestamp (microseconds since system boot) + port=None, # Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + servo1_raw=None, # Servo output 1 value, in microseconds + servo2_raw=None, # Etc.. + servo3_raw=None, + servo4_raw=None, + servo5_raw=None, + servo6_raw=None, + servo7_raw=None, + servo8_raw=None, + servo9_raw=None, + servo10_raw=None, + servo11_raw=None, + servo12_raw=None, + servo13_raw=None, + servo14_raw=None, + servo15_raw=None, + servo16_raw=None): + + self.time_boot_us = time_boot_us + self.port = port + self.servo1_raw = servo1_raw + self.servo2_raw = servo2_raw + self.servo3_raw = servo3_raw + self.servo4_raw = servo4_raw + self.servo5_raw = servo5_raw + self.servo6_raw = servo6_raw + self.servo7_raw = servo7_raw + self.servo8_raw = servo8_raw + self.servo9_raw = servo9_raw + self.servo10_raw = servo10_raw + self.servo11_raw = servo11_raw + self.servo12_raw = servo12_raw + self.servo13_raw = servo13_raw + self.servo14_raw = servo14_raw + self.servo15_raw = servo15_raw + self.servo16_raw = servo16_raw + + def __str__(self): + """ + String representation used to print the ServoOutputRaw object. + """ + return "SERVO_OUTPUT_RAW: time_boot_us={},port={},servo1_raw={},servo2_raw={},servo3_raw={},servo4_raw={},servo5_raw={},servo6_raw={},servo7_raw={},servo8_raw={},servo9_raw={},servo10_raw={},servo11_raw={},servo12_raw={},servo13_raw={},servo14_raw={},servo15_raw={},servo16_raw={}".format( + self.time_boot_us, self.port, self.servo1_raw, self.servo2_raw, self.servo3_raw, self.servo4_raw, + self.servo5_raw, self.servo6_raw, self.servo7_raw, self.servo8_raw, self.servo9_raw, self.servo10_raw, + self.servo11_raw, self.servo12_raw, self.servo13_raw, self.servo14_raw, self.servo15_raw, self.servo16_raw) + + from dronekit.mavlink import MAVConnection diff --git a/mission_basic.py b/mission_basic.py index da27eda11..a411416c2 100644 --- a/mission_basic.py +++ b/mission_basic.py @@ -183,6 +183,9 @@ def waypoint_callback(self, attribute, value): # From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently). arm_and_takeoff(10) +#Demonstrate servo output. You can also create a listener and callback function for this. +print "Servo output:", vehicle.servo_output_raw + print "Starting mission" # Reset mission set to first (0) waypoint vehicle.commands.next=0 From de0666b3f3f0f3574ec167b6c9efab8ae416a873 Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Sun, 11 Sep 2016 14:25:06 +0200 Subject: [PATCH 4/9] Now a gopto takes a pic at each waypoint. Todo: sync time, create gopro class, --- mission_basic.py | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/mission_basic.py b/mission_basic.py index a411416c2..4b1e5e97f 100644 --- a/mission_basic.py +++ b/mission_basic.py @@ -13,7 +13,6 @@ import math from pymavlink import mavutil - #Set up option parsing to get connection string import argparse parser = argparse.ArgumentParser(description='Demonstrates basic mission operations.') @@ -174,9 +173,28 @@ def arm_and_takeoff(aTargetAltitude): print 'Create a new mission (for current location)' adds_square_mission(vehicle.location.global_frame,50) +#GoPro Constants +wifipassword = "goprohero3" +photoMode = "http://10.5.5.9/camera/CM?t=" + wifipassword + "&p=%01" +shutter = "http://10.5.5.9/bacpac/SH?t=" + wifipassword + "&p=%01" + + +#GoPro Init and functions +from urllib2 import urlopen + +def SendGoProCmd(cmd): + data = urlopen(cmd).read() + +SendGoProCmd(photoMode) + + def waypoint_callback(self, attribute, value): print "A new waypoint! It's: ", value + #Now let's take a pic + SendGoProCmd(shutter) + + print "\nAdd `commands.next` attribute callback/observer on `vehicle`" vehicle.add_attribute_listener('commands.next', waypoint_callback) From 5773d2b84fd69e4053acae47cd3245434dea0277 Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Sun, 11 Sep 2016 16:28:41 +0200 Subject: [PATCH 5/9] Added GoProHero 3 class and demo in basic mission --- dronekit/cameras.py | 72 +++++++++++++++++++++++++++++++++++++++++++++ mission_basic.py | 9 +++--- 2 files changed, 76 insertions(+), 5 deletions(-) create mode 100644 dronekit/cameras.py diff --git a/dronekit/cameras.py b/dronekit/cameras.py new file mode 100644 index 000000000..c5a995afd --- /dev/null +++ b/dronekit/cameras.py @@ -0,0 +1,72 @@ +__author__ = 'anton' + +# Camera module +""" +This is a library for controlling cameras. It's a typical thing for companion computers to do, +so it's a handy extensions of dronekit. + +The lib is primariliy meant for cams with wifi capability. But USB and Ir might be possible too. + +Creating an camera object supposes your computer is connected to the wifi hotspot of the camera you are trying to control + +---- +""" + +from urllib2 import urlopen +from time import localtime + +class GoProHero3(object): + def __init__(self, wifi_password="goprohero3"): + self.wifipassword = wifi_password + + # See https://github.com/KonradIT/goprowifihack/blob/master/WiFi-Commands.mkdn + # for a list of http commands to control the GoPro camera + + #TODO: convert these into methods. + self.on = "http://10.5.5.9/bacpac/PW?t=" + self.wifipassword + "&p=%01" + self.off = "http://10.5.5.9/bacpac/PW?t=" + self.wifipassword + "&p=%00" + self.stop = "http://10.5.5.9/bacpac/SH?t=" + self.wifipassword + "&p=%00" + self.videoMode = "http://10.5.5.9/camera/CM?t=" + self.wifipassword + "&p=%00" + self.burstMode = "http://10.5.5.9/camera/CM?t=" + self.wifipassword + "&p=%02" + self.timeLapseMode = "http://10.5.5.9/camera/CM?t=" + self.wifipassword + "&p=%03" + self.previewOn = "http://10.5.5.9/camera/PV?t=" + self.wifipassword + "&p=%02" + self.previewOff = "http://10.5.5.9/camera/PV?t=" + self.wifipassword + "&p=%00" + self.wvga60 = "http://10.5.5.9/camera/VR?t=" + self.wifipassword + "&p=%00" + self.wvga120 = "http://10.5.5.9/camera/VR?t=" + self.wifipassword + "&p=%01" + self.v720p30 = "http://10.5.5.9/camera/VR?t=" + self.wifipassword + "&p=%02" + self.v720p60 = "http://10.5.5.9/camera/VR?t=" + self.wifipassword + "&p=%03" + self.v960p30 = "http://10.5.5.9/camera/VR?t=" + self.wifipassword + "&p=%04" + self.v960p48 = "http://10.5.5.9/camera/VR?t=" + self.wifipassword + "&p=%05" + self.v1080p30 = "http://10.5.5.9/camera/VR?t=" + self.wifipassword + "&p=%06" + self.viewWide = "http://10.5.5.9/camera/FV?t=" + self.wifipassword + "&p=%00" + self.viewMedium = "http://10.5.5.9/camera/FV?t=" + self.wifipassword + "&p=%01" + self.viewNarrow = "http://10.5.5.9/camera/FV?t=" + self.wifipassword + "&p=%02" + self.res11mpWide = "http://10.5.5.9/camera/PR?t=" + self.wifipassword + "&p=%00" + self.res8mpMedium = "http://10.5.5.9/camera/PR?t=" + self.wifipassword + "&p=%01" + self.res5mpWide = "http://10.5.5.9/camera/PR?t=" + self.wifipassword + "&p=%02" + self.res5mpMedium = "http://10.5.5.9/camera/PR?t=" + self.wifipassword + "&p=%03" + self.noSound = "http://10.5.5.9/camera/BS?t=" + self.wifipassword + "&p=%00" + self.sound70 = "http://10.5.5.9/camera/BS?t=" + self.wifipassword + "&p=%01" + self.sound100 = "http://10.5.5.9/camera/BS?t=" + self.wifipassword + "&p=%02" + + self.sync_time() + + @staticmethod + def send_cmd(cmd): + data = urlopen(cmd).read() + + def sync_time(self): + lt = localtime() + goprotime = "%{:02x}%{:02x}%{:02x}%{:02x}%{:02x}%{:02x}".format(lt.tm_year-2000, + lt.tm_mon, + lt.tm_mday, + lt.tm_hour, + lt.tm_min, + lt.tm_sec) + self.send_cmd("http://10.5.5.9/camera/TM?t=" + self.wifipassword + "&p=" + goprotime) + + def shutter(self): + self.send_cmd("http://10.5.5.9/bacpac/SH?t=" + self.wifipassword + "&p=%01") + + def photo_mode(self): + self.send_cmd("http://10.5.5.9/camera/CM?t=" + self.wifipassword + "&p=%01") \ No newline at end of file diff --git a/mission_basic.py b/mission_basic.py index 4b1e5e97f..abb9e4cc9 100644 --- a/mission_basic.py +++ b/mission_basic.py @@ -181,18 +181,17 @@ def arm_and_takeoff(aTargetAltitude): #GoPro Init and functions from urllib2 import urlopen +from dronekit.cameras import GoProHero3 -def SendGoProCmd(cmd): - data = urlopen(cmd).read() - -SendGoProCmd(photoMode) +my_cam = GoProHero3(wifi_password="goprohero3") +my_cam.photo_mode() def waypoint_callback(self, attribute, value): print "A new waypoint! It's: ", value #Now let's take a pic - SendGoProCmd(shutter) + my_cam.shutter() print "\nAdd `commands.next` attribute callback/observer on `vehicle`" From 49ab4cafb5c43c132db66b52f05f557912b29f4f Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Mon, 12 Sep 2016 22:52:11 +0200 Subject: [PATCH 6/9] Logging geotags --- dronekit/cameras.py | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/dronekit/cameras.py b/dronekit/cameras.py index c5a995afd..5e78f964d 100644 --- a/dronekit/cameras.py +++ b/dronekit/cameras.py @@ -14,10 +14,34 @@ from urllib2 import urlopen from time import localtime +#from __init__ import Vehicle +import logging +FORMAT = '%(asctime)s, %(message)s' +DATEFORMAT = "%d-%m-%Y, %H:%M:%S" +logging.basicConfig(format=FORMAT, datefmt=DATEFORMAT, filename="camera_log.csv") + + +class GeoTagger(object): + def __init__(self,vehicle=None): + self.vehicle = vehicle + logging.info("Date (DMY), time, picture number, waypoint, gimbal, attitude, location") + + def log_vehicle_state(self, num_picture): + if self.vehicle: + log_msg = ",".join([num_picture, self.vehicle.commands.next, self.vehicle.gimbal, self.vehicle.attitude, self.vehicle.location]) + else: + log_msg = num_picture + logging.info(log_msg) + pass + class GoProHero3(object): - def __init__(self, wifi_password="goprohero3"): + def __init__(self, wifi_password="goprohero3", geotag=True, vehicle=None): self.wifipassword = wifi_password + self.num_picture = 0 + self.geotagger = None + if geotag: + self.geotagger = GeoTagger(vehicle=vehicle) # See https://github.com/KonradIT/goprowifihack/blob/master/WiFi-Commands.mkdn # for a list of http commands to control the GoPro camera @@ -66,7 +90,10 @@ def sync_time(self): self.send_cmd("http://10.5.5.9/camera/TM?t=" + self.wifipassword + "&p=" + goprotime) def shutter(self): + self.num_picture +=1 self.send_cmd("http://10.5.5.9/bacpac/SH?t=" + self.wifipassword + "&p=%01") + if self.geotagger: + self.geotagger.log_vehicle_state(self.num_picture) def photo_mode(self): self.send_cmd("http://10.5.5.9/camera/CM?t=" + self.wifipassword + "&p=%01") \ No newline at end of file From 29f872317f559415bfdd2b46a1139ae2b0b16a4b Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Mon, 12 Sep 2016 07:47:11 +0200 Subject: [PATCH 7/9] Moved basic mission back where it belongs --- mission_basic.py => examples/mission_basic/mission_basic.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename mission_basic.py => examples/mission_basic/mission_basic.py (100%) diff --git a/mission_basic.py b/examples/mission_basic/mission_basic.py similarity index 100% rename from mission_basic.py rename to examples/mission_basic/mission_basic.py From 1559c659401f0ab3b0d073582d43222151b573d6 Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Tue, 13 Sep 2016 12:16:57 +0200 Subject: [PATCH 8/9] fixed log and str error --- dronekit/cameras.py | 60 ++++++++++++++++++++----- examples/mission_basic/mission_basic.py | 4 +- 2 files changed, 50 insertions(+), 14 deletions(-) diff --git a/dronekit/cameras.py b/dronekit/cameras.py index 5e78f964d..e62892e0d 100644 --- a/dronekit/cameras.py +++ b/dronekit/cameras.py @@ -14,34 +14,65 @@ from urllib2 import urlopen from time import localtime -#from __init__ import Vehicle +import sys, signal +from __init__ import Vehicle import logging -FORMAT = '%(asctime)s, %(message)s' -DATEFORMAT = "%d-%m-%Y, %H:%M:%S" -logging.basicConfig(format=FORMAT, datefmt=DATEFORMAT, filename="camera_log.csv") + + + +class Timeout(): + """Timeout class using ALARM signal.""" + class Timeout(Exception): + pass + + def __init__(self, sec): + self.sec = sec + + def __enter__(self): + signal.signal(signal.SIGALRM, self.raise_timeout) + signal.alarm(self.sec) + + def __exit__(self, *args): + signal.alarm(0) # disable alarm + + def raise_timeout(self, *args): + raise Timeout.Timeout() class GeoTagger(object): - def __init__(self,vehicle=None): + def __init__(self, vehicle=None): self.vehicle = vehicle - logging.info("Date (DMY), time, picture number, waypoint, gimbal, attitude, location") + FORMAT = '%(asctime)s, %(message)s' + DATEFORMAT = "%d-%m-%Y, %H:%M:%S" + logging.basicConfig(format=FORMAT, datefmt=DATEFORMAT, filename="camera_log.csv", filemode='w', level=logging.INFO) + logging.info("picture number, waypoint, gimbal pitch, gimbal yaw, gimbal roll, att pitch, att yaw, att roll, latitude, longitude, altitude") def log_vehicle_state(self, num_picture): if self.vehicle: - log_msg = ",".join([num_picture, self.vehicle.commands.next, self.vehicle.gimbal, self.vehicle.attitude, self.vehicle.location]) + log_msg = ",".join(map(str,[num_picture, self.vehicle.commands.next, + self.vehicle.gimbal.pitch, + self.vehicle.gimbal.yaw, + self.vehicle.gimbal.roll, + self.vehicle.attitude.pitch, + self.vehicle.attitude.yaw, + self.vehicle.attitude.roll, + self.vehicle.location.global_frame.lat, + self.vehicle.location.global_frame.lon, + self.vehicle.location.global_frame.alt, + ])) else: - log_msg = num_picture + log_msg = str(num_picture) logging.info(log_msg) pass class GoProHero3(object): - def __init__(self, wifi_password="goprohero3", geotag=True, vehicle=None): + def __init__(self, wifi_password="goprohero3", geotag=True, myvehicle=None): self.wifipassword = wifi_password self.num_picture = 0 self.geotagger = None if geotag: - self.geotagger = GeoTagger(vehicle=vehicle) + self.geotagger = GeoTagger(vehicle=myvehicle) # See https://github.com/KonradIT/goprowifihack/blob/master/WiFi-Commands.mkdn # for a list of http commands to control the GoPro camera @@ -77,7 +108,11 @@ def __init__(self, wifi_password="goprohero3", geotag=True, vehicle=None): @staticmethod def send_cmd(cmd): - data = urlopen(cmd).read() + try: + with Timeout(1): + data = urlopen(cmd).read() + except Exception as e: + print '>>> Exception in camera command ' + str(e) #TODO: log this def sync_time(self): lt = localtime() @@ -91,9 +126,10 @@ def sync_time(self): def shutter(self): self.num_picture +=1 - self.send_cmd("http://10.5.5.9/bacpac/SH?t=" + self.wifipassword + "&p=%01") if self.geotagger: self.geotagger.log_vehicle_state(self.num_picture) + self.send_cmd("http://10.5.5.9/bacpac/SH?t=" + self.wifipassword + "&p=%01") + def photo_mode(self): self.send_cmd("http://10.5.5.9/camera/CM?t=" + self.wifipassword + "&p=%01") \ No newline at end of file diff --git a/examples/mission_basic/mission_basic.py b/examples/mission_basic/mission_basic.py index abb9e4cc9..0e68599a1 100644 --- a/examples/mission_basic/mission_basic.py +++ b/examples/mission_basic/mission_basic.py @@ -183,12 +183,12 @@ def arm_and_takeoff(aTargetAltitude): from urllib2 import urlopen from dronekit.cameras import GoProHero3 -my_cam = GoProHero3(wifi_password="goprohero3") +my_cam = GoProHero3(wifi_password="goprohero3", myvehicle=vehicle) my_cam.photo_mode() def waypoint_callback(self, attribute, value): - print "A new waypoint! It's: ", value + print "A new waypoint! It's: " + str(value) #Now let's take a pic my_cam.shutter() From 476535d0df9cd739501a5326b68d90c22b7a2c5e Mon Sep 17 00:00:00 2001 From: Anton Vanhoucke Date: Tue, 13 Sep 2016 15:21:04 +0200 Subject: [PATCH 9/9] Clean up and log cam errors --- dronekit/cameras.py | 37 ++++++++++++++++++++----------------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/dronekit/cameras.py b/dronekit/cameras.py index e62892e0d..2bf3d7860 100644 --- a/dronekit/cameras.py +++ b/dronekit/cameras.py @@ -14,12 +14,10 @@ from urllib2 import urlopen from time import localtime -import sys, signal -from __init__ import Vehicle +import signal +from . import errprinter import logging - - class Timeout(): """Timeout class using ALARM signal.""" class Timeout(Exception): @@ -45,9 +43,9 @@ def __init__(self, vehicle=None): FORMAT = '%(asctime)s, %(message)s' DATEFORMAT = "%d-%m-%Y, %H:%M:%S" logging.basicConfig(format=FORMAT, datefmt=DATEFORMAT, filename="camera_log.csv", filemode='w', level=logging.INFO) - logging.info("picture number, waypoint, gimbal pitch, gimbal yaw, gimbal roll, att pitch, att yaw, att roll, latitude, longitude, altitude") + logging.info("picture number, waypoint, gimbal pitch, gimbal yaw, gimbal roll, att pitch, att yaw, att roll, latitude, longitude, altitude, camera message") - def log_vehicle_state(self, num_picture): + def log_vehicle_state(self, num_picture, cam_message): if self.vehicle: log_msg = ",".join(map(str,[num_picture, self.vehicle.commands.next, self.vehicle.gimbal.pitch, @@ -59,6 +57,7 @@ def log_vehicle_state(self, num_picture): self.vehicle.location.global_frame.lat, self.vehicle.location.global_frame.lon, self.vehicle.location.global_frame.alt, + cam_message ])) else: log_msg = str(num_picture) @@ -66,6 +65,16 @@ def log_vehicle_state(self, num_picture): pass +def send_http_cmd(cmd): + try: + with Timeout(1): + data = urlopen(cmd).read() + return data + except Exception as e: + errprinter('>>> Exception in http command: ' + str(e)) + return str(e) + + class GoProHero3(object): def __init__(self, wifi_password="goprohero3", geotag=True, myvehicle=None): self.wifipassword = wifi_password @@ -106,13 +115,6 @@ def __init__(self, wifi_password="goprohero3", geotag=True, myvehicle=None): self.sync_time() - @staticmethod - def send_cmd(cmd): - try: - with Timeout(1): - data = urlopen(cmd).read() - except Exception as e: - print '>>> Exception in camera command ' + str(e) #TODO: log this def sync_time(self): lt = localtime() @@ -122,14 +124,15 @@ def sync_time(self): lt.tm_hour, lt.tm_min, lt.tm_sec) - self.send_cmd("http://10.5.5.9/camera/TM?t=" + self.wifipassword + "&p=" + goprotime) + send_http_cmd("http://10.5.5.9/camera/TM?t=" + self.wifipassword + "&p=" + goprotime) def shutter(self): self.num_picture +=1 + result = send_http_cmd("http://10.5.5.9/bacpac/SH?t=" + self.wifipassword + "&p=%01") if self.geotagger: - self.geotagger.log_vehicle_state(self.num_picture) - self.send_cmd("http://10.5.5.9/bacpac/SH?t=" + self.wifipassword + "&p=%01") + self.geotagger.log_vehicle_state(self.num_picture, result) + def photo_mode(self): - self.send_cmd("http://10.5.5.9/camera/CM?t=" + self.wifipassword + "&p=%01") \ No newline at end of file + send_http_cmd("http://10.5.5.9/camera/CM?t=" + self.wifipassword + "&p=%01") \ No newline at end of file