Skip to content

Commit

Permalink
autotest: GainBackoffTakeoff test
Browse files Browse the repository at this point in the history
  • Loading branch information
andyp1per authored and peterbarker committed Sep 25, 2024
1 parent c9de940 commit a8ecc23
Showing 1 changed file with 121 additions and 0 deletions.
121 changes: 121 additions & 0 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -10630,6 +10630,7 @@ def tests1c(self):
self.SetpointBadVel,
self.SplineTerrain,
self.TakeoffCheck,
self.GainBackoffTakeoff,
])
return ret

Expand Down Expand Up @@ -10769,6 +10770,126 @@ def AHRSTrimLand(self):
self.context_pop()
self.reboot_sitl()

def GainBackoffTakeoff(self):
'''test gain backoff on takeoff'''
self.context_push()
self.progress("Test gains are fully backed off")
self.set_parameters({
"ATC_LAND_R_MULT": 0.0,
"ATC_LAND_P_MULT": 0.0,
"ATC_LAND_Y_MULT": 0.0,
"GCS_PID_MASK" : 7,
"LOG_BITMASK": 180222,
})
self.reboot_sitl()
self.wait_ready_to_arm()
self.change_mode('ALT_HOLD')

class ValidatePDZero(vehicle_test_suite.TestSuite.MessageHook):
'''asserts correct values in PID_TUNING'''

def __init__(self, suite, axis):
super(ValidatePDZero, self).__init__(suite)
self.pid_tuning_count = 0
self.p_sum = 0
self.d_sum = 0
self.i_sum = 0
self.axis = axis

def hook_removed(self):
if self.pid_tuning_count == 0:
raise NotAchievedException("Did not get PID_TUNING")
if self.i_sum == 0:
raise ValueError("I sum is zero")
print(f"ValidatePDZero: PID_TUNING count: {self.pid_tuning_count}")

def process(self, mav, m):
if m.get_type() != 'PID_TUNING' or m.axis != self.axis:
return
self.pid_tuning_count += 1
self.p_sum += m.P
self.d_sum += m.D
self.i_sum += m.I
if self.p_sum > 0:
raise ValueError("P sum is not zero")
if self.d_sum > 0:
raise ValueError("D sum is not zero")

self.progress("Check that PD values are zero")
self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_ROLL))
self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_PITCH))
self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_YAW))
# until the context pop happens, all received PID_TUNINGS will be verified as good
self.arm_vehicle()
self.set_rc(3, 1500)
self.delay_sim_time(2)
self.set_rc(2, 1250)
self.delay_sim_time(5)
self.assert_receive_message('PID_TUNING', timeout=5)
self.set_rc_default()
self.zero_throttle()
self.disarm_vehicle()
self.context_pop()

self.context_push()
self.progress("Test gains are not backed off")
self.set_parameters({
"ATC_LAND_R_MULT": 1.0,
"ATC_LAND_P_MULT": 1.0,
"ATC_LAND_Y_MULT": 1.0,
"GCS_PID_MASK" : 7,
"LOG_BITMASK": 180222,
})
self.reboot_sitl()
self.wait_ready_to_arm()
self.change_mode('ALT_HOLD')

class ValidatePDNonZero(vehicle_test_suite.TestSuite.MessageHook):
'''asserts correct values in PID_TUNING'''

def __init__(self, suite, axis):
super(ValidatePDNonZero, self).__init__(suite)
self.pid_tuning_count = 0
self.p_sum = 0
self.d_sum = 0
self.i_sum = 0
self.axis = axis

def hook_removed(self):
if self.pid_tuning_count == 0:
raise NotAchievedException("Did not get PID_TUNING")
if self.p_sum == 0:
raise ValueError("P sum is zero")
if self.i_sum == 0:
raise ValueError("I sum is zero")
print(f"ValidatePDNonZero: PID_TUNING count: {self.pid_tuning_count}")

def process(self, mav, m):
if m.get_type() != 'PID_TUNING' or m.axis != self.axis:
return
self.pid_tuning_count += 1
self.p_sum += m.P
self.d_sum += m.D
self.i_sum += m.I

self.progress("Check that PD values are non-zero")
self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_ROLL))
self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_PITCH))
self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_YAW))
# until the context pop happens, all received PID_TUNINGS will be verified as good
self.arm_vehicle()
self.set_rc(3, 1500)
self.delay_sim_time(2)
self.set_rc(2, 1250)
self.delay_sim_time(5)
self.assert_receive_message('PID_TUNING', timeout=5)
self.set_rc_default()
self.zero_throttle()
self.disarm_vehicle()

self.context_pop()
self.reboot_sitl()

def turn_safety_x(self, value):
self.mav.mav.set_mode_send(
self.mav.target_system,
Expand Down

0 comments on commit a8ecc23

Please sign in to comment.