def Trajectory3():
print("Set default/target airspeed to 3")
vehicle.airspeed = 3
point1 = LocationLocal(0, 0, 0)
vehicle.simple_goto(point1)
time.sleep(2)
point1 = LocationLocal(0, 0, 0.5)
vehicle.simple_goto(point1)
time.sleep(3)
point1 = LocationLocal(5, 0, 0.5)
vehicle.simple_goto(point1)
time.sleep(15)
point1 = LocationLocal(0, 0, 0.5)
vehicle.simple_goto(point1)
time.sleep(15)
point1 = LocationLocal(0, 0, 0)
vehicle.simple_goto(point1)
time.sleep(3)
def test_get_locations_frame(self):
relative_loc = LocationGlobalRelative(6.0, 5.0, 4.0)
global_loc = LocationGlobal(3.0, 2.0, 1.0)
local_loc = LocationLocal(0.0, -1.0, -2.0)
self.relative_mock.configure_mock(return_value=relative_loc)
self.global_mock.configure_mock(return_value=global_loc)
self.local_mock.configure_mock(return_value=local_loc)
loc1 = LocationGlobalRelative(5.4, 3.2, 1.0)
frame_loc = self.geometry.get_locations_frame(self.locations_mock, loc1)
self.assertEqual(frame_loc, relative_loc)
self.relative_mock.assert_called_once_with()
loc2 = LocationGlobal(8.6, 4.2, 0.8)
frame_loc = self.geometry.get_locations_frame(self.locations_mock, loc2)
self.assertEqual(frame_loc, global_loc)
self.global_mock.assert_called_once_with()
loc3 = LocationLocal(-2.0, 4.5, -2.5)
frame_loc = self.geometry.get_locations_frame(self.locations_mock, loc3)
self.assertEqual(frame_loc, local_loc)
self.local_mock.assert_called_once_with()
# Correct location types must be provided.
with self.assertRaisesRegexp(TypeError, "`Locations`"):
self.geometry.get_locations_frame(None, loc3)
with self.assertRaisesRegexp(TypeError, "`Location`"):
self.geometry.get_locations_frame(self.locations_mock, None)
def test_update_location_speed(self):
with patch.object(Mock_Vehicle, "_get_delta_time", return_value=(1.0, 1234567890.123)) as delta_mock:
self.vehicle.update_location()
delta_mock.assert_not_called()
self.vehicle.check_arming()
self.vehicle.armed = True
self.vehicle.speed = 1.5
self.vehicle.simple_takeoff(2.0)
# Takeoff works stepwise.
self.assertEqual(self.vehicle.location.local_frame,
LocationLocal(0.0, 0.0, -1.5))
delta_mock.assert_called_once_with()
delta_mock.reset_mock()
self.assertEqual(self.vehicle.location.local_frame,
LocationLocal(0.0, 0.0, -2.0))
delta_mock.assert_called_once_with()
self.vehicle.clear_target_location()
self.vehicle.velocity = [2.5, 0.0, 0.0]
delta_mock.reset_mock()
# Velocity step works.
self.assertEqual(self.vehicle.location.local_frame,
LocationLocal(2.5, 0.0, -2.0))
delta_mock.assert_called_once_with()
def Trajectoire1():
print("Set default/target airspeed to 3")
vehicle.airspeed = 3
point1 = LocationGlobal(0, 0, 0)
vehicle.simple_goto(point1)
time.sleep(2)
point1 = LocationLocal(0, 0, 0.5)
vehicle.simple_goto(point1)
time.sleep(5)
point1 = LocationLocal(3, 0, 0.5)
vehicle.simple_goto(point1)
time.sleep(10)
point1 = LocationLocal(3, 4, 0.5)
vehicle.simple_goto(point1)
time.sleep(10)
point1 = LocationLocal(0, 0, 0.5)
vehicle.simple_goto(point1)
time.sleep(15)
point1 = LocationLocal(0, 0, 0)
vehicle.simple_goto(point1)
time.sleep(3)
def test_get_points(self):
points = self.waypoint.get_points()
self.assertEqual(len(points), 5)
self.assertEqual(points[0], LocationLocal(0.0, 2.0, 0.0))
self.assertEqual(points[1], LocationLocal(0.0, 4.0, 0.0))
self.assertEqual(points[2], LocationLocal(0.0, 6.0, 0.0))
self.assertEqual(points[3], LocationLocal(0.0, 8.0, 0.0))
self.assertEqual(points[4], LocationLocal(0.0, 10.0, 0.0))
def test_get_projected_location(self):
location = LocationLocal(1.0, 2.0, -3.0)
self.assertEqual(self.geometry.get_projected_location(location, 0),
LocationLocal(2.0, 3.0, 0.0))
self.assertEqual(self.geometry.get_projected_location(location, 1),
LocationLocal(1.0, 3.0, 0.0))
self.assertEqual(self.geometry.get_projected_location(location, 2),
location)
def setUp(self):
self.set_arguments(vehicle_class="Vehicle")
super(TestVehicle, self).setUp()
self._location_valid_cases = [(LocationGlobal(1.0, 2.3, 4.2), True),
(LocationGlobal(None, 2.3, 4.2), False),
(LocationGlobal(1.0, 2.3, None), False),
(LocationLocal(5.0, 4.0, 3.0), True),
(LocationLocal(None, 4.0, 3.0), False)]
def get_location_local(self, location):
if isinstance(location, LocationLocal):
return location
elif isinstance(location, Locations):
return location.local_frame
dlat, dlon, dalt = self.diff_location_meters(self.home_location,
location)
if isinstance(location, LocationGlobal):
return LocationLocal(dlat, dlon, -dalt)
elif isinstance(location, LocationGlobalRelative):
return LocationLocal(dlat, dlon, -location.alt)
def test_read_serial_message(self):
with patch("thread.start_new_thread"):
self.vehicle.activate()
with patch("sys.stdout"):
# The location and direction states are updated based on the
# Arduino "location" message.
self._ttl_device.write("LOCA 2 3 E\n")
self.vehicle._read_serial_message()
self.assertEqual(self.vehicle.location,
LocationLocal(2.0, 3.0, 0.0))
self.assertEqual(self.vehicle._direction,
Line_Follower_Direction.RIGHT)
self.assertEqual(self.vehicle._state.name, "intersection")
# The direction is updated based on the Arduino "goto direction".
self._ttl_device.write("GDIR N\n")
self.vehicle._read_serial_message()
self.assertEqual(self.vehicle._direction,
Line_Follower_Direction.UP)
# The vehicle state is updated based on the Arduino
# "acknowledgement goto" message.
self.vehicle.add_waypoint(LocationLocal(4.0, 3.0, 0.0))
self.vehicle.set_next_waypoint()
self._ttl_device.write("ACKG 4 3\n")
self.vehicle._read_serial_message()
self.assertEqual(self.vehicle._state.name, "move")
# The current location is updated based on the Arduino
# "intersection passing" message.
self._ttl_device.write("PASS 0\n")
self.vehicle._read_serial_message()
self.assertEqual(self.vehicle.location,
LocationLocal(3.0, 3.0, 0.0))
self.assertFalse(self.vehicle.is_current_location_valid())
# Check that the location updates for each "intersection passing"
# message, but only becomes valid when we get a "location" message
# from the Arduino.
self._ttl_device.write("PASS 1\n")
self.vehicle._read_serial_message()
self.assertEqual(self.vehicle.location,
LocationLocal(4.0, 3.0, 0.0))
self.assertFalse(self.vehicle.is_current_location_valid())
self._ttl_device.write("LOCA 4 3 N\n")
self.vehicle._read_serial_message()
self.assertTrue(self.vehicle.is_current_location_valid())
# Incomplete messages are ignored.
self._ttl_device.write("LOCA 999\n")
self.vehicle._read_serial_message()
def test_convert_waypoint(self):
loc = LocationLocal(1.2, 3.4, 0.0)
adjusted_loc = LocationLocal(1.2, 3.4, -self.settings.get("altitude"))
self.assertEqual(self.mission._convert_waypoint(loc), adjusted_loc)
loc = LocationGlobalRelative(5.6, 7.8, 9.0)
local_loc = LocationLocal(5.6, 7.8, -9.0)
self.assertEqual(self.mission._convert_waypoint(loc), local_loc)
loc = LocationGlobalRelative(4.3, 2.1, 0.0)
new_loc = LocationGlobalRelative(4.3, 2.1,
self.settings.get("altitude"))
with patch.object(self.mission, "geometry", new=Geometry_Spherical()):
self.assertEqual(self.mission._convert_waypoint(loc), new_loc)
def test_initialization_previous_waypoint(self):
waypoint = Waypoint_Wait(1,
self.geometry,
self.location,
previous_location=LocationLocal(10, 10, 0),
wait_id=0,
wait_count=2)
points = waypoint.get_points()
self.assertEqual(len(points), 2)
self.assertEqual(points[0], LocationLocal(5.0, 10.0, 0.0))
self.assertEqual(points[1], LocationLocal(0.0, 10.0, 0.0))
self.assertIsNone(waypoint.get_required_sensors())
def test_check_waypoint(self, state_loop_mock):
with patch('sys.stdout'):
self.mission.setup()
self.mission.arm_and_takeoff()
self.mission.start()
self.assertEqual(self.vehicle.mode.name, "AUTO")
self.assertTrue(self.vehicle.armed)
state_loop_mock.assert_called_once_with()
self.assertEqual(
self.vehicle._waypoints,
list(
itertools.chain(*[[waypoint, None]
for waypoint in self.first_waypoints])))
self.assertEqual(self.vehicle.get_waypoint(), None)
with patch('sys.stdout'):
self.mission.check_waypoint()
self.vehicle._check_state()
self.assertEqual(self.vehicle._state.name, "move")
self.assertEqual(self.vehicle._current_waypoint, 0)
self.assertEqual(self.vehicle.get_waypoint(), LocationLocal(1, 0, 0))
self.assertNotEqual(self._ttl_device.readline(), "")
self.vehicle._location = (1, 0)
self.vehicle._state = Robot_State("intersection")
with patch('sys.stdout'):
self.mission.check_waypoint()
# The mission waits for the other RF sensor to send a valid location packet.
self.vehicle._check_state()
self.assertEqual(self.vehicle._current_waypoint, 1)
self.assertEqual(self.vehicle.get_waypoint(), None)
other_id = self.rf_sensor.id + 1
self.environment.set_waypoint_valid()
self.assertTrue(self.environment.location_valid())
self.assertTrue(
self.environment.location_valid(other_valid=True,
other_id=other_id,
other_index=1))
with patch('sys.stdout'):
self.mission.check_waypoint()
self.vehicle._check_state()
self.assertEqual(self.vehicle._current_waypoint, 2)
self.assertEqual(self.vehicle.get_waypoint(), LocationLocal(2, 0, 0))
def test_equalize(self):
# Local locations are kept intact.
loc1 = LocationLocal(1.0, 2.0, 3.0)
loc2 = LocationLocal(4.5, 6.7, -8.9)
new_loc1, new_loc2 = self.geometry.equalize(loc1, loc2)
self.assertEqual(loc1, new_loc1)
self.assertEqual(loc2, new_loc2)
# Base geometry does not support relative or global locations.
with self.assertRaises(TypeError):
self.geometry.equalize(self.local_location, self.relative_location)
with self.assertRaises(TypeError):
self.geometry.equalize(self.relative_location, self.global_location)
with self.assertRaises(TypeError):
self.geometry.equalize(self.global_location, self.local_location)
def test_equalize(self):
home_loc = self._make_global_location(5.0, 3.14, 10.0)
self.geometry.set_home_location(home_loc)
loc1 = self.geometry.get_location_meters(home_loc, 0.4, 0.06, 1.0)
loc2 = LocationLocal(0.4, 0.06, -1.0)
loc3 = LocationGlobalRelative(5.0, 3.14, 0.0)
self.assertEqual(*self.geometry.equalize(loc1, loc2))
self.assertEqual(*self.geometry.equalize(home_loc, loc3))
self.local_mock.configure_mock(return_value=loc2)
self.relative_mock.configure_mock(return_value=loc3)
self.assertEqual(*self.geometry.equalize(self.locations_mock, loc2))
self.local_mock.assert_called_once_with()
self.local_mock.reset_mock()
self.assertEqual(*self.geometry.equalize(loc2, self.locations_mock))
self.local_mock.assert_called_once_with()
self.assertEqual(*self.geometry.equalize(self.locations_mock,
self.locations_mock))
self.assertEqual(self.relative_mock.call_count, 2)
with self.assertRaisesRegexp(TypeError, "`location1`"):
self.geometry.equalize(None, loc2)
with self.assertRaisesRegexp(TypeError, "`location2`"):
self.geometry.equalize(loc1, None)
def setUp(self):
super(TestGeometry, self).setUp()
# Geometry object to use in the tests.
self.geometry = Geometry()
# Delta values for approximate equality checks.
# The default values handle float inaccuracies that may be caused by
# conversions in the geometry class.
self.dist_delta = sys.float_info.epsilon * 10
self.coord_delta = self.dist_delta
self.angle_delta = sys.float_info.epsilon * 10
# Create a mock version of a `Locations` object. The location frames
# have property mocks that can be configured to return a specific
# location value.
self.locations_mock = Mock(spec_set=Locations)
self.relative_mock = PropertyMock()
self.global_mock = PropertyMock()
self.local_mock = PropertyMock()
type(self.locations_mock).global_relative_frame = self.relative_mock
type(self.locations_mock).global_frame = self.global_mock
type(self.locations_mock).local_frame = self.local_mock
# Location objects that can be used by type checking tests, where the
# coordinate values do not matter at all.
self.local_location = LocationLocal(1.0, 2.0, 3.0)
self.global_location = LocationGlobal(4.0, 5.0, 6.0)
self.relative_location = LocationGlobalRelative(7.0, 8.0, 9.0)
def set_location(self, north, east, alt):
"""
Change the location to be at a certain offset from the current location.
The given arguments `north`, `east` and `alt` are in meters and specify
the location in those components. The location is immediately updated
in all frames (global, global relative and local).
This method should only be used to forcibly change the location of the
mock vehicle. It is not to be used to alter the target location,
for which `set_target_location` is responsible.
"""
local_location = self._locations.local_frame
if local_location.north is None:
local_location = LocationLocal(0.0, 0.0, 0.0)
# First try to set the new location to let the location callback handle
# the change.
new_location = self._geometry.get_location_meters(
local_location, north, east, alt)
self.location = new_location
self._updating = True
# Send a message to the message listeners
msg = LocalMessage(local_location.north + north,
local_location.east + east,
local_location.down - alt)
self.notify_message_listeners('LOCAL_POSITION_NED', msg)
self._updating = False
def _make_location(self, location_class, lat, lon, alt):
if isinstance(self._geometry, Geometry_Spherical):
return location_class(lat, lon, alt)
return LocationLocal(lat - self._home_location.lat,
lon - self._home_location.lon,
self._home_location.alt - alt)
def followPath(self, list_location, frame):
for i in range(0, len(list_location)):
# Given the global NED waypoints w.r.t. the home location (EKF origin), navigate the drone by specifying the frame.
#self.drone.goto_local_NED(list_location[i][0], list_location[i][1], list_location[i][2], frame) #waits until target is reached
# Offset calculation for positions
posNorth = list_location[i][
0] # - (self.drone.vehicle.location.local_frame.north - startPosition.north)
posEast = list_location[i][
1] # - (self.drone.vehicle.location.local_frame.east - startPosition.east)
posDown = list_location[i][
2] # - (self.drone.vehicle.location.local_frame.down - startPosition.down)
currentLocation = self.drone.vehicle.location.local_frame
targetLocation = LocationLocal(posNorth, posEast, posDown)
distance = self.drone.get_distance_metres_EKF(
currentLocation, targetLocation)
if distance >= self.drone.minimum_distance_threshold:
self.logger.info("Point {} out of {}".format(
i, len(list_location)))
self.logger.info('Goto ({}, {}, {})'.format(
posNorth, posEast, posDown))
# self.logger.info("Start location ({}, {}, {})".format(startLocation.north, startLocation.east, startLocation.down))
self.drone.goto_local_NED(
posNorth, posEast, posDown,
frame) # Waits until target is reached
else:
self.logger.warn(
"Point {} out of {} not considered. Distance too close.".
format(i, len(list_location)))
self.logger.info("Distance was {} meter".format(distance))
def test_location(self):
# Test that the location property works as expected.
with patch.object(Mock_Vehicle, "update_location") as update_mock:
location = self.vehicle.location
self.assertIsInstance(location, Locations)
self.assertEqual(location.local_frame, LocationLocal(0.0, 0.0, 0.0))
# The global frame does not have an altitude component set because
# no home location has been set.
self.assertEqual(location.global_frame,
LocationGlobal(0.0, 0.0, None))
self.assertEqual(location.global_relative_frame,
LocationGlobalRelative(0.0, 0.0, 0.0))
update_mock.assert_called_once_with()
# Updating the location works (via dronekit's Locations and the
# message listeners).
self.vehicle.location = LocationGlobalRelative(1.0, 2.0, -3.0)
self.assertEqual(self.vehicle.location.global_relative_frame,
LocationGlobalRelative(1.0, 2.0, -3.0))
msg = GlobalMessage(1.0 * 1e7, 2.0 * 1e7, -3.0 * 1000, -3.0 * 1000)
self._message_listener_mock.assert_called_once_with(self.vehicle, 'GLOBAL_POSITION_INT', msg)
# Recursive location setting is detected.
listener = lambda vehicle, attribute, value: vehicle.set_location(9.9, 8.8, 7.7)
self.vehicle.add_attribute_listener('location', listener)
with self.assertRaises(RuntimeError):
self.vehicle.location = LocationGlobal(5.0, 4.0, -2.0)
def test_setup_init(self):
with patch('sys.stdout'):
self.mission.setup()
# Check first vehicle's state.
self.assertEqual(self.vehicle.location, LocationLocal(0, 0, 0))
self.assertEqual(self.mission.id, 0)
self.assertEqual(self.mission.size, 4)
self.assertIsInstance(self.mission.chain, deque)
# 4 sides, 2 rounds per side
self.assertEqual(self.mission.round_number, 8)
self.assertIsInstance(self.mission.waypoints, list)
waypoints = list(self.mission.waypoints)
self.assertEqual(waypoints, self.first_waypoints)
# Check second vehicle's state.
self.vehicle._location = (0, 1)
with patch('sys.stdout'):
self.mission.setup()
waypoints = list(self.mission.waypoints)
rounds = [[(0, 1)] * self.wait_commands,
[(0, 0), (1, 0), (2, 0), (3, 0), (3, 1), (3, 2), (3, 3),
(2, 3)], [(1, 3)] * (self.wait_commands + 1),
[(0, 3), (0, 2), (0, 1), (0, 0), (1, 0), (2, 0), (3, 0),
(3, 1)], [(3, 2)] * (self.wait_commands + 1),
[(3, 3), (2, 3), (1, 3), (0, 3), (0, 2), (0, 1), (0, 0),
(1, 0)], [(2, 0)] * (self.wait_commands + 1),
[(3, 0), (3, 1), (3, 2), (3, 3), (2, 3), (1, 3), (0, 3),
(0, 2)], [(0, 1)]]
second_waypoints = list(itertools.chain(*rounds))
self.assertEqual(waypoints, second_waypoints)
def test_update(self):
self.collision_avoidance.update(self.home_locations,
self.first_position, 1, 2, 6)
self.collision_avoidance.update(self.home_locations, (5, 9), 2, 1, 14)
self.assertEqual(self.collision_avoidance.location,
LocationLocal(5, 9, 0))
self.assertEqual(self.collision_avoidance.distance, 6)
# Vehicles 1 and 2 are synchronized with each other, but vehicle 3 has
# not yet. Thus it can crooss either vehicle's paths, which can lead to
# a collision.
self.collision_avoidance.update(self.home_locations, (0, 5), 3, 1, 5)
self.assertEqual(self.collision_avoidance.location,
LocationLocal(0, 5, 0))
self.assertEqual(self.collision_avoidance.distance, np.inf)
def goto_absolute(self,
pos_x,
pos_y,
pos_z,
accuracy=0.5,
wait=True,
text=True):
"""Go to a position relative to the home position"""
targetLocation = LocationLocal(pos_x, pos_y, -pos_z)
self.__send_ned_position(pos_x, pos_y, -pos_z)
self.mode = VehicleMode("OFFBOARD")
if text: print("Vehicle mode should be OFFBOARD: %s" % self.mode.name)
while True:
self.__send_ned_position(pos_x, pos_y, -pos_z)
remainingDistance = get_distance_metres_local(
self.location.local_frame, targetLocation)
if remainingDistance <= accuracy:
if text: print("Arrived at target")
break
if wait == False: break
if text: print "Distance to target: ", remainingDistance
time.sleep(0.1)
def test_get_distance(self, location_mock, get_distance_meters_mock):
location = LocationLocal(5.0, 6.0, -7.0)
distance = self.proxy.get_distance(location)
location_mock.assert_called_once_with()
get_distance_meters_mock.assert_called_once_with(
location_mock.return_value, location)
self.assertEqual(distance, get_distance_meters_mock.return_value)
def rotate_location(self, loc, ang):
"""
Is currently only implemented for yaw rotations
TODO: Add support for 3 rotation axis
:param loc: a LocationLocal representation that has to be rotated
:param ang: The Attitude for the rotation
:return: LocationLocal representing the rotated element
"""
n = loc.north
e = loc.east
d = loc.down
r = ang.roll
p = ang.pitch
y = ang.yaw
# First line of the rotation matrix applied to loc
north = n * math.cos(y) * math.cos(p) + e * (
math.cos(y) * math.sin(r) * math.sin(p) - math.cos(p) *
math.sin(y)) + d * (math.sin(r) * math.sin(y) +
math.cos(r) * math.cos(y) * math.sin(p))
#Second line
east = n * math.cos(p) * math.sin(y) + e * (
math.cos(r) * math.cos(y) + math.sin(r) * math.sin(y) *
math.sin(p)) + d * (math.cos(r) * math.sin(y) * math.sin(p) -
math.cos(y) * math.sin(r))
#Third line
down = n * -math.sin(p) + e * math.cos(p) * math.sin(r) + d * math.cos(
r) * math.cos(p)
#north = n*math.cos(y) - e*math.sin(y)
#east = n*math.sin(y) + e*math.cos(y)
#down = d
return LocationLocal(north, east, down)
def test_add_waypoint(self):
with patch('sys.stdout'):
self.mission.setup()
# Packets not meant for the current RF sensor are ignored.
self._send_waypoint_add(0, 6.0, 5.0, id_offset=42)
self.assertEqual(self.mission.next_index, 0)
self.enqueue_mock.reset_mock()
self._send_waypoint_add(0, 1.0, 4.0)
self.assertEqual(self.enqueue_mock.call_count, 1)
args, kwargs = self.enqueue_mock.call_args
self.assertEqual(len(args), 1)
self.assertIsInstance(args[0], Packet)
self.assertEqual(
args[0].get_all(), {
"specification": "waypoint_ack",
"next_index": 1,
"sensor_id": self.rf_sensor.id
})
self.assertEqual(kwargs, {"to": 0})
self.assertEqual(self.mission.next_index, 1)
self.assertEqual(self.mission._point, LocationLocal(1.0, 4.0, 0.0))
self.assertEqual(self.vehicle._waypoints, [(1, 4), None])
def goto_position_target_local_ned(north, east, down):
"""
Send SET_POSITION_TARGET_LOCAL_NED command to request the vehicle fly to a specified
location in the North, East, Down frame.
"""
msg = vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0,
0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_OFFSET_NED, # frame
0b0000111111000111, # type_mask (only positions enabled)
0,
0,
0,
north,
east,
down, # x, y, z velocity in m/s (not used)
0,
0,
0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
0,
0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
# send command to vehicle
vehicle.send_mavlink(msg)
return LocationLocal(n, e, d)
def test_get_waypoint(self, commands_mock):
commands_mock.return_value.configure_mock(__getitem__=Mock(
side_effect=IndexError))
self.assertIsNone(self.vehicle.get_waypoint(waypoint=0))
cmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0, 0,
0, 0, 0, 0)
commands_mock.return_value.configure_mock(__getitem__=Mock(
return_value=cmd))
self.assertIsNone(self.vehicle.get_waypoint(waypoint=1))
cmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
3.4, 2.3, 1.2)
commands_mock.return_value.configure_mock(__getitem__=Mock(
return_value=cmd))
self.assertEqual(self.vehicle.get_waypoint(),
LocationLocal(3.4, 2.3, -1.2))
with patch.object(self.vehicle, "_geometry", new=Geometry_Spherical()):
self.assertEqual(self.vehicle.get_waypoint(),
LocationGlobalRelative(3.4, 2.3, 1.2))
def test_add_commands(self, add_waypoint_mock, get_waypoints_mock):
waypoints = [
LocationLocal(1.0, 2.0, 3.0),
LocationLocal(4.0, 5.0, 6.0),
LocationLocal(7.0, 8.0, 9.0),
LocationLocal(0.0, 1.0, -2.0)
]
get_waypoints_mock.configure_mock(return_value=waypoints)
with patch("sys.stdout"):
self.mission.add_commands()
get_waypoints_mock.assert_called_once_with()
self.assertEqual(add_waypoint_mock.call_count, len(waypoints))
for actual, expected in zip(add_waypoint_mock.call_args_list,
waypoints):
self.assertEqual(actual[0][0], expected)
def test_diff_location_meters(self):
loc = LocationLocal(5.4, 3.2, -1.0)
# 3 * 3 + 4 * 4 = 9 + 16 = 25 which is 5 squared.
loc2 = self.geometry.get_location_meters(loc, 3.0, 4.0, 5.0)
dlat, dlon, dalt = self.geometry.diff_location_meters(loc, loc2)
self.assertAlmostEqual(dlat, 3.0, delta=self.dist_delta)
self.assertAlmostEqual(dlon, 4.0, delta=self.dist_delta)
self.assertAlmostEqual(dalt, 5.0, delta=self.dist_delta)
def _make_relative_location(self, x, y, z=0.0):
"""
Create a `Location` object that is suitable as a relative location.
The returned type depends on the geometry being tested.
"""
return LocationLocal(x, y, -z)
