Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: New tests 

Autotests for takeoffs have been added for Plane, covering AUTO and
TAKEOFF mode takeoffs.

An auxiliary `set_servo` method has been added to `vehicle_test_suite.py`.
This commit is contained in:
George Zogopoulos 2024-05-21 19:34:43 +02:00 committed by Andrew Tridgell
parent eadc7c0ecc
commit 1e6e291b52
6 changed files with 406 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.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.390015 1
1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1
6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1
9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1
10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1
11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1

13
Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.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.390015 1
1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1
6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1
9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1
10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1
11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1

13
Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.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.390015 1
1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1
6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1
9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1
10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1
11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1

13
Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.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.390015 1
1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1
6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1
9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1
10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1
11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1

350
Tools/autotest/arduplane.py
View File

@ -4371,6 +4371,349 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.wait_disarmed(timeout=180)
def TakeoffAuto1(self):
'''Test the behaviour of an AUTO takeoff, pt1.
Conditions:
- ARSPD_USE=1
- TKOFF_TYPE=0
- TKOFF_THR_MAX < THR_MAX
'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"ARSPD_USE": 1.0,
"THR_MAX": 100.0,
"TKOFF_THR_MAX": 80.0,
"TKOFF_THR_MINACC": 3.0,
"TECS_PITCH_MAX": 35.0,
"PTCH_LIM_MAX_DEG": 35.0,
"RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
})
# Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
self.load_mission("catapult.txt", strict=True)
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
# Throw the catapult.
self.set_servo(7, 2000)
# Wait until we're midway through the climb.
test_alt = 50
self.wait_altitude(test_alt, test_alt+2, relative=True)
# Ensure that by then the aircraft still goes at max allowed throttle.
self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX"))
# Wait for landing waypoint.
self.wait_current_waypoint(11, timeout=1200)
self.wait_disarmed(120)
def TakeoffAuto2(self):
'''Test the behaviour of an AUTO takeoff, pt2.
Conditions:
- ARSPD_USE=1
- TKOFF_MODE=0
- TKOFF_THR_MAX > THR_MAX
'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"ARSPD_USE": 0.0,
"THR_MAX": 80.0,
"TKOFF_THR_MAX": 100.0,
"TKOFF_THR_MINACC": 3.0,
"TECS_PITCH_MAX": 35.0,
"PTCH_LIM_MAX_DEG": 35.0,
"RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
})
# Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
self.load_mission("catapult.txt", strict=True)
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
# Throw the catapult.
self.set_servo(7, 2000)
# Wait until we're midway through the climb.
test_alt = 50
self.wait_altitude(test_alt, test_alt+2, relative=True)
# Ensure that by then the aircraft still goes at max allowed throttle.
self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX"))
# Wait for landing waypoint.
self.wait_current_waypoint(11, timeout=1200)
self.wait_disarmed(120)
def TakeoffAuto3(self):
'''Test the behaviour of an AUTO takeoff, pt3.
Conditions:
- ARSPD_USE=1
- TKOFF_MODE=1
'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"ARSPD_USE": 1.0,
"THR_MAX": 80.0,
"THR_MIN": 0.0,
"TKOFF_MODE": 1.0,
"TKOFF_THR_MAX": 100.0,
"TKOFF_THR_MINACC": 3.0,
"TECS_PITCH_MAX": 35.0,
"TKOFF_THR_MAX_T": 3.0,
"PTCH_LIM_MAX_DEG": 35.0,
"RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
})
# Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
self.load_mission("catapult.txt", strict=True)
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
# Throw the catapult.
self.set_servo(7, 2000)
# Ensure that TKOFF_THR_MAX_T is respected.
self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")-1))
# Ensure that after that the aircraft does not go full throttle anymore.
test_alt = 50
self.wait_altitude(test_alt, test_alt+2, relative=True)
self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")-10, operator.lt)
# Wait for landing waypoint.
self.wait_current_waypoint(11, timeout=1200)
self.wait_disarmed(120)
def TakeoffAuto4(self):
'''Test the behaviour of an AUTO takeoff, pt4.
Conditions:
- ARSPD_USE=0
- TKOFF_MODE=1
'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"ARSPD_USE": 0.0,
"THR_MAX": 80.0,
"THR_MIN": 0.0,
"TKOFF_MODE": 1.0,
"TKOFF_THR_MAX": 100.0,
"TKOFF_THR_MINACC": 3.0,
"TECS_PITCH_MAX": 35.0,
"TKOFF_THR_MAX_T": 3.0,
"PTCH_LIM_MAX_DEG": 35.0,
"RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
})
# Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m.
self.load_mission("catapult.txt", strict=True)
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
# Throw the catapult.
self.set_servo(7, 2000)
# Ensure that TKOFF_THR_MAX_T is respected.
self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge)
# Ensure that after that the aircraft still goes to maximum throttle.
test_alt = 50
self.wait_altitude(test_alt, test_alt+2, relative=True)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge)
# Wait for landing waypoint.
self.wait_current_waypoint(11, timeout=1200)
self.wait_disarmed(120)
def TakeoffTakeoff1(self):
'''Test the behaviour of a takeoff in TAKEOFF mode, pt1.
Conditions:
- ARSPD_USE=1
- TKOFF_MODE=0
- TKOFF_THR_MAX < THR_MAX
'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"ARSPD_USE": 1.0,
"THR_MAX": 100.0,
"TKOFF_LVL_ALT": 30.0,
"TKOFF_ALT": 100.0,
"TKOFF_MODE": 0.0,
"TKOFF_THR_MINACC": 3.0,
"TKOFF_THR_MAX": 80.0,
"TECS_PITCH_MAX": 35.0,
"PTCH_LIM_MAX_DEG": 35.0,
})
self.change_mode("TAKEOFF")
self.wait_ready_to_arm()
self.arm_vehicle()
# Throw the catapult.
self.set_servo(7, 2000)
# Check whether we're at max throttle below TKOFF_LVL_ALT.
test_alt = self.get_parameter("TKOFF_LVL_ALT")-10
self.wait_altitude(test_alt, test_alt+2, relative=True)
self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX"))
# Check whether we're still at max throttle past TKOFF_LVL_ALT.
test_alt = self.get_parameter("TKOFF_LVL_ALT")+10
self.wait_altitude(test_alt, test_alt+2, relative=True)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge)
# Wait for the takeoff to complete.
target_alt = self.get_parameter("TKOFF_ALT")
self.wait_altitude(target_alt-5, target_alt, relative=True)
self.fly_home_land_and_disarm()
def TakeoffTakeoff2(self):
'''Test the behaviour of a takeoff in TAKEOFF mode, pt2.
Conditions:
- ARSPD_USE=1
- TKOFF_MODE=1
- TKOFF_THR_MAX < THR_MAX
'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"ARSPD_USE": 1.0,
"THR_MAX": 100.0,
"TKOFF_LVL_ALT": 80.0,
"TKOFF_ALT": 150.0,
"TKOFF_MODE": 1.0,
"TKOFF_THR_MINACC": 3.0,
"TKOFF_THR_MAX": 80.0,
"TECS_PITCH_MAX": 35.0,
"PTCH_LIM_MAX_DEG": 35.0,
})
self.change_mode("TAKEOFF")
self.wait_ready_to_arm()
self.arm_vehicle()
# Throw the catapult.
self.set_servo(7, 2000)
# Check whether we're at max throttle below TKOFF_LVL_ALT.
test_alt = self.get_parameter("TKOFF_LVL_ALT")-10
self.wait_altitude(test_alt, test_alt+2, relative=True)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge)
# Check whether we've receded from max throttle past TKOFF_LVL_ALT.
test_alt = self.get_parameter("TKOFF_LVL_ALT")+10
self.wait_altitude(test_alt, test_alt+2, relative=True)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MIN"))-1, operator.ge)
# Wait for the takeoff to complete.
target_alt = self.get_parameter("TKOFF_ALT")
self.wait_altitude(target_alt-5, target_alt, relative=True)
self.fly_home_land_and_disarm()
def TakeoffTakeoff3(self):
'''Test the behaviour of a takeoff in TAKEOFF mode, pt3.
This is the same as case #1, but with disabled airspeed sensor.
Conditions:
- ARSPD_USE=0
- TKOFF_MODE=0
- TKOFF_THR_MAX < THR_MAX
'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"ARSPD_USE": 0.0,
"THR_MAX": 100.0,
"TKOFF_LVL_ALT": 30.0,
"TKOFF_ALT": 100.0,
"TKOFF_MODE": 0.0,
"TKOFF_THR_MINACC": 3.0,
"TKOFF_THR_MAX": 80.0,
"TECS_PITCH_MAX": 35.0,
"PTCH_LIM_MAX_DEG": 35.0,
})
self.change_mode("TAKEOFF")
self.wait_ready_to_arm()
self.arm_vehicle()
# Throw the catapult.
self.set_servo(7, 2000)
# Check whether we're at max throttle below TKOFF_LVL_ALT.
test_alt = self.get_parameter("TKOFF_LVL_ALT")-10
self.wait_altitude(test_alt, test_alt+2, relative=True)
self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX"))
# Check whether we're still at max throttle past TKOFF_LVL_ALT.
test_alt = self.get_parameter("TKOFF_LVL_ALT")+10
self.wait_altitude(test_alt, test_alt+2, relative=True)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le)
self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge)
# Wait for the takeoff to complete.
target_alt = self.get_parameter("TKOFF_ALT")
self.wait_altitude(target_alt-5, target_alt, relative=True)
self.fly_home_land_and_disarm()
def DCMFallback(self):
'''Really annoy the EKF and force fallback'''
self.reboot_sitl()
@ -5712,6 +6055,13 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.AHRS_ORIENTATION,
self.AHRSTrim,
self.LandingDrift,
self.TakeoffAuto1,
self.TakeoffAuto2,
self.TakeoffAuto3,
self.TakeoffAuto4,
self.TakeoffTakeoff1,
self.TakeoffTakeoff2,
self.TakeoffTakeoff3,
self.ForcedDCM,
self.DCMFallback,
self.MAVFTP,

4
Tools/autotest/vehicle_test_suite.py
View File

@ -5571,6 +5571,10 @@ class TestSuite(ABC):
"""Setup a simulated RC control to a PWM value"""
self.set_rc_from_map({chan: pwm}, timeout=timeout)
def set_servo(self, chan, pwm):
"""Replicate the functionality of MAVProxy: servo set <ch> <pwm>"""
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=chan, p2=pwm)
def location_offset_ne(self, location, north, east):
'''move location in metres'''
print("old: %f %f" % (location.lat, location.lng))