diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index 438e6d7ce6658..b186711637b6b 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -175,6 +175,30 @@ def disabled_tests(self): "CPUFailsafe": " tracker doesn't have a CPU failsafe", } + def GPSForYaw(self): + '''Moving baseline GPS yaw''' + self.context_push() + + self.load_default_params_file("tracker-gps-for-yaw.parm") + self.reboot_sitl() + + self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True) + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + m_gps_raw = self.assert_receive_message("GPS2_RAW", verbose=True) + m_sim = self.assert_receive_message("SIMSTATE", verbose=True) + gps_raw_hdg = m_gps_raw.yaw * 0.01 + sim_hdg = mavextra.wrap_360(math.degrees(m_sim.yaw)) + if abs(gps_raw_hdg - sim_hdg) > 5: + raise NotAchievedException("GPS_RAW not tracking simstate yaw") + self.progress(f"yaw match ({gps_raw_hdg} vs {sim_hdg}") + + self.context_pop() + + self.reboot_sitl() + def tests(self): '''return list of all tests''' ret = super(AutoTestTracker, self).tests() @@ -186,5 +210,6 @@ def tests(self): self.NMEAOutput, self.SCAN, self.BaseMessageSet, + self.GPSForYaw, ]) return ret diff --git a/Tools/autotest/default_params/tracker-gps-for-yaw.parm b/Tools/autotest/default_params/tracker-gps-for-yaw.parm new file mode 100644 index 0000000000000..e978b5137679c --- /dev/null +++ b/Tools/autotest/default_params/tracker-gps-for-yaw.parm @@ -0,0 +1,11 @@ +# SITL GPS-for-yaw using two simulated UBlox GPSs +EK3_SRC1_YAW 2 +GPS_AUTO_CONFIG 0 +GPS_TYPE 17 +GPS_TYPE2 18 +GPS_POS1_Y -0.2 +GPS_POS2_Y 0.2 +SIM_GPS_POS_Y -0.2 +SIM_GPS2_POS_Y 0.2 +SIM_GPS2_DISABLE 0 +SIM_GPS2_HDG 1