diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 54fe5e86a5..d7c4f50bea 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3182,6 +3182,10 @@ class AutoTestPlane(AutoTest): "Test RC DisableAirspeedUse option", self.RCDisableAirspeedUse), + ("AHRS_ORIENTATION", + "Test AHRS_ORIENTATION parameter", + self.AHRS_ORIENTATION), + ("LogUpload", "Log upload", self.log_upload), diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 3eeaa13bbf..a324c2c469 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -3122,8 +3122,10 @@ class AutoTest(ABC): raise ValueError("count %u not handled" % count) self.progress("Files same") - def assert_receive_message(self, type, timeout=1): + def assert_receive_message(self, type, timeout=1, verbose=False): m = self.mav.recv_match(type=type, blocking=True, timeout=timeout) + if verbose: + self.progress("Received (%s)" % str(m)) if m is None: raise NotAchievedException("Did not get %s" % type) return m @@ -10389,6 +10391,25 @@ switch value''' if ex is not None: raise ex + def AHRS_ORIENTATION(self): + '''test AHRS_ORIENTATION parameter works''' + self.context_push() + self.wait_ready_to_arm() + original_imu = self.assert_receive_message("RAW_IMU", verbose=True) + self.set_parameter("AHRS_ORIENTATION", 16) # roll-90 + self.delay_sim_time(2) # we update this on a timer + new_imu = self.assert_receive_message("RAW_IMU", verbose=True) + delta_zacc = original_imu.zacc - new_imu.zacc + delta_z_g = delta_zacc/1000.0 # milligravities -> gravities + if delta_z_g - 1 > 0.1: # milligravities.... + raise NotAchievedException("Magic AHRS_ORIENTATION update did not work (delta_z_g=%f)" % (delta_z_g,)) + delta_yacc = original_imu.yacc - new_imu.yacc + delta_y_g = delta_yacc/1000.0 # milligravities -> gravities + if delta_y_g + 1 > 0.1: + raise NotAchievedException("Magic AHRS_ORIENTATION update did not work (delta_y_g=%f)" % (delta_y_g,)) + self.context_pop() + self.delay_sim_time(2) # we update orientation on a timer + def tests(self): return [ Test("PIDTuning",