mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-24 09:38:29 -04:00
Tools: autotest: add autotest for sink-rate triggering
This commit is contained in:
parent
4881d32b18
commit
d05a4a04c5
@ -56,8 +56,11 @@ class AutoTestPlane(AutoTest):
|
|||||||
def set_autodisarm_delay(self, delay):
|
def set_autodisarm_delay(self, delay):
|
||||||
self.set_parameter("LAND_DISARMDELAY", delay)
|
self.set_parameter("LAND_DISARMDELAY", delay)
|
||||||
|
|
||||||
def takeoff(self):
|
def takeoff(self, alt=150, alt_max=None, relative=True):
|
||||||
"""Takeoff get to 30m altitude."""
|
"""Takeoff to altitude."""
|
||||||
|
|
||||||
|
if alt_max is None:
|
||||||
|
alt_max = alt + 30
|
||||||
|
|
||||||
self.mavproxy.send('switch 4\n')
|
self.mavproxy.send('switch 4\n')
|
||||||
self.wait_mode('FBWA')
|
self.wait_mode('FBWA')
|
||||||
@ -85,9 +88,7 @@ class AutoTestPlane(AutoTest):
|
|||||||
self.set_rc(3, 2000)
|
self.set_rc(3, 2000)
|
||||||
|
|
||||||
# gain a bit of altitude
|
# gain a bit of altitude
|
||||||
self.wait_altitude(self.homeloc.alt+150,
|
self.wait_altitude(alt, alt_max, timeout=30, relative=relative)
|
||||||
self.homeloc.alt+180,
|
|
||||||
timeout=30)
|
|
||||||
|
|
||||||
# level off
|
# level off
|
||||||
self.set_rc(2, 1500)
|
self.set_rc(2, 1500)
|
||||||
@ -774,6 +775,26 @@ class AutoTestPlane(AutoTest):
|
|||||||
self.disarm_vehicle(force=True)
|
self.disarm_vehicle(force=True)
|
||||||
self.reboot_sitl()
|
self.reboot_sitl()
|
||||||
|
|
||||||
|
def test_parachute_sinkrate(self):
|
||||||
|
self.set_rc(9, 1000)
|
||||||
|
self.set_parameter("CHUTE_ENABLED", 1)
|
||||||
|
self.set_parameter("CHUTE_TYPE", 10)
|
||||||
|
self.set_parameter("SERVO9_FUNCTION", 27)
|
||||||
|
self.set_parameter("SIM_PARA_ENABLE", 1)
|
||||||
|
self.set_parameter("SIM_PARA_PIN", 9)
|
||||||
|
|
||||||
|
self.set_parameter("CHUTE_CRT_SINK", 9)
|
||||||
|
|
||||||
|
self.progress("Takeoff")
|
||||||
|
self.takeoff(alt=300)
|
||||||
|
|
||||||
|
self.progress("Diving")
|
||||||
|
self.set_rc(2, 2000)
|
||||||
|
self.mavproxy.expect("BANG")
|
||||||
|
|
||||||
|
self.disarm_vehicle(force=True)
|
||||||
|
self.reboot_sitl()
|
||||||
|
|
||||||
def run_subtest(self, desc, func):
|
def run_subtest(self, desc, func):
|
||||||
self.start_subtest(desc)
|
self.start_subtest(desc)
|
||||||
func()
|
func()
|
||||||
@ -926,6 +947,8 @@ class AutoTestPlane(AutoTest):
|
|||||||
|
|
||||||
("Parachute", "Test Parachute", self.test_parachute),
|
("Parachute", "Test Parachute", self.test_parachute),
|
||||||
|
|
||||||
|
("ParachuteSinkRate", "Test Parachute (SinkRate triggering)", self.test_parachute_sinkrate),
|
||||||
|
|
||||||
("AIRSPEED_AUTOCAL", "Test AIRSPEED_AUTOCAL", self.airspeed_autocal),
|
("AIRSPEED_AUTOCAL", "Test AIRSPEED_AUTOCAL", self.airspeed_autocal),
|
||||||
|
|
||||||
("RangeFinder",
|
("RangeFinder",
|
||||||
|
Loading…
Reference in New Issue
Block a user