Tools: add test.Copter.GPSViconSwitching to CopterTests1d

This commit is contained in:
Randy Mackay 2020-11-18 12:25:34 +09:00
parent bf5c74dbdb
commit f257dc20d0
1 changed files with 82 additions and 0 deletions

View File

@ -2238,6 +2238,84 @@ class AutoTestCopter(AutoTest):
if ex is not None:
raise ex
def fly_gps_vicon_switching(self):
"""Fly GPS and Vicon switching test"""
self.customise_SITL_commandline(["--uartF=sim:vicon:"])
"""Setup parameters including switching to EKF3"""
self.context_push()
ex = None
try:
self.set_parameter("VISO_TYPE", 2) # enable vicon
self.set_parameter("SERIAL5_PROTOCOL", 2)
self.set_parameter("EK3_ENABLE", 1)
self.set_parameter("EK3_SRC2_POSXY", 6) # External Nav
self.set_parameter("EK3_SRC2_POSZ", 6) # External Nav
self.set_parameter("EK3_SRC2_VELXY", 6) # External Nav
self.set_parameter("EK3_SRC2_VELZ", 6) # External Nav
self.set_parameter("EK3_SRC2_YAW", 2) # External Nav
self.set_parameter("RC7_OPTION", 80) # RC aux switch 7 set to Viso Align
self.set_parameter("RC8_OPTION", 90) # RC aux switch 8 set to EKF source selector
self.reboot_sitl()
self.set_parameter("EK2_ENABLE", 0)
self.set_parameter("AHRS_EKF_TYPE", 3)
# switch to use GPS
self.set_rc(8, 1000)
# require_absolute=True infers a GPS is present
self.wait_ready_to_arm(require_absolute=True)
# record starting position
old_pos = self.get_global_position_int()
print("old_pos=%s" % str(old_pos))
# align vicon yaw with ahrs heading
self.set_rc(7, 2000)
# takeoff to 10m in Loiter
self.progress("Moving to ensure location is tracked")
self.takeoff(10, mode="LOITER")
# fly forward in Loiter
self.set_rc(2, 1300)
# disable vicon
self.set_parameter("SIM_VICON_FAIL", 1)
# ensure vehicle remain in Loiter for 15 seconds
tstart = self.get_sim_time();
while self.get_sim_time() - tstart < 15:
if not self.mode_is('LOITER'):
raise NotAchievedException("Expected to stay in loiter for >15 seconds")
# re-enable vicon
self.set_parameter("SIM_VICON_FAIL", 0)
# switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter
self.set_rc(8, 1500)
self.set_parameter("GPS_TYPE", 0)
# ensure vehicle remain in Loiter for 15 seconds
tstart = self.get_sim_time();
while self.get_sim_time() - tstart < 15:
if not self.mode_is('LOITER'):
raise NotAchievedException("Expected to stay in loiter for >15 seconds")
# RTL and check vehicle arrives within 10m of home
self.set_rc(2, 1500)
self.do_RTL()
except Exception as e:
self.progress("Caught exception: %s" %
self.get_exception_stacktrace(e))
ex = e
self.context_pop()
self.disarm_vehicle(force=True)
self.reboot_sitl()
if ex is not None:
raise ex
def fly_rtl_speed(self):
"""Test RTL Speed parameters"""
rtl_speed_ms = 7
@ -5552,6 +5630,10 @@ class AutoTestCopter(AutoTest):
("VisionPosition",
"Fly Vision Position",
self.fly_vision_position), #24s
("GPSViconSwitching",
"Fly GPS and Vicon Switching",
self.fly_gps_vicon_switching),
])
return ret