From ba72c5865997b58f684dd1aef89fde4c9f791905 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 3 Aug 2023 15:41:04 +0900 Subject: [PATCH] autotest: autotest gets GSF_reset test --- Tools/autotest/arducopter.py | 41 ++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index bc82ee29ea..a10092f5e1 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7709,6 +7709,46 @@ class AutoTestCopter(AutoTest): raise NotAchievedException("Did not get %s" % want) still_want.remove(m.get_type()) + def GSF_reset(self): + '''test the Gaussian Sum filter based Emergency reset''' + self.context_push() + self.set_parameters({ + "COMPASS_ORIENT": 4, # yaw 180 + "COMPASS_USE2": 0, # disable backup compasses to avoid pre-arm failures + "COMPASS_USE3": 0, + }) + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + + # record starting position + startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) + self.progress("startpos=%s" % str(startpos)) + + # arm vehicle and takeoff to at least 5m + self.arm_vehicle() + expected_alt = 5 + self.user_takeoff(alt_min=expected_alt) + + # watch for emergency yaw reset + self.wait_text("EKF3 IMU. emergency yaw reset", timeout=5, regex=True) + + # record how far vehicle flew off + endpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) + delta_x = endpos.x - startpos.x + delta_y = endpos.y - startpos.y + dist_m = math.sqrt(delta_x*delta_x + delta_y*delta_y) + self.progress("GSF yaw reset triggered at %f meters" % dist_m) + + self.do_RTL() + self.context_pop() + self.reboot_sitl() + + # ensure vehicle did not fly too far + dist_m_max = 8 + if dist_m > dist_m_max: + raise NotAchievedException("GSF reset failed, vehicle flew too far (%f > %f)" % (dist_m, dist_m_max)) + def fly_rangefinder_mavlink(self): self.fly_rangefinder_mavlink_distance_sensor() @@ -9851,6 +9891,7 @@ class AutoTestCopter(AutoTest): self.AltEstimation, self.EKFSource, self.GSF, + self.GSF_reset, self.AP_Avoidance, self.SMART_RTL, self.RTL_TO_RALLY,