diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 9e6f77b1d..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 @@ -1148,7 +1187,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 @@ -1521,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() @@ -2748,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/examples/mission_basic/mission_basic.py b/examples/mission_basic/mission_basic.py index 4255ec25c..a411416c2 100644 --- a/examples/mission_basic/mission_basic.py +++ b/examples/mission_basic/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 + + +#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) + +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) + +#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 + +# 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()