diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index f28bcb5332..f4edcc7222 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -9,6 +9,7 @@ AP_FLAKE8_CLEAN from __future__ import print_function import math import os +import signal import time from pymavlink import quaternion @@ -2992,6 +2993,59 @@ class AutoTestPlane(AutoTest): True, True) + def WatchdogHome(self): + if self.gdb: + # we end up signalling the wrong process. I think. + # Probably need to have a "sitl_pid()" method to get the + # ardupilot process's PID. + self.progress("######## Skipping WatchdogHome test under GDB") + return + + ex = None + try: + self.progress("Enabling watchdog") + self.set_parameter("BRD_OPTIONS", 1 << 0) + self.reboot_sitl() + self.wait_ready_to_arm() + self.progress("Explicitly setting home to a known location") + orig_home = self.poll_home_position() + new_home = orig_home + new_home.latitude = new_home.latitude + 1000 + new_home.longitude = new_home.longitude + 2000 + new_home.altitude = new_home.altitude + 300000 # 300 metres + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, + 0, # p1, + 0, # p2, + 0, # p3, + 0, # p4, + new_home.latitude, + new_home.longitude, + new_home.altitude/1000.0, # mm => m + ) + old_bootcount = self.get_parameter('STAT_BOOTCNT') + self.progress("Forcing watchdog reset") + os.kill(self.sitl.pid, signal.SIGALRM) + self.detect_and_handle_reboot(old_bootcount) + self.wait_statustext("WDG:") + self.wait_statustext("IMU1 is using GPS") # won't be come armable + self.progress("Verifying home position") + post_reboot_home = self.poll_home_position() + delta = self.get_distance_int(new_home, post_reboot_home) + max_delta = 1 + if delta > max_delta: + raise NotAchievedException( + "New home not where it should be (dist=%f) (want=%s) (got=%s)" % + (delta, str(new_home), str(post_reboot_home))) + except Exception as e: + self.print_exception_caught(e) + ex = e + + self.reboot_sitl() + + if ex is not None: + raise ex + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -3125,6 +3179,10 @@ class AutoTestPlane(AutoTest): "Test DeepStall Landing", self.fly_deepstall), + ("WatchdogHome", + "Ensure home is restored after watchdog reset", + self.WatchdogHome), + ("LargeMissions", "Test Manipulation of Large missions", self.test_large_missions),