def ensure_armed(self):
"""
Attempt to arm the vehicle if it is not already.
Do NOT perform this step until a flight mode has been selected.
:param timeout: The time after which we give up on arming the drone.
"""
# If the vehicle is already armed, we're done.
if self.vehicle.armed is True:
return
# Make sure the drone is ready to be armed.
armable = blocking_poll(lambda: self.vehicle.is_armable, 0.5,
self._arm_timeout)
if not armable:
raise AvionicsException("Drone is not armable after {}".format(
self._arm_timeout))
self._vehicle.armed = True
# Arm the drone and wait for it to acknowledge being armed.
arm_success = blocking_poll(lambda: self._vehicle.armed,
0.25,
timeout_sec=self._arm_timeout)
if not arm_success:
raise AvionicsException("Asked drone to arm, but it refused. "
"Could not arm within {}".format(
self._arm_timeout))
def connect(self):
"""
Establish a connection with the vehicle's flight controller.
If no connection string was specified, assume we're using SITL.
This function also "downloads commands" from the flight controller.
"""
# If a simultaneous connect attempt follows a successful one, ignore it.
if self._vehicle is not None:
return
# Set up SITL if we're using it.
if self.fc_address is None:
if self._loc_start:
lat, lon = self._loc_start
self._sitl = dronekit_sitl.start_default(lat=lat, lon=lon)
else:
self._sitl = dronekit_sitl.start_default()
self.fc_address = self._sitl.connection_string()
# Connect to the flight controller.
self._vehicle = connect(self.fc_address,
rate=_FC_UPDATE_RATE_HZ,
wait_ready=True)
# "Download commands" from the flight controller.
# The fact that this is separate from the dronekit connect() function is puzzling.
# Many common tasks (eg, reading the starting altitude of the drone) will fail until
# we do so.
# Repeatedly download commands until the vehicle acquires a home location.
blocking_poll(self.poll_home_location, 2, 10)
def test_blocking_poll_aborts(self, mock_time):
mock_predicate = mock.Mock()
mock_predicate.side_effect = lambda: True
mock_time.side_effect = lambda: 0.0
abort_event = Event()
abort_event.set()
with self.assertRaises(FlightAbortedException):
blocking_poll(mock_predicate, 0.25, abort_event=abort_event)
# Shouldn't even have run the command in question if the abort event
# was triggered!
mock_predicate.assert_not_called()
def land(self):
"""
Land the drone. Blocks until complete.
"""
self.vehicle.mode = VehicleMode("LAND")
self.status = FlightStatus.LANDING
# Make sure the drone is *actually* landed before we declare that it's
# actually landed.
blocking_poll(
lambda: self.vehicle.location.global_relative_frame.alt <= 0.1,
0.25,
timeout_sec=120,
abort_event=self.kill_event,
)
self.status = FlightStatus.LANDED
def goto(self, coords, altitude, airspeed=1, radius=2.5, timeout_sec=None):
"""
Blocking function that tells the drone to go somewhere.
The drone must be flying and asked to go somewhere sane.
:param coords: A (lat, lon) double; both specified in an absolute reference frame.
:param altitude: An altitude specified relative to the height the drone acquired its GPS
fix at.
:param airspeed: The speed, in m/s, that the drone should travel through these
air.
:param radius: The maximum distance (in meters) the drone is permitted to be
from the position specified before considering itself arrived.
:param timeout_sec: The length of time the drone has to get there.
"""
# Make sure we're off to someplace "sane."
lat, lon = coords
home_altitude = self.vehicle.home_location.alt
position = (lat, lon, altitude + home_altitude)
self._validate_position(position)
# Tell the drone to go there.
self.vehicle.simple_goto(LocationGlobal(*position), airspeed=airspeed)
# Wait for the drone to arrive.
arrived = blocking_poll(lambda: self.distance_to(position) <= radius,
0.25,
timeout_sec=timeout_sec,
abort_event=self.panic_event)
if not arrived:
raise AvionicsException(
"Drone could not make it to {}.".format(position))
def take_off(self, height_meters):
"""
Ascend to a given height, in meters. Blocks until complete.
:param height_meters: Height, in meters, to take off to.
"""
self.vehicle.mode = VehicleMode("GUIDED")
self.status = FlightStatus.TAKING_OFF
self.ensure_armed()
self.vehicle.simple_takeoff(height_meters)
# Wait until we've reached the correct height.
blocking_poll(lambda: self.vehicle.location.global_relative_frame.alt
>= 0.95 * height_meters,
0.25,
abort_event=self.panic_event)
self.status = FlightStatus.FLYING
def test_blocking_poll_timeout(self, mock_time):
# Make a blocking poll that times out
mock_predicate = mock.Mock()
mock_predicate.side_effect = lambda: False
mock_time.time.side_effect = [0, 0.3, 2.0]
self.assertFalse(blocking_poll(mock_predicate, 0.25, timeout_sec=1.0))
# Was the predicate actually used?
self.assertEqual(mock_predicate.call_count, 2)
# Did we delay like we were supposed to?
mock_time.sleep.assert_called_once_with(0.25)
# Did we timeout?
self.assertEqual(mock_time.time.call_count, 3)
def test_blocking_poll_success(self, mock_time):
# Make a blocking poll that succeeds.
mock_predicate = mock.Mock()
mock_predicate.side_effect = lambda: True
mock_time.time.side_effect = [0, 0.3]
# Should report success
self.assertTrue(blocking_poll(mock_predicate, 0.25, timeout_sec=0.1))
# Should get an initial time
mock_time.time.assert_called_once()
# Should actually use the predicate
mock_predicate.assert_called_once()
# Should skip a useless "sleep"
mock_time.sleep.assert_not_called()