Merge pull request dronekit#476 from dronekit/hgw_more_fixes_for_goto_takeoff

tcr3dr · tcr3dr · commit 59ffbda11666 · 2015-11-23T17:13:02.000-05:00
Hgw more fixes for goto takeoff
diff --git a/docs/examples/guided-set-speed-yaw-demo.rst b/docs/examples/guided-set-speed-yaw-demo.rst
@@ -200,7 +200,6 @@ The functions sending immediate commands are:
 
 * :ref:`condition_yaw() <guided_mode_copter_set_yaw>` 
 * :ref:`set_roi(location) <guided_mode_copter_set_roi>`
-* :ref:`set_speed(speed) <guided_mode_copter_set_speed>`
 
 The example uses a number functions to convert global locations co-ordinates (decimal degrees) into local 
 coordinates relative to the vehicle (in metres). These are :ref:`described in the guide <guided_mode_copter_useful_conversion_functions>`.
diff --git a/docs/examples/simple_goto.rst b/docs/examples/simple_goto.rst
@@ -20,7 +20,7 @@ you can edit the latitude and longitude to use more appropriate positions for yo
    
 
 .. figure:: simple_goto_example_copter_path.png
-   :width: 50 %
+   :width: 75 %
    :alt: Setting destination using position and changing speed and ROI
 
    Simple Goto Example: Flight path
@@ -67,35 +67,27 @@ On the command prompt you should see (something like):
 .. code-block:: bash
 
     Connecting to vehicle on: 127.0.0.1:14550
-    >>> APM:Copter V3.4-dev (e0810c2e)
+    >>> APM:Copter V3.4-dev (d52279af)
     >>> Frame: QUAD
     Basic pre-arm checks
-     Waiting for vehicle to initialise...
-     Waiting for vehicle to initialise...
-     Waiting for vehicle to initialise...
-     Waiting for vehicle to initialise...
-     Waiting for vehicle to initialise...
     Arming motors
      Waiting for arming...
-     Waiting for arming...
-     Waiting for arming...
     >>> ARMING MOTORS
-    >>> GROUND START
-     Waiting for arming...
-     Waiting for arming...
     >>> Initialising APM...
     Taking off!
      Altitude:  0.0
-     Altitude:  0.00999999977648
-     Altitude:  0.25
-     Altitude:  0.5
-    ...
-     Altitude:  18.7299995422
-     Altitude:  19.2700004578
+     Altitude:  0.0
+     Altitude:  0.1
+     Altitude:  1.25
+     Altitude:  3.08
+     Altitude:  5.14
+     Altitude:  7.28
+     Altitude:  9.2
+     Altitude:  9.71
     Reached target altitude
     Set default/target airspeed to 3
-    Going to first point...
-    Going to second point...
+    Going towards first point for 30 seconds ...
+    Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...
     Returning to Launch
     Close vehicle object
 
@@ -136,24 +128,41 @@ call :py:func:`Vehicle.simple_goto() <dronekit.Vehicle.simple_goto>` with the ta
 
 .. code-block:: python
 
+    # set the default travel speed
+    vehicle.airspeed=3
+
     point1 = LocationGlobalRelative(-35.361354, 149.165218, 20)
     vehicle.simple_goto(point1)
 
     # sleep so we can see the change in map
     time.sleep(30)
 
-Without some sort of "wait" the next command would be executed immediately. In this example we just 
-sleep for 30 seconds, set a new destination and then sleep another 30 seconds (the vehicle doesn't reach
-its target destination in either 30 second wait). The script doesn't report anything during these sleep periods, 
-but you can observe the vehicle's movement on a ground station map.
+.. tip:: 
+
+    Without some sort of "wait" the next command would be executed immediately. In this example we just 
+    sleep for 30 seconds before executing the next command.
+
+When moving towards the first point we set the airspeed using the :py:attr:`Vehicle.airspeed <dronekit.Vehicle.airspeed>` 
+attribute. For the second point the example specifies the target groundspeed when calling 
+:py:func:`Vehicle.simple_goto() <dronekit.Vehicle.simple_goto>`
+
+.. code-block:: python
+
+    vehicle.simple_goto(point2, groundspeed=10)
+
+.. tip::
+
+    The script doesn't report anything during the sleep periods, 
+    but you can observe the vehicle's movement on a ground station map.
 
 
 
 
 RTL - Return to launch
 ------------------------
 
