Tools: heli autotune autotest save gains and add tune check
This commit is contained in:
parent
b1b4ddea94
commit
b03203c612
@ -876,7 +876,7 @@ class AutoTestHelicopter(AutoTestCopter):
|
||||
self.wait_statustext('AutoTune: Success', timeout=1000)
|
||||
now = self.get_sim_time()
|
||||
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
||||
self.land_and_disarm()
|
||||
self.autotune_land_and_save_gains()
|
||||
|
||||
# test pitch rate P and Rate D tuning
|
||||
self.set_parameters({
|
||||
@ -895,7 +895,7 @@ class AutoTestHelicopter(AutoTestCopter):
|
||||
self.wait_statustext('AutoTune: Success', timeout=1000)
|
||||
now = self.get_sim_time()
|
||||
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
||||
self.land_and_disarm()
|
||||
self.autotune_land_and_save_gains()
|
||||
|
||||
# test Roll rate P and Rate D tuning
|
||||
self.set_parameters({
|
||||
@ -914,7 +914,7 @@ class AutoTestHelicopter(AutoTestCopter):
|
||||
self.wait_statustext('AutoTune: Success', timeout=1000)
|
||||
now = self.get_sim_time()
|
||||
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
||||
self.land_and_disarm()
|
||||
self.autotune_land_and_save_gains()
|
||||
|
||||
# test Roll and pitch angle P tuning
|
||||
self.set_parameters({
|
||||
@ -935,7 +935,7 @@ class AutoTestHelicopter(AutoTestCopter):
|
||||
self.wait_statustext('AutoTune: Success', timeout=1000)
|
||||
now = self.get_sim_time()
|
||||
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
||||
self.land_and_disarm()
|
||||
self.autotune_land_and_save_gains()
|
||||
|
||||
# test yaw FF and rate P and Rate D
|
||||
self.set_parameters({
|
||||
@ -956,8 +956,7 @@ class AutoTestHelicopter(AutoTestCopter):
|
||||
self.wait_statustext('AutoTune: Success', timeout=1000)
|
||||
now = self.get_sim_time()
|
||||
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
||||
self.land_and_disarm()
|
||||
self.set_rc(8, 1000)
|
||||
self.autotune_land_and_save_gains()
|
||||
|
||||
# test yaw angle P tuning
|
||||
self.set_parameters({
|
||||
@ -974,12 +973,41 @@ class AutoTestHelicopter(AutoTestCopter):
|
||||
# hold position in loiter
|
||||
self.change_mode('AUTOTUNE')
|
||||
|
||||
tstart = self.get_sim_time()
|
||||
self.wait_statustext('AutoTune: Success', timeout=1000)
|
||||
now = self.get_sim_time()
|
||||
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
||||
self.autotune_land_and_save_gains()
|
||||
|
||||
# tune check
|
||||
self.set_parameters({
|
||||
"AUTOTUNE_AXES": 7,
|
||||
"AUTOTUNE_SEQ": 16,
|
||||
"AUTOTUNE_FRQ_MIN": 10,
|
||||
"AUTOTUNE_FRQ_MAX": 80,
|
||||
})
|
||||
|
||||
# Conduct testing from althold
|
||||
self.takeoff(10, mode="ALT_HOLD")
|
||||
|
||||
# hold position in loiter
|
||||
self.change_mode('AUTOTUNE')
|
||||
|
||||
tstart = self.get_sim_time()
|
||||
self.wait_statustext('AutoTune: Success', timeout=1000)
|
||||
now = self.get_sim_time()
|
||||
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
|
||||
self.land_and_disarm()
|
||||
|
||||
def autotune_land_and_save_gains(self):
|
||||
self.set_rc(3, 1000)
|
||||
self.context_collect('STATUSTEXT')
|
||||
self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s",
|
||||
check_context=True,
|
||||
regex=True)
|
||||
self.set_rc(8, 1000)
|
||||
self.wait_disarmed()
|
||||
|
||||
def land_and_disarm(self, **kwargs):
|
||||
super(AutoTestHelicopter, self).land_and_disarm(**kwargs)
|
||||
self.progress("Killing rotor speed")
|
||||
|
Loading…
Reference in New Issue
Block a user