From 0aeb6c0e2628cab6e70fbd1102e45f10678b3475 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Mar 2019 08:39:35 +1100 Subject: [PATCH] Tools: autotest: ensure initial home position is similar to sitl start pos --- Tools/autotest/common.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index f3e5df9dfc..78f028b2e9 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1872,6 +1872,20 @@ class AutoTest(ABC): if orig_home is None: raise AutoTestTimeoutException() self.progress("Original home: %s" % str(orig_home)) + # original home should be close to SITL home... + start_loc = self.sitl_start_location() + self.progress("SITL start loc: %s" % str(start_loc)) + if abs(orig_home.latitude * 1.0e-7 - start_loc.lat) > 0.0000001: + raise ValueError("homes differ in lat") + if abs(orig_home.longitude * 1.0e-7 - start_loc.lng) > 0.0000001: + raise ValueError("homes differ in lon") + if self.is_rover(): + self.progress("### Rover skipping altitude check unti position fixes in") + else: + home_alt_m = orig_home.altitude * 1.0e-3 + if abs(home_alt_m - start_loc.alt) > 2: # metres + raise ValueError("homes differ in alt got=%fm want=%fm" % + (home_alt_m, start_loc.alt)) new_x = orig_home.latitude + 1000 new_y = orig_home.longitude + 2000 new_z = orig_home.altitude + 300000 # 300 metres