Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tools: autotest: Plane: expand guided attitude target test

Browse Source
This commit is contained in:
Iampete1 2023-04-14 18:50:58 +01:00 committed by Andrew Tridgell
parent 5ec479579f
commit 6659b02e14
1 changed files with 70 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
Tools/autotest/arduplane.py
View File

@ -318,78 +318,83 @@ class AutoTestPlane(AutoTest):
def set_attitude_target(self, tolerance=10): def set_attitude_target(self, tolerance=10):
"""Test setting of attitude target in guided mode.""" """Test setting of attitude target in guided mode."""
self.change_mode("GUIDED") self.change_mode("GUIDED")
# self.set_parameter("STALL_PREVENTION", 0)
state_roll_over = "roll-over" steps = [{"name": "roll-over", "roll": 60, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000001},
state_stabilize_roll = "stabilize-roll" {"name": "roll-back", "roll": 0, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000001},
{"name": "pitch-up+throttle", "roll": 0, "pitch": 20, "yaw": 0, "throttle": 1, "type_mask": 0b11000010},
{"name": "pitch-back", "roll": 0, "pitch": 0, "yaw": 0, "throttle": 0, "type_mask": 0b10000010}]
state_wait = "wait"
state_hold = "hold" state_hold = "hold"
state_roll_back = "roll-back"
state_done = "done"
tstart = self.get_sim_time()
try: try:
state = state_roll_over for step in steps:
while state != state_done: step_start = self.get_sim_time_cached()
state = state_wait
state_start = self.get_sim_time_cached()
while True:
m = self.mav.recv_match(type='ATTITUDE',
blocking=True,
timeout=0.1)
now = self.get_sim_time_cached()
if now - step_start > 30:
raise AutoTestTimeoutException("Manuevers not completed")
if m is None:
continue
m = self.mav.recv_match(type='ATTITUDE', angle_error = 0
blocking=True, if (step["type_mask"] & 0b00000001) or (step["type_mask"] == 0b10000000):
timeout=0.1) angle_error += abs(math.degrees(m.roll) - step["roll"])
now = self.get_sim_time_cached()
if now - tstart > 20:
raise AutoTestTimeoutException("Manuevers not completed")
if m is None:
continue
r = math.degrees(m.roll) if (step["type_mask"] & 0b00000010) or (step["type_mask"] == 0b10000000):
if state == state_roll_over: angle_error += abs(math.degrees(m.pitch) - step["pitch"])
target_roll_degrees = 60
if abs(r - target_roll_degrees) < tolerance:
state = state_stabilize_roll
stabilize_start = now
elif state == state_stabilize_roll:
# just give it a little time to sort it self out
if now - stabilize_start > 2:
state = state_hold
hold_start = now
elif state == state_hold:
target_roll_degrees = 60
if now - hold_start > tolerance:
state = state_roll_back
if abs(r - target_roll_degrees) > tolerance:
raise NotAchievedException("Failed to hold attitude")
elif state == state_roll_back:
target_roll_degrees = 0
if abs(r - target_roll_degrees) < tolerance:
state = state_done
else:
raise ValueError("Unknown state %s" % str(state))
m_nav = self.mav.messages['NAV_CONTROLLER_OUTPUT'] if (step["type_mask"] & 0b00000100) or (step["type_mask"] == 0b10000000):
self.progress("%s Roll: %f desired=%f set=%f" % # Strictly we should angle wrap, by plane doesn't support yaw correctly anyway so its not tested here
(state, r, m_nav.nav_roll, target_roll_degrees)) angle_error += abs(math.degrees(m.yaw) - step["yaw"])
time_boot_millis = 0 # FIXME # Note were not checking throttle, however the SITL plane needs full throttle to meet the
target_system = 1 # FIXME # target pitch attitude, Pitch test will fail without throttle override
target_component = 1 # FIXME
type_mask = 0b10000001 ^ 0xFF # FIXME if state == state_wait:
# attitude in radians: # Reduced tolerance for initial trigger
q = quaternion.Quaternion([math.radians(target_roll_degrees), if angle_error < (tolerance * 0.25):
0, state = state_hold
0]) state_start = now
roll_rate_radians = 0.5
pitch_rate_radians = 0 # Allow 10 seconds to reach attitude
yaw_rate_radians = 0 if (now - state_start) > 10:
thrust = 1.0 raise NotAchievedException(step["name"] + ": Failed to get to set attitude")
self.mav.mav.set_attitude_target_send(time_boot_millis,
target_system, elif state == state_hold:
target_component, # Give 2 seconds to stabilize
type_mask, if (now - state_start) > 2 and not (angle_error < tolerance):
q, raise NotAchievedException(step["name"] + ": Failed to hold set attitude")
roll_rate_radians,
pitch_rate_radians, # Hold for 10 seconds
yaw_rate_radians, if (now - state_start) > 12:
thrust) # move onto next step
self.progress("%s Done" % (step["name"]))
break
self.progress("%s %s error: %f" % (step["name"], state, angle_error))
time_boot_millis = 0 # FIXME
target_system = 1 # FIXME
target_component = 1 # FIXME
type_mask = step["type_mask"] ^ 0xFF # FIXME
# attitude in radians:
q = quaternion.Quaternion([math.radians(step["roll"]),
math.radians(step["pitch"]),
math.radians(step["yaw"])])
self.mav.mav.set_attitude_target_send(time_boot_millis,
target_system,
target_component,
type_mask,
q,
0, # roll rate, not used in AP
0, # pitch rate, not used in AP
0, # yaw rate, not used in AP
step["throttle"])
except Exception as e: except Exception as e:
self.change_mode('FBWA') self.change_mode('FBWA')
self.set_rc(3, 1700) self.set_rc(3, 1700)