mirror of https://github.com/ArduPilot/ardupilot
autotest: add FlyEachFrame test for Plane
This commit is contained in:
parent
4909f6f0f1
commit
d3f90bde7f
|
@ -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 84 0.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1
|
||||||
|
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1
|
||||||
|
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.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 85 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1
|
|
@ -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 30.000000 1
|
||||||
|
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1
|
||||||
|
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.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
|
|
@ -20,6 +20,7 @@ from common import AutoTestTimeoutException
|
||||||
from common import NotAchievedException
|
from common import NotAchievedException
|
||||||
from common import PreconditionFailedException
|
from common import PreconditionFailedException
|
||||||
from pymavlink.rotmat import Vector3
|
from pymavlink.rotmat import Vector3
|
||||||
|
from pysim import vehicleinfo
|
||||||
|
|
||||||
import operator
|
import operator
|
||||||
|
|
||||||
|
@ -522,7 +523,7 @@ class AutoTestPlane(AutoTest):
|
||||||
|
|
||||||
return self.wait_level_flight()
|
return self.wait_level_flight()
|
||||||
|
|
||||||
def fly_mission(self, filename, mission_timeout=60.0, strict=True):
|
def fly_mission(self, filename, mission_timeout=60.0, strict=True, quadplane=False):
|
||||||
"""Fly a mission from a file."""
|
"""Fly a mission from a file."""
|
||||||
self.progress("Flying mission %s" % filename)
|
self.progress("Flying mission %s" % filename)
|
||||||
num_wp = self.load_mission(filename, strict=strict)-1
|
num_wp = self.load_mission(filename, strict=strict)-1
|
||||||
|
@ -530,7 +531,10 @@ class AutoTestPlane(AutoTest):
|
||||||
self.change_mode('AUTO')
|
self.change_mode('AUTO')
|
||||||
self.wait_waypoint(1, num_wp, max_dist=60)
|
self.wait_waypoint(1, num_wp, max_dist=60)
|
||||||
self.wait_groundspeed(0, 0.5, timeout=mission_timeout)
|
self.wait_groundspeed(0, 0.5, timeout=mission_timeout)
|
||||||
self.wait_statustext("Auto disarmed", timeout=60)
|
if quadplane:
|
||||||
|
self.wait_statustext("Throttle disarmed", timeout=60)
|
||||||
|
else:
|
||||||
|
self.wait_statustext("Auto disarmed", timeout=60)
|
||||||
self.progress("Mission OK")
|
self.progress("Mission OK")
|
||||||
|
|
||||||
def fly_do_reposition(self):
|
def fly_do_reposition(self):
|
||||||
|
@ -2880,6 +2884,49 @@ class AutoTestPlane(AutoTest):
|
||||||
want_result=mavutil.mavlink.MAV_RESULT_DENIED
|
want_result=mavutil.mavlink.MAV_RESULT_DENIED
|
||||||
)
|
)
|
||||||
|
|
||||||
|
def fly_each_frame(self):
|
||||||
|
vinfo = vehicleinfo.VehicleInfo()
|
||||||
|
vinfo_options = vinfo.options[self.vehicleinfo_key()]
|
||||||
|
known_broken_frames = {
|
||||||
|
"firefly": "falls out of sky after transition",
|
||||||
|
"plane-tailsitter": "does not take off; immediately emits 'AP: Transition VTOL done' while on ground",
|
||||||
|
"quadplane-cl84": "falls out of sky instead of transitioning",
|
||||||
|
"quadplane-tilttri": "falls out of sky instead of transitioning",
|
||||||
|
"quadplane-tilttrivec": "loses attitude control and crashes",
|
||||||
|
}
|
||||||
|
for frame in sorted(vinfo_options["frames"].keys()):
|
||||||
|
self.start_subtest("Testing frame (%s)" % str(frame))
|
||||||
|
if frame in known_broken_frames:
|
||||||
|
self.progress("Actually, no I'm not - it is known-broken (%s)" %
|
||||||
|
(known_broken_frames[frame]))
|
||||||
|
continue
|
||||||
|
frame_bits = vinfo_options["frames"][frame]
|
||||||
|
print("frame_bits: %s" % str(frame_bits))
|
||||||
|
if frame_bits.get("external", False):
|
||||||
|
self.progress("Actually, no I'm not - it is an external simulation")
|
||||||
|
continue
|
||||||
|
model = frame_bits.get("model", frame)
|
||||||
|
# the model string for Callisto has crap in it.... we
|
||||||
|
# should really have another entry in the vehicleinfo data
|
||||||
|
# to carry the path to the JSON.
|
||||||
|
actual_model = model.split(":")[0]
|
||||||
|
defaults = self.model_defaults_filepath(actual_model)
|
||||||
|
if type(defaults) != list:
|
||||||
|
defaults = [defaults]
|
||||||
|
self.customise_SITL_commandline(
|
||||||
|
["--defaults", ','.join(defaults), ],
|
||||||
|
model=model,
|
||||||
|
wipe=True,
|
||||||
|
)
|
||||||
|
mission_file = "basic.txt"
|
||||||
|
quadplane = self.get_parameter('Q_ENABLE')
|
||||||
|
if quadplane:
|
||||||
|
mission_file = "basic-quadplane.txt"
|
||||||
|
self.wait_ready_to_arm()
|
||||||
|
self.arm_vehicle()
|
||||||
|
self.fly_mission(mission_file, strict=False, quadplane=quadplane)
|
||||||
|
self.wait_disarmed()
|
||||||
|
|
||||||
def tests(self):
|
def tests(self):
|
||||||
'''return list of all tests'''
|
'''return list of all tests'''
|
||||||
ret = super(AutoTestPlane, self).tests()
|
ret = super(AutoTestPlane, self).tests()
|
||||||
|
@ -3069,6 +3116,10 @@ class AutoTestPlane(AutoTest):
|
||||||
"Test smart battery logging etc",
|
"Test smart battery logging etc",
|
||||||
self.SmartBattery),
|
self.SmartBattery),
|
||||||
|
|
||||||
|
("FlyEachFrame",
|
||||||
|
"Fly each supported internal frame",
|
||||||
|
self.fly_each_frame),
|
||||||
|
|
||||||
("LogUpload",
|
("LogUpload",
|
||||||
"Log upload",
|
"Log upload",
|
||||||
self.log_upload),
|
self.log_upload),
|
||||||
|
|
|
@ -269,23 +269,31 @@ class VehicleInfo(object):
|
||||||
"gazebo-zephyr": {
|
"gazebo-zephyr": {
|
||||||
"waf_target": "bin/arduplane",
|
"waf_target": "bin/arduplane",
|
||||||
"default_params_filename": "default_params/gazebo-zephyr.parm",
|
"default_params_filename": "default_params/gazebo-zephyr.parm",
|
||||||
|
"external": True,
|
||||||
},
|
},
|
||||||
"last_letter": {
|
"last_letter": {
|
||||||
"waf_target": "bin/arduplane",
|
"waf_target": "bin/arduplane",
|
||||||
|
"default_params_filename": "default_params/plane.parm",
|
||||||
|
"external": True,
|
||||||
},
|
},
|
||||||
"CRRCSim": {
|
"CRRCSim": {
|
||||||
"waf_target": "bin/arduplane",
|
"waf_target": "bin/arduplane",
|
||||||
|
"default_params_filename": "default_params/plane.parm",
|
||||||
|
"external": True,
|
||||||
},
|
},
|
||||||
"jsbsim": {
|
"jsbsim": {
|
||||||
"waf_target": "bin/arduplane",
|
"waf_target": "bin/arduplane",
|
||||||
"default_params_filename": "default_params/plane-jsbsim.parm",
|
"default_params_filename": "default_params/plane-jsbsim.parm",
|
||||||
|
"external": True,
|
||||||
},
|
},
|
||||||
"scrimmage-plane" : {
|
"scrimmage-plane" : {
|
||||||
"waf_target": "bin/arduplane",
|
"waf_target": "bin/arduplane",
|
||||||
"default_params_filename": "default_params/plane.parm",
|
"default_params_filename": "default_params/plane.parm",
|
||||||
|
"external": True,
|
||||||
},
|
},
|
||||||
"calibration": {
|
"calibration": {
|
||||||
"extra_mavlink_cmds": "module load sitl_calibration;",
|
"extra_mavlink_cmds": "module load sitl_calibration;",
|
||||||
|
"external": True, # lies! OTOH, hard to take off with this
|
||||||
},
|
},
|
||||||
},
|
},
|
||||||
},
|
},
|
||||||
|
|
Loading…
Reference in New Issue