From 68214f21718f2396a1b675d8c57a0baa38f5f073 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 17 Feb 2016 16:38:20 +1100 Subject: [PATCH 1/9] Add changes for try_set_armed() method --- dronekit/__init__.py | 89 +++++++++++++++++++++++++++++++++++++++----- 1 file changed, 80 insertions(+), 9 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 3391824e4..54bf0ef57 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1625,20 +1625,19 @@ def gps_0(self): @property def armed(self): """ - This attribute can be used to get and set the ``armed`` state of the vehicle (``boolean``). - - The code below shows how to read the state, and to arm/disarm the vehicle: + This attribute can be used to get the ``armed`` state of the vehicle (``boolean``). .. code:: python # Print the armed state for the vehicle print "Armed: %s" % vehicle.armed + + To arm/disarm the vehicle use :py:func:`try_set_armed`. - # Disarm the vehicle - vehicle.armed = False - - # Arm the vehicle - vehicle.armed = True + .. warning:: + + The attribute can also be used to *set* the armed state. + This is deprecated, and may be removed in a future version. """ return self._armed @@ -1649,11 +1648,83 @@ def armed(self, value): self._master.arducopter_arm() else: self._master.arducopter_disarm() + + + def try_set_armed(self, value=True, wait_ready=True, retries=2, timeout=3): + """ + Send a message to arm (default) or disarm the vehicle. + + The API should primarily be used as shown below: + + .. code-block:: python + + # Arm the vehicle + if vehicle.is_armable: //Required! + vehicle.try_set_armed() #returns when armed + + # Disarm the vehicle + vehicle.try_set_armed(value=False) #returns when disarmed + + When used in this way the method will send a message (with retries) to arm/disarm + the vehicle and will either return when the :py:attr:`armed` state has changed or + raise an exception on failure. You can also set the number of retries and the timeout + between re-sending the message, if needed. + + The function will return immediately if the new value is the same as the current value. + The function will raise an exception if called to arm a vehicle that is not + armable (see :py:attr:`is_armable`). + + Setting ``wait_ready=False`` sends the message and then returns immediately + (without checking the result). + + .. tip:: + + We recommend the default (``wait_ready=True``) because it safely implements all + the checking that you would otherwise have to do yourself to determine when the + armed state has changed. + + :param Bool value: ``True`` (default) to arm, ``False`` to disarm. + :param Bool wait_ready: ``True`` (default) wait until the value has changed before completing. + ``False`` to send the request and complete immediately. + :param int retries: Number of attempts to resend the message. + :param int timeout: Time to wait for the value to change before retrying/exiting (in seconds). + """ + # Return immediately if target value is current value. + if bool(value) == self._armed: + return + + # Invoke callback or raise exception if attempting to arm when not armable. + if self._armed==False and not self.is_armable and bool(value)==True: + raise APIException('Attempting to arm when not armable.') + + #If method is non waiting send command immediately. + if wait_ready==False: + if value: + self._master.arducopter_arm() + else: + self._master.arducopter_disarm() + return +#chus + #Otherwise execute retry code + for retry_num in range(0,retries): + if value: + self._master.arducopter_arm() + else: + self._master.arducopter_disarm() + start_time=time.time() + while time.time()-start_time <= timeout: + if bool(value) == self._armed: + return + time.sleep(0.1) + + #No more retries. Raise exception. + raise APIException('Unable to arm.') + @property def is_armable(self): """ - Returns ``True`` if the vehicle is ready to arm, false otherwise (``Boolean``). + Returns ``True`` if the vehicle is ready to :py:func:`arm `, ``false`` otherwise (``Boolean``). This attribute wraps a number of pre-arm checks, ensuring that the vehicle has booted, has a good GPS fix, and that the EKF pre-arm is complete. From 063f61e30adef718d5d9668406532446d78932d6 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 18 Feb 2016 16:11:54 +1100 Subject: [PATCH 2/9] Add try_set_target_airspeed --- dronekit/__init__.py | 129 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 124 insertions(+), 5 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 54bf0ef57..f44c78aa2 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1495,7 +1495,22 @@ def _mode_mapping(self): @property def mode(self): """ - This attribute is used to get and set the current flight mode (:py:class:`VehicleMode`). + This attribute is used to get the current vehicle mode (:py:class:`VehicleMode`). + + .. code:: python + + # Print the vehicle mode + print "Mode: %s" % vehicle.mode + + # Print the vehicle mode name + print "Mode name: %s" % vehicle.mode.name + + The value of the attribute can be set using :py:func:`try_set_mode`. + + .. warning:: + + The attribute can also be used to *set* the mode. + This is deprecated, and may be removed in a future version. """ if not self._flightmode: return None @@ -1504,6 +1519,67 @@ def mode(self): @mode.setter def mode(self, v): self._master.set_mode(self._mode_mapping[v.name]) + + + def try_set_mode(self, value, wait_ready=True, retries=2, timeout=3): + """ + Send a message to change the vehicle mode. + + The API should primarily be used as shown below: + + .. code-block:: python + + # Set the mode using a VehicleMode + vehicle.try_set_mode(VehicleMode("GUIDED")) + + # Set the mode using a string + vehicle.try_set_mode("STABILIZE") + + When used in this way the method will send a message (with retries) to set the new mode + and will either return when the :py:attr:`mode` has changed or raise an exception + on failure. You can also set the number of retries and the timeout + between re-sending the message, if needed. + + The function will return immediately if the new value is the same as the current value. + + Setting ``wait_ready=False`` sends the message and then returns immediately + (without checking the result). + + .. tip:: + + We recommend the default (``wait_ready=True``) because it safely implements + the checking and resending that you would otherwise have to do yourself to determine when the + mode has changed. + + :param VehicleMode value: The new :py:class:`VehicleMode` (or mode name, as a string). + :param Bool wait_ready: ``True`` (default) wait until the value has changed before completing. + ``False`` to send the request and complete immediately. + :param int retries: Number of attempts to resend the message. + :param int timeout: Time to wait for the value to change before retrying/exiting (in seconds). + """ + # Allow users to specify the target mode as a string. + if type(value)==str: + value=VehicleMode(value) + # Return immediately if target value is current value. + if value.name == self.mode.name: + return + + #If method is non waiting send command immediately. + if wait_ready==False: + self._master.set_mode(self._mode_mapping[value.name]) + return + + #Otherwise execute retry code + for retry_num in range(0,retries): + self._master.set_mode(self._mode_mapping[value.name]) + start_time=time.time() + while time.time()-start_time <= timeout: + if value.name == self.mode.name: + return + time.sleep(0.1) + + #No more retries. Raise exception. + raise APIException('Unable to change mode.') @property def location(self): @@ -1704,7 +1780,7 @@ def try_set_armed(self, value=True, wait_ready=True, retries=2, timeout=3): else: self._master.arducopter_disarm() return -#chus + #Otherwise execute retry code for retry_num in range(0,retries): if value: @@ -1801,9 +1877,12 @@ def airspeed(self): """ Current airspeed in metres/second (``double``). - This attribute is settable. The set value is the default target airspeed - when moving the vehicle using :py:func:`simple_goto` (or other position-based - movement commands). + To set the airspeed use :py:func:`try_set_target_airspeed`. + + .. warning:: + + The ability to directly set the airspeed using this attribute + is deprecated, and may be removed in a future version. """ return self._airspeed @@ -1822,6 +1901,46 @@ def airspeed(self, speed): # send command to vehicle self.send_mavlink(msg) + + def try_set_target_airspeed(self, speed): + """ + Send a message to change the vehicle speed. The set value is the + default target airspeed when moving the vehicle using :py:func:`simple_goto` + (or other position-based movement commands). + + The message will be sent immediately but may not be received + or acted on by the autopilot. Users can monitor :py:attr:`airspeed` + to determine if the command has had any effect. + + The API should be used as shown below: + + .. code-block:: python + + # Set the speed + vehicle.try_set_target_airspeed(5) + + .. todo:: + + We should monitor for receipt of the message but there is no + robust way to trust the COMMAND_ACK and no method to + read back the current speed setting: + https://github.com/diydrones/ardupilot/issues/3636 + + :param float speed: The target speed. + """ + speed_type = 0 # air speed + msg = self.message_factory.command_long_encode( + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, #command + 0, #confirmation + speed_type, #param 1 + speed, # speed in metres/second + -1, 0, 0, 0, 0 #param 3 - 7 + ) + + self.send_mavlink(msg) + + @property def gimbal(self): From 6d937bb59fa827e0a2242369f04abdc6c765bd31 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 18 Feb 2016 16:16:37 +1100 Subject: [PATCH 3/9] Add try_set_target_groundspeed --- dronekit/__init__.py | 54 ++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 49 insertions(+), 5 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index f44c78aa2..25bc1ad49 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1849,10 +1849,13 @@ def heading(self): def groundspeed(self): """ Current groundspeed in metres/second (``double``). - - This attribute is settable. The set value is the default target groundspeed - when moving the vehicle using :py:func:`simple_goto` (or other position-based - movement commands). + + To set the groundspeed use :py:func:`try_set_target_groundspeed`. + + .. warning:: + + The ability to directly set the airspeed using this attribute + is deprecated, and may be removed in a future version. """ return self._groundspeed @@ -1871,6 +1874,46 @@ def groundspeed(self, speed): # send command to vehicle self.send_mavlink(msg) + + def try_set_target_groundspeed(self, speed): + """ + Send a message to change the vehicle groundspeed. The set value is the + default target speed when moving the vehicle using :py:func:`simple_goto` + (or other position-based movement commands). + + The message will be sent immediately but may not be received + or acted on by the autopilot. Users can monitor :py:attr:`groundspeed` + to determine if the command has had any effect. + + The API should be used as shown below: + + .. code-block:: python + + # Set the speed + vehicle.try_set_target_groundspeed(5) + + .. todo:: + + We should monitor for receipt of the message but there is no + robust way to trust the COMMAND_ACK and no method to + read back the current speed setting: + https://github.com/diydrones/ardupilot/issues/3636 + + + :param float speed: The target speed. + """ + speed_type = 1 # ground speed + msg = self.message_factory.command_long_encode( + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, #command + 0, #confirmation + speed_type, #param 1 + speed, # speed in metres/second + -1, 0, 0, 0, 0 #param 3 - 7 + ) + + self.send_mavlink(msg) + @property def airspeed(self): @@ -1904,7 +1947,7 @@ def airspeed(self, speed): def try_set_target_airspeed(self, speed): """ - Send a message to change the vehicle speed. The set value is the + Send a message to change the vehicle airspeed. The set value is the default target airspeed when moving the vehicle using :py:func:`simple_goto` (or other position-based movement commands). @@ -1925,6 +1968,7 @@ def try_set_target_airspeed(self, speed): robust way to trust the COMMAND_ACK and no method to read back the current speed setting: https://github.com/diydrones/ardupilot/issues/3636 + :param float speed: The target speed. """ From 283087490e4a4d5d1ee85c97b66e63b2fa0b5350 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 18 Feb 2016 16:30:43 +1100 Subject: [PATCH 4/9] Fix up example test code to use new setter functions rather than attributes --- examples/simple_goto/simple_goto.py | 19 +++++---- examples/vehicle_state/vehicle_state.py | 52 +++++++++++++++++-------- 2 files changed, 48 insertions(+), 23 deletions(-) diff --git a/examples/simple_goto/simple_goto.py b/examples/simple_goto/simple_goto.py index b2e581dc2..553cbb0c8 100644 --- a/examples/simple_goto/simple_goto.py +++ b/examples/simple_goto/simple_goto.py @@ -10,6 +10,11 @@ import time + + + + + #Set up option parsing to get connection string import argparse parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.') @@ -52,12 +57,9 @@ def arm_and_takeoff(aTargetAltitude): print "Arming motors" # Copter should arm in GUIDED mode vehicle.mode = VehicleMode("GUIDED") - vehicle.armed = True - - # Confirm vehicle armed before attempting to take off - while not vehicle.armed: - print " Waiting for arming..." - time.sleep(1) + + # Arm the vehicle + vehicle.try_set_armed() #returns when armed print "Taking off!" vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude @@ -74,17 +76,20 @@ def arm_and_takeoff(aTargetAltitude): arm_and_takeoff(10) + print "Set default/target airspeed to 3" -vehicle.airspeed = 3 +vehicle.try_set_target_airspeed(3) print "Going towards first point for 30 seconds ..." point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) vehicle.simple_goto(point1) + # sleep so we can see the change in map time.sleep(30) print "Going towards second point for 30 seconds (groundspeed set to 10 m/s) ..." +vehicle.try_set_target_groundspeed(10) point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) vehicle.simple_goto(point2, groundspeed=10) diff --git a/examples/vehicle_state/vehicle_state.py b/examples/vehicle_state/vehicle_state.py index 1d73e1de4..435083067 100644 --- a/examples/vehicle_state/vehicle_state.py +++ b/examples/vehicle_state/vehicle_state.py @@ -78,10 +78,10 @@ print " Heading: %s" % vehicle.heading print " Is Armable?: %s" % vehicle.is_armable print " System status: %s" % vehicle.system_status.state -print " Groundspeed: %s" % vehicle.groundspeed # settable -print " Airspeed: %s" % vehicle.airspeed # settable -print " Mode: %s" % vehicle.mode.name # settable -print " Armed: %s" % vehicle.armed # settable +print " Groundspeed: %s" % vehicle.groundspeed +print " Airspeed: %s" % vehicle.airspeed +print " Mode: %s" % vehicle.mode.name +print " Armed: %s" % vehicle.armed @@ -113,11 +113,21 @@ print " New Home Location (from vehicle - altitude should be 222): %s" % vehicle.home_location -print "\nSet Vehicle.mode = GUIDED (currently: %s)" % vehicle.mode.name -vehicle.mode = VehicleMode("GUIDED") -while not vehicle.mode.name=='GUIDED': #Wait until mode has changed - print " Waiting for mode change ..." - time.sleep(1) +print "\nSet Vehicle mode to GUIDED (currently: %s)" % vehicle.mode.name +vehicle.try_set_mode(VehicleMode("GUIDED")) +print " New mode: %s" % vehicle.mode.name + +print "\nSet Vehicle mode to STABILIZE (currently: %s)" % vehicle.mode.name +vehicle.try_set_mode("STABILIZE") +print " New mode: %s" % vehicle.mode.name + +print "\nSet Vehicle.mode = GUIDED without waiting (currently: %s)" % vehicle.mode.name +vehicle.try_set_mode(value=VehicleMode("GUIDED"), wait_ready=False) +while not vehicle.mode.name=="GUIDED": + print " Waiting for mode change..." + time.sleep(0.3) +print " New mode: %s" % vehicle.mode.name + # Check that vehicle is armable @@ -126,12 +136,22 @@ time.sleep(1) # If required, you can provide additional information about initialisation # using `vehicle.gps_0.fix_type` and `vehicle.mode.name`. + +print "\nSend arm message and wait (currently: %s)" % vehicle.armed +if vehicle.is_armable: + vehicle.try_set_armed() #returns when armed + print " Vehicle is armed: %s" % vehicle.armed + +print "\nSend disarm message and wait (currently: %s)" % vehicle.armed +vehicle.try_set_armed(value=False) #returns when armed +print " Vehicle is armed: %s" % vehicle.armed -print "\nSet Vehicle.armed=True (currently: %s)" % vehicle.armed -vehicle.armed = True +print "\nSend arm message without waiting (currently: %s)" % vehicle.armed +# This non-waiting form is not recommended. +vehicle.try_set_armed(wait_ready=False) while not vehicle.armed: print " Waiting for arming..." - time.sleep(1) + time.sleep(0.3) print " Vehicle is armed: %s" % vehicle.armed @@ -170,8 +190,8 @@ def decorated_mode_callback(self, attr_name, value): # `value` is the updated attribute value. print " CALLBACK: Mode changed to", value -print " Set mode=STABILIZE (currently: %s) and wait for callback" % vehicle.mode.name -vehicle.mode = VehicleMode("STABILIZE") +print " Set mode=STABILIZE (currently: %s) and wait for callback" % vehicle.mode.name +vehicle.try_set_mode(VehicleMode("STABILIZE")) print " Wait 2s so callback invoked before moving to next example" time.sleep(2) @@ -249,8 +269,8 @@ def any_parameter_callback(self, attr_name, value): ## Reset variables to sensible values. print "\nReset vehicle attributes/parameters and exit" -vehicle.mode = VehicleMode("STABILIZE") -vehicle.armed = False +vehicle.try_set_mode(value=VehicleMode("STABILIZE")) +vehicle.try_set_armed(value=False) vehicle.parameters['THR_MIN']=130 vehicle.parameters['THR_MID']=500 From 055c4841cdd73d49a945cd24cbf262e9171390ad Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 18 Feb 2016 20:20:43 +1100 Subject: [PATCH 5/9] Make airspeed and groundspeed setters have exception if used with wait_ready=True --- dronekit/__init__.py | 76 +++++++++++++++++++++++------ examples/simple_goto/simple_goto.py | 5 +- 2 files changed, 63 insertions(+), 18 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 25bc1ad49..c179c9744 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1875,22 +1875,28 @@ def groundspeed(self, speed): self.send_mavlink(msg) - def try_set_target_groundspeed(self, speed): + def try_set_target_groundspeed(self, speed, wait_ready=True): """ Send a message to change the vehicle groundspeed. The set value is the default target speed when moving the vehicle using :py:func:`simple_goto` (or other position-based movement commands). - The message will be sent immediately but may not be received - or acted on by the autopilot. Users can monitor :py:attr:`groundspeed` - to determine if the command has had any effect. - - The API should be used as shown below: + The API **must** be used as shown below (or an exception will be raised): .. code-block:: python # Set the speed - vehicle.try_set_target_groundspeed(5) + vehicle.try_set_target_groundspeed(5, wait_ready=False) + + Using ``wait_ready=False`` the message will be sent immediately but may not be received + or acted on by the autopilot. Users can monitor :py:attr:`groundseed` + to determine if the command has had any effect. + + .. note:: + + ``wait_ready=True`` (default) is not supported and will raise an exception. + This is inconvenient, but is done to future proof the API for when a robust + synchronous speed setter can be written. .. todo:: @@ -1902,6 +1908,8 @@ def try_set_target_groundspeed(self, speed): :param float speed: The target speed. """ + if wait_ready == True: + raise APIException('try_set_target_groundspeed() must be called with wait_ready=True (False not supported).') speed_type = 1 # ground speed msg = self.message_factory.command_long_encode( 0, 0, # target system, target component @@ -1945,23 +1953,30 @@ def airspeed(self, speed): self.send_mavlink(msg) - def try_set_target_airspeed(self, speed): + def try_set_target_airspeed(self, speed, wait_ready=True): """ Send a message to change the vehicle airspeed. The set value is the default target airspeed when moving the vehicle using :py:func:`simple_goto` (or other position-based movement commands). - The message will be sent immediately but may not be received - or acted on by the autopilot. Users can monitor :py:attr:`airspeed` - to determine if the command has had any effect. - - The API should be used as shown below: + The API **must** be used as shown below (or an exception will be raised): .. code-block:: python # Set the speed - vehicle.try_set_target_airspeed(5) - + vehicle.try_set_target_airspeed(5, wait_ready=False) + + Using ``wait_ready=False`` the message will be sent immediately but may not be received + or acted on by the autopilot. Users can monitor :py:attr:`airspeed` + to determine if the command has had any effect. + + .. note:: + + ``wait_ready=True`` (default) is not supported and if used will raise an exception. + This is inconvenient, but is done to future proof the API for when a robust + synchronous speed setter can be written. + + .. todo:: We should monitor for receipt of the message but there is no @@ -1971,7 +1986,10 @@ def try_set_target_airspeed(self, speed): :param float speed: The target speed. + :param Boolean wait_ready: Set ``False`` to send the command immediately. This is required as waiting version is not implemented. """ + if wait_ready == True: + raise APIException('try_set_target_airspeed() must be called with wait_ready=True (False not supported).') speed_type = 0 # air speed msg = self.message_factory.command_long_encode( 0, 0, # target system, target component @@ -2107,6 +2125,34 @@ def home_location(self, pos): 0, 0, 0, # params 2-4 pos.lat, pos.lon, pos.alt)) + + def try_set_home_location(self, pos): + """ + Attempts to set the home location (``LocationGlobal``). + + The value cannot be set until it has successfully been read from the vehicle. After being + set the value is *cached* in the home_location attribute and does not have to be re-read. + + .. note:: + + Setting the value will fail silently if the specified location is more than 50km from the EKF origin. + """ + + if not isinstance(pos, LocationGlobal): + raise Exception('Excepting home_location to be set to a LocationGlobal.') + + # Set cached home location. + self._home_location = copy.copy(pos) + + # Send MAVLink update. + self.send_mavlink(self.message_factory.command_long_encode( + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_DO_SET_HOME, # command + 0, # confirmation + 2, # param 1: 1 to use current position, 2 to use the entered values. + 0, 0, 0, # params 2-4 + pos.lat, pos.lon, pos.alt)) + @property def commands(self): """ diff --git a/examples/simple_goto/simple_goto.py b/examples/simple_goto/simple_goto.py index 553cbb0c8..cfc720afa 100644 --- a/examples/simple_goto/simple_goto.py +++ b/examples/simple_goto/simple_goto.py @@ -78,7 +78,7 @@ def arm_and_takeoff(aTargetAltitude): print "Set default/target airspeed to 3" -vehicle.try_set_target_airspeed(3) +vehicle.try_set_target_airspeed(3, wait_ready=False) print "Going towards first point for 30 seconds ..." point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) @@ -88,8 +88,7 @@ def arm_and_takeoff(aTargetAltitude): # sleep so we can see the change in map time.sleep(30) -print "Going towards second point for 30 seconds (groundspeed set to 10 m/s) ..." -vehicle.try_set_target_groundspeed(10) +print "Going towards second point for 30 seconds (groundspeed set to 10 m/s in goto) ..." point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) vehicle.simple_goto(point2, groundspeed=10) From 41153fbe001be66a2ad57517a971febb216eb8f1 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 18 Feb 2016 21:48:11 +1100 Subject: [PATCH 6/9] Add method for setting home_location --- dronekit/__init__.py | 86 +++++++++++++++++++------ examples/simple_goto/simple_goto.py | 5 -- examples/vehicle_state/vehicle_state.py | 13 +--- 3 files changed, 71 insertions(+), 33 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index c179c9744..f14fc68e7 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -2085,15 +2085,18 @@ def home_location(self): The ``home_location`` is not observable. - The attribute can be written (in the same way as any other attribute) after it has successfully - been populated from the vehicle. The value sent to the vehicle is cached in the attribute - (and can potentially get out of date if you don't re-download ``Vehicle.commands``): - - .. warning:: - - Setting the value will fail silently if the specified location is more than 50km from the EKF origin. + The attribute can be written using :py:func:`try_set_home_location` after it has successfully + been populated from the vehicle. - + .. warning:: + + This setter is deprecated. Instead use :py:func:`try_set_home_location`. + + If you use this method the value sent to the vehicle is cached in the attribute + (and can potentially get out of date if you don't re-download ``Vehicle.commands``): + + Setting the value with this method will fail silently + if the specified location is more than 50km from the EKF origin. """ return copy.copy(self._home_location) @@ -2108,10 +2111,14 @@ def home_location(self, pos): .. note:: Setting the value will fail silently if the specified location is more than 50km from the EKF origin. + + .. warning:: + + This setter is deprecated. Instead use :py:func:`try_set_home_location`. """ if not isinstance(pos, LocationGlobal): - raise Exception('Excepting home_location to be set to a LocationGlobal.') + raise Exception('Expecting home_location to be set to a LocationGlobal.') # Set cached home location. self._home_location = copy.copy(pos) @@ -2124,25 +2131,56 @@ def home_location(self, pos): 2, # param 1: 1 to use current position, 2 to use the entered values. 0, 0, 0, # params 2-4 pos.lat, pos.lon, pos.alt)) + - def try_set_home_location(self, pos): + def try_set_home_location(self, pos, wait_ready=True): """ Attempts to set the home location (``LocationGlobal``). - The value cannot be set until it has successfully been read from the vehicle. After being - set the value is *cached* in the home_location attribute and does not have to be re-read. + The value cannot be set until it has successfully been read from the vehicle. + Setting the value will also fail if the target location is more than 50km from the EKF origin. + + The method can be called with ``wait_ready=True`` (the default) as shown: + + .. code-block:: python + + a_location = LocationGlobal(-34.364114, 149.166022, 30) + vehicle.try_set_home_location(a_location) #Returns if home_location is verified as set + + This verifies the home location was changed by re-downloading the home location + and comparing it to the original value. It raises an exception if the values do not match. + + Calling the method with ``wait_ready=False`` will try and send the home location value to the autopilot. + This can fail silently and readers will need to write code (as below) to populate the :py:attr:`home_location` + and confirm the value is correct. + + .. code-block:: python + + a_location = LocationGlobal(-34.364114, 149.166022, 30) + # Set home location + vehicle.try_set_home_location(a_location, wait_ready=False) #Returns immediately + + # Get new value of home location by re-downloading commands + cmds = vehicle.commands + cmds.download() + cmds.wait_ready() + print " New Home Location (from vehicle): %s" % vehicle.home_location .. note:: - Setting the value will fail silently if the specified location is more than 50km from the EKF origin. + The ``wait_ready=True`` version verifies the value has changed but can take a long time to complete + if the autopilot has a large mission (On ArduPilot the home_location is stored in the mission data). + By contrast the other method is fast, but can silently fail. + + :param LocationGlobal pos: The `LocationGlobal` to set as home location. + :param Boolean wait_ready: ``True`` (default) to wait until the + home location has been re-downloaded/confirmed before returning. Set ``False`` to just set the home + location and then return. """ if not isinstance(pos, LocationGlobal): - raise Exception('Excepting home_location to be set to a LocationGlobal.') - - # Set cached home location. - self._home_location = copy.copy(pos) + raise Exception('Expecting home_location to be set to a LocationGlobal.') # Send MAVLink update. self.send_mavlink(self.message_factory.command_long_encode( @@ -2152,7 +2190,19 @@ def try_set_home_location(self, pos): 2, # param 1: 1 to use current position, 2 to use the entered values. 0, 0, 0, # params 2-4 pos.lat, pos.lon, pos.alt)) - + + if wait_ready==True: + old_home_location=self.home_location + cmds = self.commands + cmds.download() + cmds.wait_ready() + # Test that home location has changed for success. + # Would be better to test that new home location is same as pos + # but that is subject to rounding errors so direct comparion difficult. + if old_home_location==self.home_location: + raise Exception('Setting home location failed. Can only be called after home first set by GPS.') + + @property def commands(self): """ diff --git a/examples/simple_goto/simple_goto.py b/examples/simple_goto/simple_goto.py index cfc720afa..093b756dc 100644 --- a/examples/simple_goto/simple_goto.py +++ b/examples/simple_goto/simple_goto.py @@ -10,11 +10,6 @@ import time - - - - - #Set up option parsing to get connection string import argparse parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.') diff --git a/examples/vehicle_state/vehicle_state.py b/examples/vehicle_state/vehicle_state.py index 435083067..8ce005791 100644 --- a/examples/vehicle_state/vehicle_state.py +++ b/examples/vehicle_state/vehicle_state.py @@ -99,18 +99,11 @@ # Set vehicle home_location, mode, and armed attributes (the only settable attributes) print "\nSet new home location" -# Home location must be within 50km of EKF home location (or setting will fail silently) -# In this case, just set value to current location with an easily recognisable altitude (222) +# Set home location to current location but change altitude to 222) my_location_alt = vehicle.location.global_frame my_location_alt.alt = 222.0 -vehicle.home_location = my_location_alt -print " New Home Location (from attribute - altitude should be 222): %s" % vehicle.home_location - -#Confirm current value on vehicle by re-downloading commands -cmds = vehicle.commands -cmds.download() -cmds.wait_ready() -print " New Home Location (from vehicle - altitude should be 222): %s" % vehicle.home_location +vehicle.try_set_home_location(my_location_alt) +print " New Home Location (altitude should be 222): %s" % vehicle.home_location print "\nSet Vehicle mode to GUIDED (currently: %s)" % vehicle.mode.name From 1bf9b6a2d1d2303c13358300b9e5cf906412d9a9 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 14 Apr 2016 15:05:33 +1000 Subject: [PATCH 7/9] New mode setter sync function --- dronekit/__init__.py | 56 +++++++++++++++++++++++++++++--------------- 1 file changed, 37 insertions(+), 19 deletions(-) diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 270771184..768680000 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1540,7 +1540,8 @@ def mode(self): # Print the vehicle mode name print "Mode name: %s" % vehicle.mode.name - The value of the attribute can be set using :py:func:`try_set_mode`. + The value of the attribute can be changed using :py:func:`sync_set_mode` or + :py:func:`set_mode`. .. warning:: @@ -1556,39 +1557,60 @@ def mode(self, v): self._master.set_mode(self._mode_mapping[v.name]) - def try_set_mode(self, value, wait_ready=True, retries=2, timeout=3): + def set_mode(self, value): """ - Send a message to change the vehicle mode. + Send a message to attempt to change the vehicle mode and return immediately (without checking the result). + + .. warning:: + + It is possible for the message to be lost or ignored by the connected vehicle. + If using this method you may need to explicitly check that the mode has changed. + + .. tip:: + + Use :py:func:`sync_set_mode` to robustly wait for the mode to change before continuing. + + The message is not sent if the target mode is the current mode. + + :param VehicleMode value: The target :py:class:`VehicleMode` (or mode name, as a string). + """ + # Allow users to specify the target mode as a string. + if type(value)==str: + value=VehicleMode(value) + # Return immediately if target value is current value. + if value.name == self.mode.name: + return + + #If method is non waiting send command immediately. + self._master.set_mode(self._mode_mapping[value.name]) + + + def sync_set_mode(self, value, retries=2, timeout=3): + """ + Send a message to change the vehicle mode and synchronously wait for success. The API should primarily be used as shown below: .. code-block:: python # Set the mode using a VehicleMode - vehicle.try_set_mode(VehicleMode("GUIDED")) + vehicle.sync_set_mode(VehicleMode("GUIDED")) # Set the mode using a string - vehicle.try_set_mode("STABILIZE") + vehicle.sync_set_mode("STABILIZE") - When used in this way the method will send a message (with retries) to set the new mode + The method will sends a message (with retries) to set the new mode and will either return when the :py:attr:`mode` has changed or raise an exception on failure. You can also set the number of retries and the timeout between re-sending the message, if needed. The function will return immediately if the new value is the same as the current value. - Setting ``wait_ready=False`` sends the message and then returns immediately - (without checking the result). - .. tip:: - We recommend the default (``wait_ready=True``) because it safely implements - the checking and resending that you would otherwise have to do yourself to determine when the - mode has changed. + The :py:func:`set_mode` method should be used if you don't want to synchronously wait for the mode change. :param VehicleMode value: The new :py:class:`VehicleMode` (or mode name, as a string). - :param Bool wait_ready: ``True`` (default) wait until the value has changed before completing. - ``False`` to send the request and complete immediately. :param int retries: Number of attempts to resend the message. :param int timeout: Time to wait for the value to change before retrying/exiting (in seconds). """ @@ -1598,11 +1620,6 @@ def try_set_mode(self, value, wait_ready=True, retries=2, timeout=3): # Return immediately if target value is current value. if value.name == self.mode.name: return - - #If method is non waiting send command immediately. - if wait_ready==False: - self._master.set_mode(self._mode_mapping[value.name]) - return #Otherwise execute retry code for retry_num in range(0,retries): @@ -1616,6 +1633,7 @@ def try_set_mode(self, value, wait_ready=True, retries=2, timeout=3): #No more retries. Raise exception. raise APIException('Unable to change mode.') + @property def location(self): """ From 5025a863372d8c2231667f651fee374eb559a4f8 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Tue, 19 Apr 2016 15:44:47 +1000 Subject: [PATCH 8/9] Add set_ and set_sync_ methods for the settalbe attributes and update docs --- docs/examples/vehicle_state.rst | 57 ++-- docs/guide/vehicle_state_and_parameters.rst | 109 +++++--- dronekit/__init__.py | 280 +++++++++++--------- dronekit/test/sitl/test_110.py | 6 +- dronekit/test/sitl/test_115.py | 6 +- examples/vehicle_state/vehicle_state.py | 64 +++-- 6 files changed, 318 insertions(+), 204 deletions(-) diff --git a/docs/examples/vehicle_state.rst b/docs/examples/vehicle_state.rst index 64a367f39..78a9b0bb4 100644 --- a/docs/examples/vehicle_state.rst +++ b/docs/examples/vehicle_state.rst @@ -95,23 +95,46 @@ In summary, after cloning the repository: Home location: LocationGlobal:lat=-35.3632621765,lon=149.165237427,alt=583.989990234 - Set new home location - New Home Location (from attribute - altitude should be 222): LocationGlobal:lat=-35.363261,lon=149.1652299,alt=222 - New Home Location (from vehicle - altitude should be 222): LocationGlobal:lat=-35.3632621765,lon=149.165237427,alt=222.0 - - Set Vehicle.mode=GUIDED (currently: STABILIZE) - Waiting for mode change ... - - Set Vehicle.armed=True (currently: False) - Waiting for arming... - Waiting for arming... - Waiting for arming... - >>> ARMING MOTORS - >>> GROUND START - Waiting for arming... - Waiting for arming... - >>> Initialising APM... - Vehicle is armed: True + Set new home location using synchronous function (returns when value confirmed changed) + New Home Location (altitude should be 222): LocationGlobal:lat=-35.3632621765,lon=149.165237427,alt=222.0 + + Set new home location using asynchronous function (returns immediately) + New Home Location (from vehicle - should be 333.0): LocationGlobal:lat=-35.3632621765,lon=149.165237427,alt=333.0 + + Set Vehicle mode to GUIDED using synchronous setter (currently: STABILIZE) + New mode: GUIDED + + Set Vehicle mode to STABILIZE using synchronous setter and 'mode string' (currently: GUIDED) + New mode: STABILIZE + + Set Vehicle.mode = GUIDED using asynchronous setter (currently: STABILIZE) + Waiting for mode change... + Waiting for mode change... + Waiting for mode change... + Waiting for mode change... + New mode: GUIDED + + Send arm message and return when value changed (currently: False) + >>> ARMING MOTORS + >>> GROUND START + >>> Initialising APM... + Vehicle is armed: True + + Send disarm message and return when value changed (currently: True) + >>> DISARMING MOTORS + Vehicle is armed: False + + Send arm message and poll in loop for value to change (currently: False) + Waiting for arming... + >>> ARMING MOTORS + Waiting for arming... + Waiting for arming... + Waiting for arming... + Vehicle is armed: True + + Send 'set target groundspeed' message (currently: 0.0) + + Set 'set target airspeed' message (currently: 0.0) Add `attitude` attribute callback/observer on `vehicle` Wait 2s so callback invoked before observer removed diff --git a/docs/guide/vehicle_state_and_parameters.rst b/docs/guide/vehicle_state_and_parameters.rst index 1bb23ddde..2cbf21dd9 100644 --- a/docs/guide/vehicle_state_and_parameters.rst +++ b/docs/guide/vehicle_state_and_parameters.rst @@ -79,8 +79,6 @@ regularly updated from MAVLink messages sent by the vehicle). print "Attitude: %s" % vehicle.attitude print "Velocity: %s" % vehicle.velocity print "GPS: %s" % vehicle.gps_0 - print "Groundspeed: %s" % vehicle.groundspeed - print "Airspeed: %s" % vehicle.airspeed print "Gimbal status: %s" % vehicle.gimbal print "Battery: %s" % vehicle.battery print "EKF OK?: %s" % vehicle.ekf_ok @@ -91,6 +89,8 @@ regularly updated from MAVLink messages sent by the vehicle). print "Heading: %s" % vehicle.heading print "Is Armable?: %s" % vehicle.is_armable print "System status: %s" % vehicle.system_status.state + print "Groundspeed: %s" % vehicle.groundspeed # settable + print "Airspeed: %s" % vehicle.airspeed # settable print "Mode: %s" % vehicle.mode.name # settable print "Armed: %s" % vehicle.armed # settable @@ -119,39 +119,76 @@ regularly updated from MAVLink messages sent by the vehicle). Setting attributes ------------------ -The :py:attr:`Vehicle.mode `, :py:attr:`Vehicle.armed ` -, :py:attr:`Vehicle.airspeed ` and :py:attr:`Vehicle.groundspeed `, -attributes can all be "directly" written (:py:attr:`Vehicle.home_location ` can also be directly written, +The :py:attr:`Vehicle.mode `, :py:attr:`Vehicle.armed `, +:py:attr:`Vehicle.airspeed ` and :py:attr:`Vehicle.groundspeed `, +attributes can all be written using setter functions (:py:attr:`Vehicle.home_location ` can also be written, but has special considerations that are :ref:`discussed below `). -These attributes are set by assigning a value: +The setter functions generally take two forms: -.. code:: python +* Setters with the name prefix ``set_`` simply send the associated MAVLink message to set the attribute and then return. + These include: :py:func:`set_mode() `, :py:func:`set_armed() `, + :py:func:`set_home_location() `, :py:func:`set_target_groundspeed() `, + :py:func:`set_target_airspeed() `. +* Setters with the name prefix ``sync_set_`` are "synchronous" - they send the message (with retries) and + either return when the value has changed or raise an exception. These include: :py:func:`sync_set_armed() `, + :py:func:`sync_set_mode() `, :py:func:`sync_set_home_location() `. + + .. note:: - #disarm the vehicle - vehicle.armed = False - - #set the default groundspeed to be used in movement commands - vehicle.groundspeed = 3.2 + Not every setter has a ``sync_set_`` variant. For example, we could not create ``set_sync_target_airspeed()`` because + there is no reliable way to check whether the message has been received. +.. warning:: -Commands to change a value are **not guaranteed to succeed** (or even to be received) and code should be written with this in mind. -For example, the code snippet below polls the attribute values to confirm they have changed before proceeding. + It is also possible to set the value of most of the *settable attributes* by directly assigning a value to the attribute itself. + This form is deprecated because it creates the incorrect impression for developers that the write operation must succeed, and confusion + when a re-read value does not match a just-set value. This feature will be removed in a future version. + +Using the synchronous version is often preferred because the methods are guaranteed to succeed or fail in an obvious way. +Commands to change a value are **not guaranteed to succeed** (or even to be received) so if you use the asynchronous version +you may have to write your own code to check for success. On the other hand, the ``set_`` methods are more flexible, and are useful +for developers who prefer to set values and use observers to drive the program flow. + + +The code snippet below shows the use of the synchronous functions: .. code:: python + + # Set mode using synchronous function + vehicle.sync_set_mode(VehicleMode("GUIDED")) + print " New mode: %s" % vehicle.mode.name - vehicle.mode = VehicleMode("GUIDED") - vehicle.armed = True - while not vehicle.mode.name=='GUIDED' and not vehicle.armed and not api.exit: - print " Getting ready to take off ..." + # Check that vehicle is armable (required) + while not vehicle.is_armable: + print " Waiting for vehicle to initialise..." time.sleep(1) + # If required, you can provide additional information about initialisation + # using `vehicle.gps_0.fix_type` and `vehicle.mode.name`. + + # Arm the vehicle (return when armed) + vehicle.sync_set_armed() #returns when armed. + +The code snippet below shows a similar use of the ``set_`` functions: -.. note:: +.. code:: python + + # Check that vehicle is armable (required) + while not vehicle.is_armable: + print " Waiting for vehicle to initialise..." + time.sleep(1) - While the autopilot does send information about the success (or failure) of the request, - this is `not currently handled by DroneKit `_. + #set mode and armed at same time + vehicle.set_mode(VehicleMode("GUIDED")) + vehicle.set_armed() -:py:attr:`Vehicle.gimbal ` can't be written directly, but the gimbal can be controlled using the + while not vehicle.armed: + print " Waiting for arming..." + time.sleep(0.3) + #vehicle not armed. + + +The gimbal (:py:attr:`Vehicle.gimbal `) can be controlled using the :py:func:`Vehicle.gimbal.rotate() ` and :py:func:`Vehicle.gimbal.target_location() ` methods. The first method lets you set the precise orientation of the gimbal while the second makes the gimbal track a specific "region of interest". @@ -312,21 +349,21 @@ waypoints is set relative to this position. # We have a home location. print "\n Home location: %s" % vehicle.home_location -* The attribute can be *set* to a :py:class:`LocationGlobal ` object - (the code fragment below sets it to the current location): +* The home location can be *set* to a :py:class:`LocationGlobal ` object using + :py:attr:`Vehicle.set_home_location ` or + :py:attr:`Vehicle.sync_set_home_location `. + + Both variants require that the home location have previously been read as shown above and the write will + fail if the new location is not within 50 km of the EKF origin. + + The main difference is that the ``sync_set`` variant re-downloads the home location in order to verify that + the value actually changed. This is more "robust" and a lot easier to use, but can take a long time to complete: .. code:: python - vehicle.home_location=vehicle.location.global_frame - - There are some caveats: - - * You must be able to read a non-``None`` value before you can write it - (the autopilot has to set the value initially before it can be written or read). - * The new location must be within 50 km of the EKF origin or setting the value will silently fail. - * The value is cached in the ``home_location``. If the variable can potentially change on the vehicle - you will need to re-download the ``Vehicle.commands`` in order to confirm the value. - + # Set the home location to the current location + vehicle.sync_set_home_location(vehicle.location.global_frame) + * The attribute is not observable. @@ -339,6 +376,7 @@ waypoints is set relative to this position. We hope to improve this attribute in later versions of ArduPilot, where there may be specific commands to get the home location from the vehicle. + .. _vehicle_state_parameters: @@ -451,9 +489,6 @@ for "any parameter") using the vehicle.parameters.add_attribute_listener('*', any_parameter_callback) - - - .. _api-information-known-issues: Known issues diff --git a/dronekit/__init__.py b/dronekit/__init__.py index 768680000..681195704 100644 --- a/dronekit/__init__.py +++ b/dronekit/__init__.py @@ -1545,8 +1545,8 @@ def mode(self): .. warning:: - The attribute can also be used to *set* the mode. - This is deprecated, and may be removed in a future version. + This attribute can also be used to directly *set* the mode. + This approach is deprecated, and may be removed in a future version. """ if not self._flightmode: return None @@ -1599,7 +1599,7 @@ def sync_set_mode(self, value, retries=2, timeout=3): # Set the mode using a string vehicle.sync_set_mode("STABILIZE") - The method will sends a message (with retries) to set the new mode + The method will send a message (with retries) to set the new mode and will either return when the :py:attr:`mode` has changed or raise an exception on failure. You can also set the number of retries and the timeout between re-sending the message, if needed. @@ -1761,7 +1761,7 @@ def armed(self): # Print the armed state for the vehicle print "Armed: %s" % vehicle.armed - To arm/disarm the vehicle use :py:func:`try_set_armed`. + To arm/disarm the vehicle use :py:func:`sync_set_armed` or :py:func:`set_armed`. .. warning:: @@ -1779,22 +1779,58 @@ def armed(self, value): self._master.arducopter_disarm() - def try_set_armed(self, value=True, wait_ready=True, retries=2, timeout=3): + def set_armed(self, value=True): """ - Send a message to arm (default) or disarm the vehicle. + Send a message to attempt to change the :py:attr:`armed` state (armed/disarmed) of the vehicle and then return immediately (without checking the result). + + .. warning:: + + It is possible for the message to be lost or ignored by the connected vehicle. + If using this method you may need to explicitly check that the armed state has changed. + + .. tip:: + + Use :py:func:`sync_set_armed` to synchronously wait for the armed state to change before continuing. + + The message is not sent if the target armed state is the current state. The method will raise an exception + if called to arm the vehicle when it is not armable (see :py:attr:`is_armable`). + + :param Bool value: ``True`` (default) to arm, ``False`` to disarm. + """ + # Return immediately if target value is current value. + if bool(value) == self._armed: + return + + # Raise exception if attempting to arm when not armable. + if self._armed==False and not self.is_armable and bool(value)==True: + raise APIException('Attempting to arm when not armable.') + + #Send arm or disarm command then return. + if value: + self._master.arducopter_arm() + else: + self._master.arducopter_disarm() + + + + + + def sync_set_armed(self, value=True, wait_ready=True, retries=2, timeout=3): + """ + Send a message to arm (default) or disarm the vehicle and synchronously wait for success. The API should primarily be used as shown below: .. code-block:: python - # Arm the vehicle + # Arm the vehicle (first checking it is armable) if vehicle.is_armable: //Required! - vehicle.try_set_armed() #returns when armed + vehicle.sync_set_armed() #returns when armed # Disarm the vehicle - vehicle.try_set_armed(value=False) #returns when disarmed - - When used in this way the method will send a message (with retries) to arm/disarm + vehicle.sync_set_armed(value=False) #returns when disarmed + + The method will send a message (with retries) to arm/disarm the vehicle and will either return when the :py:attr:`armed` state has changed or raise an exception on failure. You can also set the number of retries and the timeout between re-sending the message, if needed. @@ -1802,19 +1838,9 @@ def try_set_armed(self, value=True, wait_ready=True, retries=2, timeout=3): The function will return immediately if the new value is the same as the current value. The function will raise an exception if called to arm a vehicle that is not armable (see :py:attr:`is_armable`). - - Setting ``wait_ready=False`` sends the message and then returns immediately - (without checking the result). - - .. tip:: - - We recommend the default (``wait_ready=True``) because it safely implements all - the checking that you would otherwise have to do yourself to determine when the - armed state has changed. + :param Bool value: ``True`` (default) to arm, ``False`` to disarm. - :param Bool wait_ready: ``True`` (default) wait until the value has changed before completing. - ``False`` to send the request and complete immediately. :param int retries: Number of attempts to resend the message. :param int timeout: Time to wait for the value to change before retrying/exiting (in seconds). """ @@ -1822,19 +1848,11 @@ def try_set_armed(self, value=True, wait_ready=True, retries=2, timeout=3): if bool(value) == self._armed: return - # Invoke callback or raise exception if attempting to arm when not armable. + # Raise exception if attempting to arm when not armable. if self._armed==False and not self.is_armable and bool(value)==True: raise APIException('Attempting to arm when not armable.') - - #If method is non waiting send command immediately. - if wait_ready==False: - if value: - self._master.arducopter_arm() - else: - self._master.arducopter_disarm() - return - #Otherwise execute retry code + #Execute retry code for retry_num in range(0,retries): if value: self._master.arducopter_arm() @@ -1848,12 +1866,13 @@ def try_set_armed(self, value=True, wait_ready=True, retries=2, timeout=3): #No more retries. Raise exception. raise APIException('Unable to arm.') - + + @property def is_armable(self): """ - Returns ``True`` if the vehicle is ready to :py:func:`arm `, ``false`` otherwise (``Boolean``). + Returns ``True`` if the vehicle is ready to :py:attr:`arm `, ``false`` otherwise (``Boolean``). This attribute wraps a number of pre-arm checks, ensuring that the vehicle has booted, has a good GPS fix, and that the EKF pre-arm is complete. @@ -1903,7 +1922,7 @@ def groundspeed(self): """ Current groundspeed in metres/second (``double``). - To set the groundspeed use :py:func:`try_set_target_groundspeed`. + To set the groundspeed use :py:func:`set_target_groundspeed`. .. warning:: @@ -1928,41 +1947,33 @@ def groundspeed(self, speed): self.send_mavlink(msg) - def try_set_target_groundspeed(self, speed, wait_ready=True): + def set_target_groundspeed(self, speed): """ - Send a message to change the vehicle groundspeed. The set value is the - default target speed when moving the vehicle using :py:func:`simple_goto` + Send a message to change the target vehicle :py:attr:`groundspeed` and return immediately (without checking the result). + + The set value is the default target speed when moving the vehicle using :py:func:`simple_goto` (or other position-based movement commands). - The API **must** be used as shown below (or an exception will be raised): + The API should be used as shown below: .. code-block:: python # Set the speed - vehicle.try_set_target_groundspeed(5, wait_ready=False) - - Using ``wait_ready=False`` the message will be sent immediately but may not be received - or acted on by the autopilot. Users can monitor :py:attr:`groundseed` - to determine if the command has had any effect. - - .. note:: - - ``wait_ready=True`` (default) is not supported and will raise an exception. - This is inconvenient, but is done to future proof the API for when a robust - synchronous speed setter can be written. + vehicle.set_target_groundspeed(5) - .. todo:: + .. warning:: - We should monitor for receipt of the message but there is no - robust way to trust the COMMAND_ACK and no method to - read back the current speed setting: - https://github.com/diydrones/ardupilot/issues/3636 + It is possible for the message to be lost or ignored by the connected vehicle. + If using this method you may need to explicitly check that the speed has changed. + .. note:: + + We do not provide a synchronous setter that confirms successful update of the groundspeed value. + This is because there is no robust way to trust the COMMAND_ACK and currently no method + to read back the current speed setting: https://github.com/diydrones/ardupilot/issues/3636 :param float speed: The target speed. """ - if wait_ready == True: - raise APIException('try_set_target_groundspeed() must be called with wait_ready=True (False not supported).') speed_type = 1 # ground speed msg = self.message_factory.command_long_encode( 0, 0, # target system, target component @@ -1981,7 +1992,7 @@ def airspeed(self): """ Current airspeed in metres/second (``double``). - To set the airspeed use :py:func:`try_set_target_airspeed`. + To set the airspeed use :py:func:`set_target_airspeed`. .. warning:: @@ -2006,43 +2017,33 @@ def airspeed(self, speed): self.send_mavlink(msg) - def try_set_target_airspeed(self, speed, wait_ready=True): + def set_target_airspeed(self, speed): """ - Send a message to change the vehicle airspeed. The set value is the - default target airspeed when moving the vehicle using :py:func:`simple_goto` + Send a message to change the target vehicle :py:attr:`airspeed` and return immediately (without checking the result). + + The set value is the default target speed when moving the vehicle using :py:func:`simple_goto` (or other position-based movement commands). - The API **must** be used as shown below (or an exception will be raised): + The API should be used as shown below: .. code-block:: python # Set the speed - vehicle.try_set_target_airspeed(5, wait_ready=False) - - Using ``wait_ready=False`` the message will be sent immediately but may not be received - or acted on by the autopilot. Users can monitor :py:attr:`airspeed` - to determine if the command has had any effect. + vehicle.set_target_airspeed(5) + + .. warning:: + It is possible for the message to be lost or ignored by the connected vehicle. + If using this method you may need to explicitly check that the speed has changed. + .. note:: - ``wait_ready=True`` (default) is not supported and if used will raise an exception. - This is inconvenient, but is done to future proof the API for when a robust - synchronous speed setter can be written. - - - .. todo:: - - We should monitor for receipt of the message but there is no - robust way to trust the COMMAND_ACK and no method to - read back the current speed setting: - https://github.com/diydrones/ardupilot/issues/3636 - + We do not provide a synchronous setter that confirms successful update of the airspeed value. + This is because there is no robust way to trust the COMMAND_ACK and currently no method + to read back the current speed setting: https://github.com/diydrones/ardupilot/issues/3636 :param float speed: The target speed. - :param Boolean wait_ready: Set ``False`` to send the command immediately. This is required as waiting version is not implemented. """ - if wait_ready == True: - raise APIException('try_set_target_airspeed() must be called with wait_ready=True (False not supported).') speed_type = 0 # air speed msg = self.message_factory.command_long_encode( 0, 0, # target system, target component @@ -2138,18 +2139,17 @@ def home_location(self): The ``home_location`` is not observable. - The attribute can be written using :py:func:`try_set_home_location` after it has successfully - been populated from the vehicle. + The attribute can be written using :py:func:`sync_set_home_location` or :py:func:`set_home_location` + *after* it has successfully been populated from the vehicle. .. warning:: - This setter is deprecated. Instead use :py:func:`try_set_home_location`. + This setter is deprecated. Instead use :py:func:`sync_set_home_location` or :py:func:`set_home_location`. If you use this method the value sent to the vehicle is cached in the attribute (and can potentially get out of date if you don't re-download ``Vehicle.commands``): - Setting the value with this method will fail silently - if the specified location is more than 50km from the EKF origin. + Setting the value with this attribute will fail silently if the specified location is more than 50km from the EKF origin. """ return copy.copy(self._home_location) @@ -2167,7 +2167,7 @@ def home_location(self, pos): .. warning:: - This setter is deprecated. Instead use :py:func:`try_set_home_location`. + This setter is deprecated. Instead use :py:func:`sync_set_home_location` or :py:func:`set_home_location`. """ if not isinstance(pos, LocationGlobal): @@ -2186,33 +2186,25 @@ def home_location(self, pos): pos.lat, pos.lon, pos.alt)) - - def try_set_home_location(self, pos, wait_ready=True): + def set_home_location(self, pos): """ - Attempts to set the home location (``LocationGlobal``). - - The value cannot be set until it has successfully been read from the vehicle. - Setting the value will also fail if the target location is more than 50km from the EKF origin. - - The method can be called with ``wait_ready=True`` (the default) as shown: - - .. code-block:: python + Sends a message to attempt to set the home location (``LocationGlobal``) and returns immediately (without checking for success). - a_location = LocationGlobal(-34.364114, 149.166022, 30) - vehicle.try_set_home_location(a_location) #Returns if home_location is verified as set + Setting the value can fail silently: - This verifies the home location was changed by re-downloading the home location - and comparing it to the original value. It raises an exception if the values do not match. + * The value cannot be set until after the autopilot has first set the value (on getting GPS). + * The value cannot be set until after it has successfully been read from the vehicle. + * Setting the value will silently fail if the target location is more than 50km from the EKF origin. - Calling the method with ``wait_ready=False`` will try and send the home location value to the autopilot. - This can fail silently and readers will need to write code (as below) to populate the :py:attr:`home_location` - and confirm the value is correct. + The method will raise an exception if it is called with anything other than a ``LocationGlobal``. + It should called as shown below (this example re-downloads the home location so you can verify the value has changed): + .. code-block:: python a_location = LocationGlobal(-34.364114, 149.166022, 30) # Set home location - vehicle.try_set_home_location(a_location, wait_ready=False) #Returns immediately + vehicle.set_home_location(a_location) #Returns immediately # Get new value of home location by re-downloading commands cmds = vehicle.commands @@ -2220,16 +2212,14 @@ def try_set_home_location(self, pos, wait_ready=True): cmds.wait_ready() print " New Home Location (from vehicle): %s" % vehicle.home_location - .. note:: + .. tip:: - The ``wait_ready=True`` version verifies the value has changed but can take a long time to complete + The :py:func:`sync_set_home_location` method can be use to synchronously set the home location and then + return when the value change is confirmed. This method can take a long time to complete if the autopilot has a large mission (On ArduPilot the home_location is stored in the mission data). - By contrast the other method is fast, but can silently fail. + By contrast, this method is fast, but can silently fail. :param LocationGlobal pos: The `LocationGlobal` to set as home location. - :param Boolean wait_ready: ``True`` (default) to wait until the - home location has been re-downloaded/confirmed before returning. Set ``False`` to just set the home - location and then return. """ if not isinstance(pos, LocationGlobal): @@ -2244,16 +2234,62 @@ def try_set_home_location(self, pos, wait_ready=True): 0, 0, 0, # params 2-4 pos.lat, pos.lon, pos.alt)) - if wait_ready==True: - old_home_location=self.home_location - cmds = self.commands - cmds.download() - cmds.wait_ready() - # Test that home location has changed for success. - # Would be better to test that new home location is same as pos - # but that is subject to rounding errors so direct comparion difficult. - if old_home_location==self.home_location: - raise Exception('Setting home location failed. Can only be called after home first set by GPS.') + + + + def sync_set_home_location(self, pos, wait_ready=True): + """ + Sends a message to attempt to set the :py:attr:`home_location` and returns once the value has changed (or raises an exception). + + .. warning:: + + * The home location must already have been downloaded from the vehicle before this method is called. + * The method re-downloads the home location to verify the value has changed (note - not that it matches the set value!) + This can be a long-running operation if the autopilot has a large mission + (on ArduPilot the home location is stored as part of the mission data.) + + The method will raise an exception if it is called with anything other than a ``LocationGlobal`` or if the + value is not detected as having changed. Possible reasons for failure include: + + * Home location cannot be set until after the autopilot has first set the value (on getting GPS). + * Home location cannot be set using this method until after it has successfully been read from the vehicle. + * Home location must be within 50km of the EKF origin. + + The method can be called as shown: + + .. code-block:: python + + ... + #The home_location must already have been read at least once before calling this method + ... + + a_location = LocationGlobal(-34.364114, 149.166022, 30) + vehicle.sync_set_home_location(a_location) #Returns if home_location value has changed + + :param LocationGlobal pos: The `LocationGlobal` to set as home location. + """ + + if not isinstance(pos, LocationGlobal): + raise Exception('Expecting home_location to be set to a LocationGlobal.') + + # Send MAVLink update. + self.send_mavlink(self.message_factory.command_long_encode( + 0, 0, # target system, target component + mavutil.mavlink.MAV_CMD_DO_SET_HOME, # command + 0, # confirmation + 2, # param 1: 1 to use current position, 2 to use the entered values. + 0, 0, 0, # params 2-4 + pos.lat, pos.lon, pos.alt)) + + old_home_location=self.home_location + cmds = self.commands + cmds.download() + cmds.wait_ready() + # Test that home location has changed for success. + # Would be better to test that new home location is same as pos + # but that is subject to rounding errors so direct comparion difficult. + if old_home_location==self.home_location: + raise Exception('Setting home location failed. Can only be called after home first set by GPS.') @property diff --git a/dronekit/test/sitl/test_110.py b/dronekit/test/sitl/test_110.py index 8386ba0b2..3c70ca59d 100644 --- a/dronekit/test/sitl/test_110.py +++ b/dronekit/test/sitl/test_110.py @@ -20,7 +20,7 @@ def test_110(connpath): wait_for(lambda : vehicle.is_armable, 60) # Change the vehicle into STABILIZE mode - vehicle.mode = VehicleMode("GUIDED") + vehicle.set_mode(VehicleMode("GUIDED")) # NOTE wait crudely for ACK on mode update time.sleep(3) @@ -40,7 +40,7 @@ def armed_callback(vehicle, attribute, value): vehicle.add_attribute_listener('armed', armed_callback) # arm and see update. - vehicle.armed = True + vehicle.set_armed() # Wait for ACK. wait_for(lambda : armed_callback.called, 10) @@ -58,7 +58,7 @@ def armed_callback(vehicle, attribute, value): callcount = armed_callback.called # Disarm and see update. - vehicle.armed = False + vehicle.set_armed(False) # Wait for ack time.sleep(3) diff --git a/dronekit/test/sitl/test_115.py b/dronekit/test/sitl/test_115.py index 28f4a3f11..fb050c341 100644 --- a/dronekit/test/sitl/test_115.py +++ b/dronekit/test/sitl/test_115.py @@ -20,7 +20,7 @@ def mavlink_callback(*args): v.add_message_listener('*', mavlink_callback) # Change the vehicle into STABILIZE mode - v.mode = VehicleMode("STABILIZE") + v.set_mode(VehicleMode("STABILIZE")) # NOTE wait crudely for ACK on mode update time.sleep(3) @@ -32,7 +32,7 @@ def mavlink_callback(*args): savecount = mavlink_callback.count # Disarm. A callback of None should not throw errors - v.armed = False + v.set_mode(False) # NOTE wait crudely for ACK on mode update time.sleep(3) @@ -40,7 +40,7 @@ def mavlink_callback(*args): assert_equals(savecount, mavlink_callback.count) # Re-arm should not throw errors. - v.armed = True + v.set_armed() # NOTE wait crudely for ACK on mode update time.sleep(3) diff --git a/examples/vehicle_state/vehicle_state.py b/examples/vehicle_state/vehicle_state.py index 8ce005791..df76c00f7 100644 --- a/examples/vehicle_state/vehicle_state.py +++ b/examples/vehicle_state/vehicle_state.py @@ -96,26 +96,39 @@ print "\n Home location: %s" % vehicle.home_location -# Set vehicle home_location, mode, and armed attributes (the only settable attributes) +# Set vehicle home_location, mode, armed, groundspeed and airspeed attributes (the only settable attributes) -print "\nSet new home location" +print "\nSet new home location using synchronous function (returns when value confirmed changed)" # Set home location to current location but change altitude to 222) -my_location_alt = vehicle.location.global_frame -my_location_alt.alt = 222.0 -vehicle.try_set_home_location(my_location_alt) +my_location = vehicle.location.global_frame +my_location.alt = 222.0 +vehicle.sync_set_home_location(my_location) print " New Home Location (altitude should be 222): %s" % vehicle.home_location +print "\nSet new home location using asynchronous function (returns immediately)" +my_location = vehicle.location.global_frame +# Set home location to current location but change altitude to 333) +my_location.alt = 333.0 +vehicle.set_home_location(my_location) #Returns immediately -print "\nSet Vehicle mode to GUIDED (currently: %s)" % vehicle.mode.name -vehicle.try_set_mode(VehicleMode("GUIDED")) +# Get new value of home location by re-downloading commands +cmds = vehicle.commands +cmds.download() +cmds.wait_ready() +print " New Home Location (from vehicle - should be 333.0): %s" % vehicle.home_location + + + +print "\nSet Vehicle mode to GUIDED using synchronous setter (currently: %s)" % vehicle.mode.name +vehicle.sync_set_mode(VehicleMode("GUIDED")) print " New mode: %s" % vehicle.mode.name -print "\nSet Vehicle mode to STABILIZE (currently: %s)" % vehicle.mode.name -vehicle.try_set_mode("STABILIZE") +print "\nSet Vehicle mode to STABILIZE using synchronous setter and 'mode string' (currently: %s)" % vehicle.mode.name +vehicle.sync_set_mode("STABILIZE") #return when value changed print " New mode: %s" % vehicle.mode.name -print "\nSet Vehicle.mode = GUIDED without waiting (currently: %s)" % vehicle.mode.name -vehicle.try_set_mode(value=VehicleMode("GUIDED"), wait_ready=False) +print "\nSet Vehicle.mode = GUIDED using asynchronous setter (currently: %s)" % vehicle.mode.name +vehicle.set_mode(value=VehicleMode("GUIDED")) while not vehicle.mode.name=="GUIDED": print " Waiting for mode change..." time.sleep(0.3) @@ -130,23 +143,29 @@ # If required, you can provide additional information about initialisation # using `vehicle.gps_0.fix_type` and `vehicle.mode.name`. -print "\nSend arm message and wait (currently: %s)" % vehicle.armed +print "\nSend arm message and return when value changed (currently: %s)" % vehicle.armed if vehicle.is_armable: - vehicle.try_set_armed() #returns when armed + vehicle.sync_set_armed() #returns when armed print " Vehicle is armed: %s" % vehicle.armed -print "\nSend disarm message and wait (currently: %s)" % vehicle.armed -vehicle.try_set_armed(value=False) #returns when armed +print "\nSend disarm message and return when value changed (currently: %s)" % vehicle.armed +vehicle.sync_set_armed(value=False) #returns when armed print " Vehicle is armed: %s" % vehicle.armed -print "\nSend arm message without waiting (currently: %s)" % vehicle.armed -# This non-waiting form is not recommended. -vehicle.try_set_armed(wait_ready=False) +print "\nSend arm message and poll in loop for value to change (currently: %s)" % vehicle.armed +vehicle.set_armed() while not vehicle.armed: print " Waiting for arming..." time.sleep(0.3) print " Vehicle is armed: %s" % vehicle.armed +#Set groundspeed and airspeed target values asynchronously. There is no reliable way to +# check that the target value has been set and hence no equivalent synchronous method. +print "\nSend 'set target groundspeed' message (currently: %s)" % vehicle.groundspeed +vehicle.set_target_groundspeed(5) +print "\nSend 'set target airspeed' message (currently: %s)" % vehicle.airspeed +vehicle.set_target_airspeed(5) + # Add and remove and attribute callbacks @@ -183,8 +202,8 @@ def decorated_mode_callback(self, attr_name, value): # `value` is the updated attribute value. print " CALLBACK: Mode changed to", value -print " Set mode=STABILIZE (currently: %s) and wait for callback" % vehicle.mode.name -vehicle.try_set_mode(VehicleMode("STABILIZE")) +print " Set mode=STABILIZE asynchronously (currently: %s) and wait for callback" % vehicle.mode.name +vehicle.set_mode(VehicleMode("STABILIZE")) print " Wait 2s so callback invoked before moving to next example" time.sleep(2) @@ -262,8 +281,8 @@ def any_parameter_callback(self, attr_name, value): ## Reset variables to sensible values. print "\nReset vehicle attributes/parameters and exit" -vehicle.try_set_mode(value=VehicleMode("STABILIZE")) -vehicle.try_set_armed(value=False) +vehicle.sync_set_mode(value=VehicleMode("STABILIZE")) +vehicle.sync_set_armed(value=False) vehicle.parameters['THR_MIN']=130 vehicle.parameters['THR_MID']=500 @@ -281,3 +300,4 @@ def any_parameter_callback(self, attr_name, value): + From 1000338621c6320ecc591b6be2be6b10b6bacdf4 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Tue, 19 Apr 2016 15:51:47 +1000 Subject: [PATCH 9/9] Fix incorrect function call in tests --- dronekit/test/sitl/test_115.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dronekit/test/sitl/test_115.py b/dronekit/test/sitl/test_115.py index fb050c341..c4d47db59 100644 --- a/dronekit/test/sitl/test_115.py +++ b/dronekit/test/sitl/test_115.py @@ -32,7 +32,7 @@ def mavlink_callback(*args): savecount = mavlink_callback.count # Disarm. A callback of None should not throw errors - v.set_mode(False) + v.set_armed(False) # NOTE wait crudely for ACK on mode update time.sleep(3)