Skip to content

Commit

Permalink
Tools: Heli Autotune Autotest
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Feb 28, 2024
1 parent ac76901 commit 171b7e3
Showing 1 changed file with 226 additions and 16 deletions.
242 changes: 226 additions & 16 deletions Tools/autotest/helicopter.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -842,24 +842,234 @@ def PIDNotches(self):
if ex is not None:
raise ex

def AutoTune(self):
"""Test autotune mode"""
#test roll and pitch FF tuning
self.set_parameters({
"ATC_ANG_RLL_P": 4.5,
"ATC_RAT_RLL_P": 0,
"ATC_RAT_RLL_I": 0.1,
"ATC_RAT_RLL_D": 0,
"ATC_RAT_RLL_FF": 0.15,
"ATC_ANG_PIT_P": 4.5,
"ATC_RAT_PIT_P": 0,
"ATC_RAT_PIT_I": 0.1,
"ATC_RAT_PIT_D": 0,
"ATC_RAT_PIT_FF": 0.15,
"ATC_ANG_YAW_P": 4.5,
"ATC_RAT_YAW_P": 0.18,
"ATC_RAT_YAW_I": 0.024,
"ATC_RAT_YAW_D": 0.003,
"ATC_RAT_YAW_FF": 0.024,
"AUTOTUNE_AXES": 3,
"AUTOTUNE_SEQ": 1,
})

# Conduct testing from althold
self.takeoff(10, mode="ALT_HOLD")

# hold position in loiter
self.change_mode('AUTOTUNE')

tstart = self.get_sim_time()
sim_time_expected = 600
deadline = tstart + sim_time_expected
while self.get_sim_time_cached() < deadline:
now = self.get_sim_time_cached()
m = self.mav.recv_match(type='STATUSTEXT',
blocking=True,
timeout=1)
if m is None:
continue
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
if "AutoTune: Success" in m.text:
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
self.set_rc(3, 1000)
self.wait_statustext("SIM Hit ground at")
self.set_rc(8, 1000)
self.delay_sim_time(10)
self.disarm_vehicle()
# check the original gains have been re-instated
# if (rlld != self.get_parameter("ATC_RAT_RLL_D") or
# rlli != self.get_parameter("ATC_RAT_RLL_I") or
# rllp != self.get_parameter("ATC_RAT_RLL_P")):
# raise NotAchievedException("AUTOTUNE gains still present")
continue

# test pitch rate P and Rate D tuning
self.set_parameters({
"AUTOTUNE_AXES": 2,
"AUTOTUNE_SEQ": 2,
"AUTOTUNE_GN_MAX": 2.0,
})

# Conduct testing from althold
self.takeoff(10, mode="ALT_HOLD")

# hold position in loiter
self.change_mode('AUTOTUNE')

tstart = self.get_sim_time()
sim_time_expected = 600
deadline = tstart + sim_time_expected
while self.get_sim_time_cached() < deadline:
now = self.get_sim_time_cached()
m = self.mav.recv_match(type='STATUSTEXT',
blocking=True,
timeout=1)
if m is None:
continue
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
if "AutoTune: Success" in m.text:
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
self.set_rc(3, 1000)
self.wait_statustext("SIM Hit ground at")
self.set_rc(8, 1000)
self.delay_sim_time(10)
self.disarm_vehicle()
# check the original gains have been re-instated
# if (rlld != self.get_parameter("ATC_RAT_RLL_D") or
# rlli != self.get_parameter("ATC_RAT_RLL_I") or
# rllp != self.get_parameter("ATC_RAT_RLL_P")):
# raise NotAchievedException("AUTOTUNE gains still present")
continue

# test Roll rate P and Rate D tuning
self.set_parameters({
"AUTOTUNE_AXES": 1,
"AUTOTUNE_SEQ": 2,
"AUTOTUNE_GN_MAX": 1.8,
})

# Conduct testing from althold
self.takeoff(10, mode="ALT_HOLD")

# hold position in loiter
self.change_mode('AUTOTUNE')