-To return to the home position and land, we set the mode to ``RTL``:
+To return to the home position and land, we set the mode to ``RTL``.
+The vehicle travels at the previously set default speed:
 
 .. code-block:: python
 
diff --git a/docs/examples/simple_goto_example_copter_path.png b/docs/examples/simple_goto_example_copter_path.png
diff --git a/docs/guide/copter/guided_mode.rst b/docs/guide/copter/guided_mode.rst
@@ -58,7 +58,27 @@ to indicate when the vehicle has reached its destination. Developers can use eit
 :ref:`measure proximity to the target <example_guided_mode_goto_convenience>` to give the vehicle an 
 opportunity to reach its destination. The :ref:`example-guided-mode-setting-speed-yaw` shows both approaches.
 
-When moving the vehicle you can send a separate command to :ref:`control the speed <guided_mode_copter_set_speed>` (and other vehicle behaviour).
+You can optionally set the target movement speed using the function's ``airspeed`` or ``groundspeed`` parameters 
+(this is equivalent to setting :py:attr:`Vehicle.airspeed <dronekit.Vehicle.airspeed>`
+or :py:attr:`Vehicle.groundspeed <dronekit.Vehicle.groundspeed>`). The speed setting will then be used
+for all positional movement commands until it is set to another value. 
+
+.. code-block:: python
+
+    # Set airspeed using attribute
+    vehicle.airspeed = 5 #m/s
+
+    # Set groundspeed using attribute
+    vehicle.groundspeed = 7.5 #m/s
+    
+    # Set groundspeed using `simple_goto()` parameter
+    vehicle.simple_goto(a_location, groundspeed=10)
+
+.. note::
+
+    ``Vehicle.simple_goto()`` will use the last speed value set. If both speed values are set at the
+    same time the resulting behaviour will be vehicle dependent.
+
 
 .. tip::
 
@@ -256,13 +276,14 @@ Supported commands
 `Copter Commands in Guided Mode <http://dev.ardupilot.com/wiki/copter-commands-in-guided-mode/>`_ lists all the commands that *can* be sent to Copter in GUIDED mode (in fact most of the commands can be sent in any mode!)
 
 DroneKit-Python provides a friendly Python API that abstracts many of the commands. 
-Where possible you should use the API rather than send messages directly. 
-For example it is better to use :py:func:`Vehicle.simple_takeoff() <dronekit.Vehicle.simple_takeoff>` 
-than to explicitly send the ``MAV_CMD_NAV_TAKEOFF`` command.
+Where possible you should use the API rather than send messages directly> For example, use:
+
+* :py:func:`Vehicle.simple_takeoff() <dronekit.Vehicle.simple_takeoff>` instead of the ``MAV_CMD_NAV_TAKEOFF`` command. 
+* :py:func:`Vehicle.simple_goto() <dronekit.Vehicle.simple_goto>`, :py:attr:`Vehicle.airspeed <dronekit.Vehicle.airspeed>`, 
+   or :py:attr:`Vehicle.groundspeed <dronekit.Vehicle.groundspeed>` rather than ``MAV_CMD_DO_CHANGE_SPEED``.
 
 Some of the MAV_CMD commands that you might want to send include: 
 :ref:`MAV_CMD_CONDITION_YAW <guided_mode_copter_set_yaw>`, 
-:ref:`MAV_CMD_DO_CHANGE_SPEED <guided_mode_copter_set_speed>`, 
 :ref:`MAV_CMD_DO_SET_ROI <guided_mode_copter_set_roi>`, 
 ``MAV_CMD_DO_SET_SERVO``, 
 ``MAV_CMD_DO_REPEAT_SERVO``, 
@@ -317,36 +338,6 @@ The command allows you to specify that whether the heading is an absolute angle
 
 
 
-.. _guided_mode_copter_set_speed:
-
-Setting the speed
------------------
-
-Send `MAV_CMD_DO_CHANGE_SPEED <http://copter.ardupilot.com/common-mavlink-mission-command-messages-mav_cmd/#mav_cmd_do_change_speed>`_ to change the current speed (metres/second) when travelling to a point. 
-
-.. code-block:: python
-
-    def set_speed(speed):
-        msg = vehicle.message_factory.command_long_encode(
-            0, 0,    # target system, target component
-            mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, #command
-            0, #confirmation
-            0, #param 1
-            speed, # speed in metres/second
-            0, 0, 0, 0, 0 #param 3 - 7
-            )
-
-        # send command to vehicle
-        vehicle.send_mavlink(msg)
-
-
-The command is useful when setting the vehicle position directly. It is not needed when controlling movement using velocity vectors.
-
-.. note:: 
-
-    In AC3.2.1 Copter will accelerate to the target speed across the journey and then decelerate as it reaches the target. In AC3.3 the speed changes immediately.
-
-
 
 .. _guided_mode_copter_set_roi:
 
