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

autotest: Add a test for GLIDE_SLOPE_THR

Browse Source
This commit is contained in:
Samuel Tabor 2021-12-15 10:36:41 +00:00 committed by Peter Barker
parent a715472353
commit 71596c2c9f
2 changed files with 102 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Tools/autotest/ArduPlane_Tests/GlideSlopeThresh/rapid-descent-then-climb.txt Normal file
View File

@ -0,0 +1,13 @@
QGC WPL 110
0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1
1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361065 149.164916 48.849998 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359467 149.161697 100.000000 1
3 0 3 31 0.000000 0.000000 0.000000 0.000000 -35.359908 149.161790 200.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361402 149.161984 200.000000 1
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361477 149.161986 99.550003 1
6 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363121 149.162222 200.000000 1
7 0 3 31 0.000000 0.000000 0.000000 0.000000 -35.365708 149.162588 100.000000 1
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366333 149.162659 80.000000 1
9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366161 149.165617 50.000000 1
10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365403 149.165512 30.000000 1
11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362777 149.165172 0.000000 1

89
Tools/autotest/arduplane.py
View File

@ -3348,6 +3348,91 @@ function'''
if m.intake_manifold_temperature < 20:
raise NotAchievedException("Bad intake manifold temperature")
def test_glide_slope_threshold(self):
# Test that GLIDE_SLOPE_THRESHOLD correctly controls re-planning glide slope
# in the scenario that aircraft is above planned slope and slope is positive (climbing).
#
#
# Behaviour with GLIDE_SLOPE_THRESH = 0 (no slope replanning)
# (2).. __(4)
# | \..__/
# | __/
# (3)
#
# Behaviour with GLIDE_SLOPE_THRESH = 5 (slope replanning when >5m error)
# (2)........__(4)
# | __/
# | __/
# (3)
# Solid is plan, dots are actual flightpath.
self.load_mission('rapid-descent-then-climb.txt', strict=False)
self.set_current_waypoint(1)
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
#
# Initial run with GLIDE_SLOPE_THR = 5 (default).
#
self.set_parameter("GLIDE_SLOPE_THR", 5)
# Wait for waypoint commanding rapid descent, followed by climb.
self.wait_current_waypoint(5, timeout=1200)
# Altitude should not descend significantly below the initial altitude
init_altitude = self.get_altitude(relative=True, timeout=2)
timeout = 600
wpnum = 7
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > timeout:
raise AutoTestTimeoutException("Did not get wanted current waypoint")
if (self.get_altitude(relative=True, timeout=2) - init_altitude) < -10:
raise NotAchievedException("Descended >10m before reaching desired waypoint,\
indicating slope was not replanned")
seq = self.mav.waypoint_current()
self.progress("Waiting for wp=%u current=%u" % (wpnum, seq))
if seq == wpnum:
break
self.set_current_waypoint(2)
#
# Second run with GLIDE_SLOPE_THR = 0 (no re-plan).
#
self.set_parameter("GLIDE_SLOPE_THR", 0)
# Wait for waypoint commanding rapid descent, followed by climb.
self.wait_current_waypoint(5, timeout=1200)
# This time altitude should descend significantly below the initial altitude
init_altitude = self.get_altitude(relative=True, timeout=2)
timeout = 600
wpnum = 7
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > timeout:
raise AutoTestTimeoutException("Did not get wanted altitude")
seq = self.mav.waypoint_current()
self.progress("Waiting for wp=%u current=%u" % (wpnum, seq))
if seq == wpnum:
raise NotAchievedException("Reached desired waypoint without first decending 10m,\
indicating slope was replanned unexpectedly")
if (self.get_altitude(relative=True, timeout=2) - init_altitude) < -10:
break
# Disarm
self.wait_disarmed(timeout=600)
self.progress("Mission OK")
def tests(self):
'''return list of all tests'''
ret = super(AutoTestPlane, self).tests()
@ -3601,6 +3686,10 @@ function'''
"Test soaring speed-to-fly",
self.fly_soaring_speed_to_fly),
("GlideSlopeThresh",
"Test rebuild glide slope if above and climbing",
self.test_glide_slope_threshold),
("LogUpload",
"Log upload",
self.log_upload),