tstart = self.get_sim_time()
sim_time_expected = 600
deadline = tstart + sim_time_expected
while self.get_sim_time_cached() < deadline:
now = self.get_sim_time_cached()
m = self.mav.recv_match(type='STATUSTEXT',
blocking=True,
timeout=1)
if m is None:
continue
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
if "AutoTune: Success" in m.text:
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
self.set_rc(3, 1000)
self.wait_statustext("SIM Hit ground at")
self.set_rc(8, 1000)
self.delay_sim_time(10)
self.disarm_vehicle()
# check the original gains have been re-instated
# if (rlld != self.get_parameter("ATC_RAT_RLL_D") or
# rlli != self.get_parameter("ATC_RAT_RLL_I") or
# rllp != self.get_parameter("ATC_RAT_RLL_P")):
# raise NotAchievedException("AUTOTUNE gains still present")
continue

# test Roll and pitch angle P tuning
self.set_parameters({
"AUTOTUNE_AXES": 3,
"AUTOTUNE_SEQ": 4,
"AUTOTUNE_GN_MAX": 2.0,
})

# Conduct testing from althold
self.takeoff(10, mode="ALT_HOLD")

# hold position in loiter
self.change_mode('AUTOTUNE')

tstart = self.get_sim_time()
sim_time_expected = 600
deadline = tstart + sim_time_expected
while self.get_sim_time_cached() < deadline:
now = self.get_sim_time_cached()
m = self.mav.recv_match(type='STATUSTEXT',
blocking=True,
timeout=1)
if m is None:
continue
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
if "AutoTune: Success" in m.text:
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
self.set_rc(3, 1000)
self.wait_statustext("SIM Hit ground at")
self.set_rc(8, 1000)
self.delay_sim_time(10)
self.disarm_vehicle()
# check the original gains have been re-instated
# if (rlld != self.get_parameter("ATC_RAT_RLL_D") or
# rlli != self.get_parameter("ATC_RAT_RLL_I") or
# rllp != self.get_parameter("ATC_RAT_RLL_P")):
# raise NotAchievedException("AUTOTUNE gains still present")
continue

# test yaw FF, rate P and Rate D, and angle P tuning
self.set_parameters({
"AUTOTUNE_AXES": 4,
"AUTOTUNE_SEQ": 7,
"AUTOTUNE_GN_MAX": 2.0,
})

# Conduct testing from althold
self.takeoff(10, mode="ALT_HOLD")

# hold position in loiter
self.change_mode('AUTOTUNE')

tstart = self.get_sim_time()
sim_time_expected = 1800
deadline = tstart + sim_time_expected
while self.get_sim_time_cached() < deadline:
now = self.get_sim_time_cached()
m = self.mav.recv_match(type='STATUSTEXT',
blocking=True,
timeout=1)
if m is None:
continue
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
if "AutoTune: Success" in m.text:
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
self.set_rc(3, 1000)
self.wait_statustext("SIM Hit ground at")
self.set_rc(8, 1000)
self.delay_sim_time(10)
self.disarm_vehicle()
# check the original gains have been re-instated
# if (rlld != self.get_parameter("ATC_RAT_RLL_D") or
# rlli != self.get_parameter("ATC_RAT_RLL_I") or
# rllp != self.get_parameter("ATC_RAT_RLL_P")):
# raise NotAchievedException("AUTOTUNE gains still present")
continue
raise NotAchievedException("AUTOTUNE failed (%u seconds)" %
(self.get_sim_time() - tstart))

def tests(self):
'''return list of all tests'''
ret = vehicle_test_suite.TestSuite.tests(self)
ret.extend([
self.AVCMission,
self.RotorRunup,
self.PosHoldTakeOff,
self.StabilizeTakeOff,
self.SplineWaypoint,
self.AutoRotation,
self.ManAutoRotation,
self.governortest,
self.FlyEachFrame,
self.AirspeedDrivers,
self.TurbineStart,
self.NastyMission,
self.PIDNotches,
])
# ret = vehicle_test_suite.TestSuite.tests(self)
ret = ([self.AutoTune])
# ret.extend([
# self.AVCMission,
# self.RotorRunup,
# self.PosHoldTakeOff,
# self.StabilizeTakeOff,
# self.SplineWaypoint,
# self.AutoRotation,
# self.ManAutoRotation,
# self.governortest,
# self.FlyEachFrame,
# self.AirspeedDrivers,
# self.TurbineStart,
# self.NastyMission,
# self.PIDNotches,
# self.AutoTune,
# ])
return ret

def disabled_tests(self):
Expand Down

0 comments on commit 171b7e3

Please sign in to comment.