diff --git a/Tools/autotest/default_params/quadplane.parm b/Tools/autotest/default_params/quadplane.parm index 1bd268b724..9da3368889 100644 --- a/Tools/autotest/default_params/quadplane.parm +++ b/Tools/autotest/default_params/quadplane.parm @@ -76,8 +76,8 @@ Q_A_RAT_RLL_D 0.001 Q_A_RAT_PIT_P 0.103 Q_A_RAT_PIT_I 0.103 Q_A_RAT_PIT_D 0.001 -Q_A_RAT_YAW_P 0.4 -Q_A_RAT_YAW_P 0.04 +Q_A_RAT_YAW_P 0.2 +Q_A_RAT_YAW_P 0.02 Q_A_ANG_RLL_P 6 Q_A_ANG_PIT_P 6 RTL_ALTITUDE 20.00 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 7296e34945..721752a5c3 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -363,6 +363,20 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite): def QAUTOTUNE(self): '''test Plane QAutoTune mode''' + # adjust tune so QAUTOTUNE can cope + self.set_parameters({ + "Q_A_RAT_RLL_P" : 0.15, + "Q_A_RAT_RLL_I" : 0.25, + "Q_A_RAT_RLL_D" : 0.002, + "Q_A_RAT_PIT_P" : 0.15, + "Q_A_RAT_PIT_I" : 0.25, + "Q_A_RAT_PIT_D" : 0.002, + "Q_A_RAT_YAW_P" : 0.18, + "Q_A_RAT_YAW_I" : 0.018, + "Q_A_ANG_RLL_P" : 4.5, + "Q_A_ANG_PIT_P" : 4.5, + }) + # this is a list of all parameters modified by QAUTOTUNE. Set # them so that when the context is popped we get the original # values back: @@ -1411,6 +1425,81 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite): self.wait_disarmed(timeout=120) + def VTOLQuicktune_CPP(self): + '''VTOL Quicktune in C++''' + self.set_parameters({ + "RC7_OPTION": 181, + "QWIK_ENABLE" : 1, + "QWIK_DOUBLE_TIME" : 5, # run faster for autotest + }) + + self.context_push() + self.context_collect('STATUSTEXT') + + # reduce roll/pitch gains by 2 + gain_mul = 0.5 + soften_params = ['Q_A_RAT_RLL_P', 'Q_A_RAT_RLL_I', 'Q_A_RAT_RLL_D', + 'Q_A_RAT_PIT_P', 'Q_A_RAT_PIT_I', 'Q_A_RAT_PIT_D', + 'Q_A_RAT_YAW_P', 'Q_A_RAT_YAW_I'] + + original_values = self.get_parameters(soften_params) + + softened_values = {} + for p in original_values.keys(): + softened_values[p] = original_values[p] * gain_mul + self.set_parameters(softened_values) + + self.wait_ready_to_arm() + self.change_mode("QLOITER") + self.set_rc(7, 1000) + self.arm_vehicle() + self.takeoff(20, 'QLOITER') + + # use rc switch to start tune + self.set_rc(7, 1500) + + self.wait_text("Quicktune: starting tune", check_context=True) + for axis in ['Roll', 'Pitch', 'Yaw']: + self.wait_text("Starting %s tune" % axis, check_context=True) + self.wait_text("Quicktune: %s D done" % axis, check_context=True, timeout=120) + self.wait_text("Quicktune: %s P done" % axis, check_context=True, timeout=120) + self.wait_text("Quicktune: %s done" % axis, check_context=True, timeout=120) + + new_values = self.get_parameters(soften_params) + for p in original_values.keys(): + threshold = 0.8 * original_values[p] + self.progress("tuned param %s %.4f need %.4f" % (p, new_values[p], threshold)) + if new_values[p] < threshold: + raise NotAchievedException( + "parameter %s %.4f not increased over %.4f" % + (p, new_values[p], threshold)) + + self.progress("ensure we are not overtuned") + self.set_parameter('SIM_ENGINE_MUL', 0.9) + + self.delay_sim_time(5) + + # and restore it + self.set_parameter('SIM_ENGINE_MUL', 1) + + for i in range(5): + self.wait_heartbeat() + + if self.statustext_in_collections("ABORTING"): + raise NotAchievedException("tune has aborted, overtuned") + + self.progress("using aux fn for save tune") + + # to test aux function method, use aux fn for save + self.run_auxfunc(181, 2) + self.wait_text("Quicktune: saved", check_context=True) + self.change_mode("QLAND") + + self.wait_disarmed(timeout=120) + self.set_parameter("QWIK_ENABLE", 0) + self.context_pop() + self.reboot_sitl() + def PrecisionLanding(self): '''VTOL precision landing''' @@ -2104,6 +2193,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite): self.LoiterAltQLand, self.VTOLLandSpiral, self.VTOLQuicktune, + self.VTOLQuicktune_CPP, self.PrecisionLanding, self.ShipLanding, Test(self.MotorTest, kwargs={ # tests motors 4 and 2