diff --git a/Tools/autotest/ArduPlane_Tests/ParachuteLanding/parachute_landing_mission.txt b/Tools/autotest/ArduPlane_Tests/ParachuteLanding/parachute_landing_mission.txt new file mode 100644 index 00000000000000..7b4c6d598a10b7 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/ParachuteLanding/parachute_landing_mission.txt @@ -0,0 +1,11 @@ +QGC WPL 110 +0 1 0 16 0 0 0 0 -35.3632622 149.1652376 584.210000 1 +1 0 3 22 15.00000000 0.00000000 0.00000000 0.00000000 -35.36271850 149.16512130 10.000000 1 +2 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36000620 149.16476730 100.000000 1 +3 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.35930620 149.16991710 100.000000 1 +4 0 0 178 0.00000000 18.00000000 -1.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +5 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36021620 149.17107580 100.000000 1 +6 0 3 18 2.00000000 0.00000000 80.00000000 1.00000000 -35.36543070 149.17191270 100.000000 1 +7 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.37064510 149.17274950 100.000000 1 +8 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.37113500 149.16626930 100.000000 1 +9 0 3 21 0.00000000 0.00000000 0.00000000 1.00000000 -35.36336600 149.16519640 80.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index a580e70d5e7af4..2453d182dc3376 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1429,6 +1429,56 @@ def GCSFailsafe(self): self.reboot_sitl() self.end_subtest("Completed Parachute Failsafe test") + def ParachuteLanding(self): + '''Test parachute landing method''' + self.start_subtest("Ensure TECS_LAND_ARSPD is adopted on approach") + self.set_parameters({ + "CHUTE_ENABLED": 1, + "CHUTE_TYPE": 10, + "SERVO9_FUNCTION": 27, + "SIM_PARA_ENABLE": 1, + "SIM_PARA_PIN": 9, + "FS_LONG_ACTN": 3, + "TECS_LAND_ARSPD":15, + "TRIM_ARSPD_CM":2400, + "LAND_TYPE":2, + "LOG_BITMASK":1081279,# ensure full rate attitude logging + }) + self.load_mission("parachute_landing_mission.txt") + self.reboot_sitl() + self.context_push() + + # Iterate over the test a number of times to look for an inconsistancy + n_tests = 1 + for t in range(n_tests): + self.progress("Landing test: %i of %i" % (t+1, n_tests)) + + self.wait_ready_to_arm() + self.set_rc(3, 1000) + self.change_mode("AUTO") + self.arm_vehicle() + self.set_rc(3, 1500) + + # Test for expected airspeeds around the mission + checks = [ + (3, self.get_parameter("TRIM_ARSPD_CM") * 0.01), + (5, 18), # looking for do_change_speed value + (9, self.get_parameter("TECS_LAND_ARSPD")), # looking for approach speed + ] + for (current_waypoint, want_airspeed) in checks: + self.wait_current_waypoint(current_waypoint, timeout=150) + self.wait_airspeed(want_airspeed-1, want_airspeed+1, minimum_duration=6, timeout=120) + + # If we got this far look for the parachute deployment + self.wait_statustext("BANG", timeout=60) + self.set_heartbeat_rate(self.speedup) + self.disarm_vehicle(force=True) + self.reboot_sitl() + self.end_subtest("Completed parachute landing test %i" % t) + + self.context_pop() + + def TestGripperMission(self): '''Test Gripper mission items''' self.context_push() @@ -5345,6 +5395,7 @@ def tests(self): self.TerrainRally, self.MAV_CMD_NAV_LOITER_UNLIM, self.MAV_CMD_NAV_RETURN_TO_LAUNCH, + self.ParachuteLanding, ]) return ret