mirror of https://github.com/ArduPilot/ardupilot
autotest: add PID notches test
This commit is contained in:
parent
aad51b7066
commit
61e91dde35
|
@ -5960,6 +5960,49 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
||||||
freqs.append(m.PkAvg)
|
freqs.append(m.PkAvg)
|
||||||
return numpy.median(numpy.asarray(freqs)), len(freqs)
|
return numpy.median(numpy.asarray(freqs)), len(freqs)
|
||||||
|
|
||||||
|
def PIDNotches(self):
|
||||||
|
"""Use dynamic harmonic notch to control motor noise."""
|
||||||
|
self.progress("Flying with PID notches")
|
||||||
|
self.context_push()
|
||||||
|
|
||||||
|
ex = None
|
||||||
|
try:
|
||||||
|
self.set_parameters({
|
||||||
|
"AHRS_EKF_TYPE": 10,
|
||||||
|
"INS_LOG_BAT_MASK": 3,
|
||||||
|
"INS_LOG_BAT_OPT": 0,
|
||||||
|
"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour
|
||||||
|
"LOG_BITMASK": 65535,
|
||||||
|
"LOG_DISARMED": 0,
|
||||||
|
"SIM_VIB_FREQ_X": 120, # roll
|
||||||
|
"SIM_VIB_FREQ_Y": 120, # pitch
|
||||||
|
"SIM_VIB_FREQ_Z": 180, # yaw
|
||||||
|
"ATC_RAT_RLL_ADV": 1,
|
||||||
|
"ATC_RAT_RLL_NEF": 120,
|
||||||
|
"ATC_RAT_RLL_NBW": 50,
|
||||||
|
"ATC_RAT_PIT_ADV": 1,
|
||||||
|
"ATC_RAT_PIT_NEF": 120,
|
||||||
|
"ATC_RAT_PIT_NBW": 50,
|
||||||
|
"ATC_RAT_YAW_ADV": 1,
|
||||||
|
"ATC_RAT_YAW_NEF": 180,
|
||||||
|
"ATC_RAT_YAW_NBW": 50,
|
||||||
|
"SIM_GYR1_RND": 5,
|
||||||
|
})
|
||||||
|
self.reboot_sitl()
|
||||||
|
|
||||||
|
self.takeoff(10, mode="ALT_HOLD")
|
||||||
|
|
||||||
|
freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.print_exception_caught(e)
|
||||||
|
ex = e
|
||||||
|
|
||||||
|
self.context_pop()
|
||||||
|
|
||||||
|
if ex is not None:
|
||||||
|
raise ex
|
||||||
|
|
||||||
def ThrottleGainBoost(self):
|
def ThrottleGainBoost(self):
|
||||||
"""Use PD and Angle P boost for anti-gravity."""
|
"""Use PD and Angle P boost for anti-gravity."""
|
||||||
# basic gyro sample rate test
|
# basic gyro sample rate test
|
||||||
|
@ -10561,6 +10604,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
|
||||||
Test(self.DynamicNotches, attempts=4),
|
Test(self.DynamicNotches, attempts=4),
|
||||||
self.PositionWhenGPSIsZero,
|
self.PositionWhenGPSIsZero,
|
||||||
Test(self.DynamicRpmNotches, attempts=4),
|
Test(self.DynamicRpmNotches, attempts=4),
|
||||||
|
self.PIDNotches,
|
||||||
self.RefindGPS,
|
self.RefindGPS,
|
||||||
Test(self.GyroFFT, attempts=1, speedup=8),
|
Test(self.GyroFFT, attempts=1, speedup=8),
|
||||||
Test(self.GyroFFTHarmonic, attempts=4, speedup=8),
|
Test(self.GyroFFTHarmonic, attempts=4, speedup=8),
|
||||||
|
|
|
@ -801,6 +801,49 @@ class AutoTestHelicopter(AutoTestCopter):
|
||||||
self.set_rc(8, 1000)
|
self.set_rc(8, 1000)
|
||||||
self.disarm_vehicle()
|
self.disarm_vehicle()
|
||||||
|
|
||||||
|
def PIDNotches(self):
|
||||||
|
"""Use dynamic harmonic notch to control motor noise."""
|
||||||
|
self.progress("Flying with PID notches")
|
||||||
|
self.context_push()
|
||||||
|
|
||||||
|
ex = None
|
||||||
|
try:
|
||||||
|
self.set_parameters({
|
||||||
|
"AHRS_EKF_TYPE": 10,
|
||||||
|
"INS_LOG_BAT_MASK": 3,
|
||||||
|
"INS_LOG_BAT_OPT": 0,
|
||||||
|
"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour
|
||||||
|
"LOG_BITMASK": 65535,
|
||||||
|
"LOG_DISARMED": 0,
|
||||||
|
"SIM_VIB_FREQ_X": 120, # roll
|
||||||
|
"SIM_VIB_FREQ_Y": 120, # pitch
|
||||||
|
"SIM_VIB_FREQ_Z": 180, # yaw
|
||||||
|
"ATC_RAT_RLL_ADV": 1,
|
||||||
|
"ATC_RAT_RLL_NEF": 120,
|
||||||
|
"ATC_RAT_RLL_NBW": 50,
|
||||||
|
"ATC_RAT_PIT_ADV": 1,
|
||||||
|
"ATC_RAT_PIT_NEF": 120,
|
||||||
|
"ATC_RAT_PIT_NBW": 50,
|
||||||
|
"ATC_RAT_YAW_ADV": 1,
|
||||||
|
"ATC_RAT_YAW_NEF": 180,
|
||||||
|
"ATC_RAT_YAW_NBW": 50,
|
||||||
|
"SIM_GYR1_RND": 5,
|
||||||
|
})
|
||||||
|
self.reboot_sitl()
|
||||||
|
|
||||||
|
self.takeoff(10, mode="ALT_HOLD")
|
||||||
|
|
||||||
|
freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.print_exception_caught(e)
|
||||||
|
ex = e
|
||||||
|
|
||||||
|
self.context_pop()
|
||||||
|
|
||||||
|
if ex is not None:
|
||||||
|
raise ex
|
||||||
|
|
||||||
def tests(self):
|
def tests(self):
|
||||||
'''return list of all tests'''
|
'''return list of all tests'''
|
||||||
ret = vehicle_test_suite.TestSuite.tests(self)
|
ret = vehicle_test_suite.TestSuite.tests(self)
|
||||||
|
@ -817,6 +860,7 @@ class AutoTestHelicopter(AutoTestCopter):
|
||||||
self.AirspeedDrivers,
|
self.AirspeedDrivers,
|
||||||
self.TurbineStart,
|
self.TurbineStart,
|
||||||
self.NastyMission,
|
self.NastyMission,
|
||||||
|
self.PIDNotches,
|
||||||
])
|
])
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue