Tools: autotest: add tests for Plane fence
This commit is contained in:
Peter Barker
Peter Barker
parent
10120cee54
commit
c8a4af76fe
6
Tools/autotest/CMAC-fence.txt
Normal file
6
Tools/autotest/CMAC-fence.txt
Normal file
@ -0,0 +1,6 @@
|
||||
-35.363720 149.163651
|
||||
-35.358738 149.165070
|
||||
-35.359295 149.154434
|
||||
-35.372292 149.157135
|
||||
-35.368290 149.166809
|
||||
-35.358738 149.165070
|
||||
@ -16,6 +16,8 @@ from common import AutoTestTimeoutException
|
||||
from common import NotAchievedException
|
||||
from common import PreconditionFailedException
|
||||
|
||||
from MAVProxy.modules.lib import mp_util
|
||||
|
||||
# get location of scripts
|
||||
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 585, 354)
|
||||
@ -846,6 +848,245 @@ class AutoTestPlane(AutoTest):
|
||||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def assert_fence_sys_status(self, present, enabled, health):
|
||||
self.delay_sim_time(1)
|
||||
self.drain_mav_unparsed()
|
||||
m = self.mav.recv_match(type='SYS_STATUS', blocking=True, timeout=1)
|
||||
if m is None:
|
||||
raise NotAchievedException("Did not receive SYS_STATUS")
|
||||
tests = [ ( "present", present, m.onboard_control_sensors_present ),
|
||||
( "enabled", enabled, m.onboard_control_sensors_enabled ),
|
||||
( "health", health, m.onboard_control_sensors_health ),
|
||||
]
|
||||
bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
|
||||
for test in tests:
|
||||
(name, want, field) = test
|
||||
got = (field & bit) != 0
|
||||
if want != got:
|
||||
raise NotAchievedException("fence status incorrect; %s want=%u got=%u" %
|
||||
(name, want, got))
|
||||
|
||||
def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
|
||||
if value:
|
||||
p1 = 1
|
||||
else:
|
||||
p1 = 0
|
||||
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE,
|
||||
p1, # param1
|
||||
0, # param2
|
||||
0, # param3
|
||||
0, # param4
|
||||
0, # param5
|
||||
0, # param6
|
||||
0, # param7
|
||||
want_result=want_result)
|
||||
|
||||
def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
|
||||
self.do_fence_en_or_dis_able(True, want_result=want_result)
|
||||
|
||||
def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
|
||||
self.do_fence_en_or_dis_able(False, want_result=want_result)
|
||||
|
||||
def wait_circling_point_with_radius(self, loc, want_radius, epsilon=2.0, min_circle_time=5):
|
||||
on_radius_start_heading = None
|
||||
average_radius = 0.0
|
||||
circle_time_start = 0
|
||||
done_time = False
|
||||
done_angle = False
|
||||
while True:
|
||||
here = self.mav.location()
|
||||
got_radius = self.get_distance(loc, here)
|
||||
average_radius = 0.95*average_radius + 0.05*got_radius
|
||||
now = self.get_sim_time()
|
||||
on_radius = abs(got_radius - want_radius) < epsilon
|
||||
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
heading = m.heading
|
||||
on_string = "off"
|
||||
got_angle = ""
|
||||
if on_radius_start_heading is not None:
|
||||
got_angle = "%0.2f" % abs(on_radius_start_heading - heading) # FIXME
|
||||
on_string = "on"
|
||||
|
||||
want_angle = 180 # we don't actually get this (angle-substraction issue. But we get enough...
|
||||
self.progress("wait-circling: got-r=%0.2f want-r=%f avg-r=%f %s want-a=%0.1f got-a=%s" %
|
||||
(got_radius, want_radius, average_radius, on_string, want_angle, got_angle))
|
||||
if on_radius:
|
||||
if on_radius_start_heading is None:
|
||||
on_radius_start_heading = heading
|
||||
average_radius = got_radius
|
||||
circle_time_start = self.get_sim_time()
|
||||
continue
|
||||
if abs(on_radius_start_heading - heading) > want_angle: # FIXME
|
||||
done_angle = True
|
||||
if self.get_sim_time() - circle_time_start > min_circle_time:
|
||||
done_time = True
|
||||
if done_time and done_angle:
|
||||
return
|
||||
continue
|
||||
if on_radius_start_heading is not None:
|
||||
average_radius = 0.0
|
||||
on_radius_start_heading = None
|
||||
circle_time_start = 0
|
||||
|
||||
def test_fence_static(self):
|
||||
ex = None
|
||||
try:
|
||||
self.progress("Checking for bizarre healthy-when-not-present-or-enabled")
|
||||
self.assert_fence_sys_status(False, False, True)
|
||||
fence_filepath = os.path.join(self.mission_directory(),
|
||||
"CMAC-fence.txt")
|
||||
self.mavproxy.send("fence load %s\n" % fence_filepath)
|
||||
self.mavproxy.expect("Loaded 6 geo-fence")
|
||||
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
|
||||
if m is not None:
|
||||
raise NotAchievedException("Got FENCE_STATUS unexpectedly");
|
||||
self.drain_mav_unparsed()
|
||||
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE) # report only
|
||||
self.assert_fence_sys_status(False, False, True)
|
||||
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) # report only
|
||||
self.assert_fence_sys_status(True, False, True)
|
||||
self.mavproxy.send('fence enable\n')
|
||||
self.mavproxy.expect("fence enabled")
|
||||
self.assert_fence_sys_status(True, True, True)
|
||||
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
|
||||
if m is None:
|
||||
raise NotAchievedException("Did not get FENCE_STATUS");
|
||||
if m.breach_status:
|
||||
raise NotAchievedException("Breached fence unexpectedly (%u)" %
|
||||
(m.breach_status))
|
||||
self.mavproxy.send('fence disable\n')
|
||||
self.mavproxy.expect("fence disabled")
|
||||
self.assert_fence_sys_status(True, False, True)
|
||||
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE)
|
||||
self.assert_fence_sys_status(False, False, True)
|
||||
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL)
|
||||
self.assert_fence_sys_status(True, False, True)
|
||||
self.mavproxy.send("fence clear\n")
|
||||
self.mavproxy.expect("fence removed")
|
||||
if self.get_parameter("FENCE_TOTAL") != 0:
|
||||
raise NotAchievedException("Expected zero points remaining")
|
||||
self.assert_fence_sys_status(False, False, True)
|
||||
self.progress("Trying to enable fence with no points")
|
||||
self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED)
|
||||
|
||||
# test a rather unfortunate behaviour:
|
||||
self.progress("Killing a live fence with fence-clear")
|
||||
self.mavproxy.send("fence load %s\n" % fence_filepath)
|
||||
self.mavproxy.expect("Loaded 6 geo-fence")
|
||||
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL)
|
||||
self.do_fence_enable()
|
||||
self.assert_fence_sys_status(True, True, True)
|
||||
self.mavproxy.send("fence clear\n")
|
||||
self.mavproxy.expect("fence removed")
|
||||
if self.get_parameter("FENCE_TOTAL") != 0:
|
||||
raise NotAchievedException("Expected zero points remaining")
|
||||
self.assert_fence_sys_status(False, False, True)
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught:")
|
||||
self.progress(self.get_exception_stacktrace(e))
|
||||
ex = e
|
||||
self.mavproxy.send('fence clear\n')
|
||||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def test_fence_breach_circle_at(self, loc):
|
||||
ex = None
|
||||
try:
|
||||
fence_filepath = os.path.join(self.mission_directory(),
|
||||
"CMAC-fence.txt")
|
||||
self.mavproxy.send("fence load %s\n" % fence_filepath)
|
||||
self.mavproxy.expect("Loaded 6 geo-fence")
|
||||
want_radius = 100
|
||||
# when ArduPlane is fixed, remove this fudge factor
|
||||
REALLY_BAD_FUDGE_FACTOR = 1.16
|
||||
expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius
|
||||
self.set_parameter("RTL_RADIUS", want_radius)
|
||||
self.set_parameter("NAVL1_LIM_BANK", 60)
|
||||
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL)
|
||||
|
||||
self.do_fence_enable()
|
||||
self.assert_fence_sys_status(True, True, True)
|
||||
|
||||
self.takeoff(alt=45, alt_max=300)
|
||||
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
if self.get_sim_time() - tstart > 30:
|
||||
raise NotAchievedException("Did not breach fence")
|
||||
m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
|
||||
if m is None:
|
||||
raise NotAchievedException("Did not get FENCE_STATUS");
|
||||
if m.breach_status == 0:
|
||||
continue
|
||||
|
||||
# we've breached; check our state;
|
||||
if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY:
|
||||
raise NotAchievedException("Unexpected breach type %u" %
|
||||
(m.breach_type,))
|
||||
if m.breach_count == 0:
|
||||
raise NotAchievedException("Unexpected breach count %u" %
|
||||
(m.breach_count,))
|
||||
self.assert_fence_sys_status(True, True, False)
|
||||
break
|
||||
|
||||
self.wait_circling_point_with_radius(loc, expected_radius)
|
||||
|
||||
self.disarm_vehicle(force=True)
|
||||
self.reboot_sitl()
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught:")
|
||||
self.progress(self.get_exception_stacktrace(e))
|
||||
ex = e
|
||||
self.mavproxy.send('fence clear\n')
|
||||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def test_fence_rtl(self):
|
||||
self.progress("Testing FENCE_ACTION_RTL no rally point")
|
||||
self.test_fence_breach_circle_at(self.home_position_as_mav_location())
|
||||
|
||||
def location_offset_ne(self, location, north, east):
|
||||
print("old: %f %f" % (location.lat, location.lng))
|
||||
(lat, lng) = mp_util.gps_offset(location.lat, location.lng, east, north)
|
||||
location.lat = lat
|
||||
location.lng = lng
|
||||
print("new: %f %f" % (location.lat, location.lng))
|
||||
|
||||
def test_fence_rtl_rally(self):
|
||||
ex = None
|
||||
target_system = 1
|
||||
target_component = 1
|
||||
try:
|
||||
self.progress("Testing FENCE_ACTION_RTL with rally point")
|
||||
|
||||
self.wait_ready_to_arm()
|
||||
loc = self.home_position_as_mav_location()
|
||||
self.location_offset_ne(loc, 50, -50)
|
||||
|
||||
self.set_parameter("RALLY_TOTAL", 1)
|
||||
self.mav.mav.rally_point_send(target_system,
|
||||
target_component,
|
||||
0, # sequence number
|
||||
1, # total count
|
||||
int(loc.lat * 1e7),
|
||||
int(loc.lng * 1e7),
|
||||
15,
|
||||
0, # "break" alt?!
|
||||
0, # "land dir"
|
||||
0) # flags
|
||||
self.delay_sim_time(1)
|
||||
self.mavproxy.send("rally list\n")
|
||||
self.test_fence_breach_circle_at(loc)
|
||||
except Exception as e:
|
||||
self.progress("Exception caught:")
|
||||
self.progress(self.get_exception_stacktrace(e))
|
||||
ex = e
|
||||
self.mavproxy.send('rally clear\n')
|
||||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def test_parachute(self):
|
||||
self.set_rc(9, 1000)
|
||||
self.set_parameter("CHUTE_ENABLED", 1)
|
||||
@ -1045,6 +1286,18 @@ class AutoTestPlane(AutoTest):
|
||||
"Test RangeFinder Basic Functionality",
|
||||
self.test_rangefinder),
|
||||
|
||||
("FenceStatic",
|
||||
"Test Basic Fence Functionality",
|
||||
self.test_fence_static),
|
||||
|
||||
("FenceRTL",
|
||||
"Test Fence RTL",
|
||||
self.test_fence_rtl),
|
||||
|
||||
("FenceRTLRally",
|
||||
"Test Fence RTL Rally",
|
||||
self.test_fence_rtl_rally),
|
||||
|
||||
("LogDownLoad",
|
||||
"Log download",
|
||||
lambda: self.log_download(
|
||||
|
||||
@ -13,6 +13,8 @@ import pexpect
|
||||
import fnmatch
|
||||
import operator
|
||||
|
||||
from MAVProxy.modules.lib import mp_util
|
||||
|
||||
from pymavlink import mavwp, mavutil, DFReader
|
||||
from pysim import util, vehicleinfo
|
||||
|
||||
@ -1170,16 +1172,35 @@ class AutoTest(ABC):
|
||||
#################################################
|
||||
# UTILITIES
|
||||
#################################################
|
||||
@staticmethod
|
||||
def longitude_scale(lat):
|
||||
ret = math.cos(lat * (math.radians(1)));
|
||||
print("scale=%f" % ret)
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def get_distance(loc1, loc2):
|
||||
"""Get ground distance between two locations."""
|
||||
return AutoTest.get_distance_accurate(loc1, loc2)
|
||||
dlat = loc2.lat - loc1.lat
|
||||
try:
|
||||
dlong = loc2.lng - loc1.lng
|
||||
except AttributeError:
|
||||
dlong = loc2.lon - loc1.lon
|
||||
|
||||
return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
|
||||
return math.sqrt((dlat*dlat) + (dlong*dlong)*AutoTest.longitude_scale(loc2.lat)) * 1.113195e5
|
||||
|
||||
@staticmethod
|
||||
def get_distance_accurate(loc1, loc2):
|
||||
"""Get ground distance between two locations."""
|
||||
try:
|
||||
lon1 = loc1.lng
|
||||
lon2 = loc2.lng
|
||||
except AttributeError:
|
||||
lon1 = loc1.lon
|
||||
lon2 = loc2.lon
|
||||
|
||||
return mp_util.gps_distance(loc1.lat, lon1, loc2.lat, lon2)
|
||||
|
||||
@staticmethod
|
||||
def get_latlon_attr(loc, attrs):
|
||||
@ -1984,6 +2005,10 @@ class AutoTest(ABC):
|
||||
blocking=True)
|
||||
return self.get_distance_int(m, here)
|
||||
|
||||
def home_position_as_mav_location(self):
|
||||
m = self.poll_home_position()
|
||||
return mavutil.location(m.latitude*1.0e-7, m.longitude*1.0e-7, m.altitude*1.0e-3, 0)
|
||||
|
||||
def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user