@@ -430,7 +421,14 @@ to the Earth's poles.
         #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)
+        if type(original_location) is LocationGlobal:
+            targetlocation=LocationGlobal(newlat, newlon,original_location.alt)
+        elif type(original_location) is LocationGlobalRelative:
+            targetlocation=LocationGlobalRelative(newlat, newlon,original_location.alt)
+        else:
+            raise Exception("Invalid Location object passed")
+            
+        return targetlocation;
 
 
 .. code-block:: python
diff --git a/docs/guide/taking_off.rst b/docs/guide/taking_off.rst
@@ -16,10 +16,16 @@ and then call :py:func:`Vehicle.simple_takeoff() <dronekit.Vehicle.simple_takeof
 
 .. tip::
 
-    Copter is always started in ``GUIDED`` mode. Copter will not take off ``AUTO`` mode even if you have a 
-    `MAV_CMD_NAV_TAKEOFF <http://copter.ardupilot.com/common-mavlink-mission-command-messages-mav_cmd/#copter-2>`_ waypoint 
-    in your mission (you can run a mission by switching to ``AUTO`` mode after you're in the air).
+    Copter is usually started in ``GUIDED`` mode. 
     
+    * For Copter 3.2.1 and earlier you cannot take off in ``AUTO`` mode (if you need to run a mission you take off
+      in ``GUIDED`` mode and then switch to ``AUTO`` mode once you're in the air).    
+    * Starting from Copter 3.3 you can takeoff in ``AUTO`` mode (provided the mission has a 
+      `MAV_CMD_NAV_TAKEOFF <http://copter.ardupilot.com/common-mavlink-mission-command-messages-mav_cmd/#copter-2>`_ command)
+      but the mission will not start until you explicitly send the 
+      `MAV_CMD_MISSION_START <http://copter.ardupilot.com/wiki/common-mavlink-mission-command-messages-mav_cmd/#mav_cmd_mission_start>`_ 
+      message.
+      
     By contrast, Plane apps take off using the ``MAV_CMD_NAV_TAKEOFF`` command in a mission. Plane should first arm and then change to
     ``AUTO`` mode to start the mission. 
 
@@ -56,7 +62,8 @@ The code below shows a function to arm a Copter, take off, and fly to a specifie
         #  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: #Just below target, in case of undershoot.
+            #Break and return from function just below target altitude. 
+            if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95:
                 print "Reached target altitude"
                 break
             time.sleep(1)
@@ -128,7 +135,8 @@ concerned about reaching a particular height, a simpler implementation might jus
 
         while True:
             print " Altitude: ", vehicle.location.global_relative_frame.alt
-            if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95: #Just below target, in case of undershoot.
+            #Break and return from function just below target altitude. 
+            if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95:
                 print "Reached target altitude"
                 break
             time.sleep(1)
diff --git a/dronekit/__init__.py b/dronekit/__init__.py
@@ -2158,25 +2158,6 @@ class CommandSequence(object):
         cmds.add(cmd)
         cmds.upload()
 
-    .. py:function:: takeoff(altitude)
-
-        .. note:: This function should only be used on Copter vehicles.
-
-        Take off and fly the vehicle to the specified altitude (in metres) and then wait for another command.
-
-        The vehicle must be in ``GUIDED`` mode and armed before this is called.
-
-        There is no mechanism for notification when the correct altitude is reached, and if another command arrives
-        before that point (e.g. :py:func:`simple_goto`) it will be run instead.
-
-        .. warning::
-
-            Apps should code to ensure that the vehicle will reach a safe altitude before other commands are executed.
-            A good example is provided in the guide topic :ref:`taking-off`.
-
-        :param altitude: Target height, in metres.
-
-        .. todo:: This is a hack. The actual function should be defined here. See https://github.com/dronekit/dronekit-python/issues/64
     """
 
     def __init__(self, vehicle):
diff --git a/examples/guided_set_speed_yaw/guided_set_speed_yaw.py b/examples/guided_set_speed_yaw/guided_set_speed_yaw.py
@@ -1,3 +1,4 @@
+
 """
 guided_set_speed_yaw.py: (Copter Only)
 
@@ -6,7 +7,7 @@
 Example documentation: http://python.dronekit.io/examples/guided-set-speed-yaw-demo.html
 """
 
-from dronekit import connect, VehicleMode, LocationGlobal
+from dronekit import connect, VehicleMode, LocationGlobal, LocationGlobalRelative
 from pymavlink import mavutil # Needed for command message definitions
 import time
 import math
@@ -132,34 +133,6 @@ def set_roi(location):
     vehicle.send_mavlink(msg)
 
 
-def set_speed(speed):
-    """
-    Send MAV_CMD_DO_CHANGE_SPEED to change the current speed when travelling to a point.
-
-    In AC3.2.1 Copter will accelerate to this speed near the centre of its journey and then 
-    decelerate as it reaches the target. In AC3.3 the speed changes immediately.
-
-    This method is only useful when controlling the vehicle using position/goto commands. 
-    It is not needed when controlling vehicle movement using velocity components.
-
-    For more information see: 
-    http://copter.ardupilot.com/common-mavlink-mission-command-messages-mav_cmd/#mav_cmd_do_change_speed
-    """
-    # create the MAV_CMD_DO_CHANGE_SPEED command
-    msg = vehicle.message_factory.command_long_encode(
-        0, 0,    # target system, target component
-        mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, #command
-        0, #confirmation
-        0, #param 1
-        speed, # speed
-        0, 0, 0, 0, 0 #param 3 - 7
-        )
-
-    # send command to vehicle
-    vehicle.send_mavlink(msg)
-
-
-
 
 """
 Functions to make it easy to convert between the different frames-of-reference. In particular these
@@ -197,7 +170,14 @@ def get_location_metres(original_location, dNorth, dEast):
     #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)
+    if type(original_location) is LocationGlobal:
+        targetlocation=LocationGlobal(newlat, newlon,original_location.alt)
+    elif type(original_location) is LocationGlobalRelative:
+        targetlocation=LocationGlobalRelative(newlat, newlon,original_location.alt)
+    else:
+        raise Exception("Invalid Location object passed")
+        
+    return targetlocation;
 
 
 def get_distance_metres(aLocation1, aLocation2):
@@ -315,9 +295,6 @@ def goto(dNorth, dEast, gotoFunction=vehicle.simple_goto):
     targetLocation=get_location_metres(currentLocation, dNorth, dEast)
     targetDistance=get_distance_metres(currentLocation, targetLocation)
     gotoFunction(targetLocation)
-    
-    
-
 
     while vehicle.mode.name=="GUIDED": #Stop action if we are no longer in guided mode.
         remainingDistance=get_distance_metres(vehicle.location.global_relative_frame, targetLocation)
@@ -420,8 +397,10 @@ def send_global_velocity(velocity_x, velocity_y, velocity_z, duration):
 the distance-to-target.
 """	
 print("TRIANGLE path using standard Vehicle.simple_goto()")
-print("Set speed to 5m/s.")
-set_speed(5)
+
+print("Set groundspeed to 5m/s.")
+vehicle.groundspeed=5
+
 print("Position North 80 West 50")
 goto(80, -50)
 
@@ -450,17 +429,17 @@ def send_global_velocity(velocity_x, velocity_y, velocity_z, duration):
 print("TRIANGLE path using standard SET_POSITION_TARGET_GLOBAL_INT message and with varying speed.")
 print("Position South 100 West 130")
 
-print("Set speed to 5m/s.")
-set_speed(5)
+print("Set groundspeed to 5m/s.")
+vehicle.groundspeed=5
 goto(-100, -130, goto_position_target_global_int)
 
-print("Set speed to 15m/s (max).")
-set_speed(15)
+print("Set groundspeed to 15m/s (max).")
+vehicle.groundspeed=15
 print("Position South 0 East 200")
 goto(0, 260, goto_position_target_global_int)
 
-print("Set speed to 10m/s (max).")
-set_speed(10)
+print("Set airspeed to 10m/s (max).")
+vehicle.airspeed=10
 
 print("Position North 100 West 130")
 goto(100, -130, goto_position_target_global_int)
diff --git a/examples/simple_goto/simple_goto.py b/examples/simple_goto/simple_goto.py
