diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py
index 17124a65ca94bb2486f3a6311665024854a5ff0a..28b47163cd507dda8d3fb820b3049875fae36267 100755
--- a/integrationtests/python_src/px4_it/mavros/mission_test.py
+++ b/integrationtests/python_src/px4_it/mavros/mission_test.py
@@ -42,6 +42,7 @@ import rospy
import math
import rosbag
import sys
+import os
import mavros
from pymavlink import mavutil
@@ -49,7 +50,7 @@ from mavros import mavlink
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import CommandLong, WaypointPush
-from mavros_msgs.msg import Mavlink, Waypoint
+from mavros_msgs.msg import Mavlink, Waypoint, ExtendedState
from sensor_msgs.msg import NavSatFix
from mavros.mission import QGroundControlWP
#from px4_test_helper import PX4TestHelper
@@ -66,6 +67,7 @@ class MavrosMissionTest(unittest.TestCase):
rospy.Subscriber("mavros/local_position/pose", PoseStamped, self.position_callback)
rospy.Subscriber("mavros/global_position/global", NavSatFix, self.global_position_callback)
+ rospy.Subscriber("mavros/extended_state", ExtendedState, self.extended_state_callback)
self.pub_mavlink = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
rospy.wait_for_service('mavros/cmd/command', 30)
self._srv_cmd_long = rospy.ServiceProxy('mavros/cmd/command', CommandLong, persistent=True)
@@ -74,7 +76,10 @@ class MavrosMissionTest(unittest.TestCase):
self.has_global_pos = False
self.local_position = PoseStamped()
self.global_position = NavSatFix()
+ self.extended_state = ExtendedState()
self.home_alt = 0
+ self.mc_rad = 5
+ self.fw_rad = 40
# need to simulate heartbeat for datalink loss detection
rospy.Timer(rospy.Duration(0.5), self.send_heartbeat)
@@ -96,6 +101,9 @@ class MavrosMissionTest(unittest.TestCase):
self.home_alt = data.altitude
self.has_global_pos = True
+ def extended_state_callback(self, data):
+ self.extended_state = data
+
#
# Helper methods
#
@@ -115,13 +123,18 @@ class MavrosMissionTest(unittest.TestCase):
d = R * c
alt_d = abs(alt - self.global_position.altitude)
- rospy.loginfo("d: %f, alt_d: %f", d, alt_d)
+ #rospy.loginfo("d: %f, alt_d: %f", d, alt_d)
return d < offset and alt_d < offset
- def reach_position(self, lat, lon, alt, radius, timeout):
+ def reach_position(self, lat, lon, alt, timeout):
# does it reach the position in X seconds?
count = 0
while count < timeout:
+ # use different radius matching vehicle state
+ radius = self.mc_rad
+ if self.extended_state.vtol_state == ExtendedState.VTOL_STATE_FW:
+ radius = self.fw_rad
+
if self.is_at_position(lat, lon, alt, radius):
break
count = count + 1
@@ -153,13 +166,15 @@ class MavrosMissionTest(unittest.TestCase):
"""Test mission"""
if len(sys.argv) < 2:
- self.fail("no mission argument")
+ self.fail("usage: mission_test.py mission_file")
return
- rospy.loginfo("reading mission")
+ file = os.path.dirname(os.path.realpath(__file__)) + "/" + sys.argv[1]
+
+ rospy.loginfo("reading mission %s", file)
mission = QGroundControlWP()
wps = []
- for wp in mission.read(open(sys.argv[1], 'r')):
+ for wp in mission.read(open(file, 'r')):
wps.append(wp)
rospy.logdebug(wp)
@@ -187,7 +202,7 @@ class MavrosMissionTest(unittest.TestCase):
alt = wp.z_alt
if wp.frame == Waypoint.FRAME_GLOBAL_REL_ALT:
alt += self.home_alt
- self.reach_position(wp.x_lat, wp.y_long, alt, 10, 300)
+ self.reach_position(wp.x_lat, wp.y_long, alt, 600)
if __name__ == '__main__':
diff --git a/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt b/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6d61dbfc96150569a5fe193419013b89e54f8e6d
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/vtol_mis_1.txt
@@ -0,0 +1,9 @@
+QGC WPL 110
+0 1 0 16 0 0 0 0 47.3979949951 8.54559612274 12.0 1
+1 0 2 3000 4.0 0.0 0.0 0.0 47.3980331421 8.54578971863 12.0 1
+2 0 3 16 0.0 0.0 -0.0 0.0 47.399269104 8.54557228088 12.0 1
+3 0 3 16 0.0 0.0 -0.0 0.0 47.3992652893 8.54230213165 12.0 1
+4 0 3 16 0.0 0.0 -0.0 0.0 47.3974761963 8.54239082336 12.0 1
+5 0 3 16 0.0 0.0 -0.0 0.0 47.3976669312 8.54509449005 12.0 1
+6 0 2 3000 3.0 0.0 0.0 0.0 47.3977851868 8.54526233673 12.0 1
+7 0 3 21 25.0 0.0 -0.0 0.0 47.3979797363 8.54460906982 12.0 1
diff --git a/launch/mavros_posix_tests_standard_vtol.launch b/launch/mavros_posix_tests_standard_vtol.launch
new file mode 100644
index 0000000000000000000000000000000000000000..58b928995d88215161367dae3cd1aa080e041c17
--- /dev/null
+++ b/launch/mavros_posix_tests_standard_vtol.launch
@@ -0,0 +1,18 @@
+<launch>
+ <!-- Posix SITL MAVROS integration tests -->
+
+ <arg name="ns" default="/"/>
+ <arg name="headless" default="true"/>
+ <arg name="gui" default="false"/>
+
+ <include file="$(find px4)/launch/mavros_posix_sitl.launch">
+ <arg name="ns" value="$(arg ns)"/>
+ <arg name="headless" value="$(arg headless)"/>
+ <arg name="gui" value="$(arg gui)"/>
+ <arg name="world" value="standard_vtol"/>
+ </include>
+
+ <group ns="$(arg ns)">
+ <test test-name="mission_test" pkg="px4" type="mission_test.py" time-limit="300.0" args="vtol_mis_1.txt" />
+ </group>
+</launch>
diff --git a/posix-configs/SITL/init/rcS_gazebo_standard_vtol b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
index f43d6330b4b321cb6901a6feb1560dafa1a64c7f..44294fa2037d18c78983d4b077b7e004e9f23710 100644
--- a/posix-configs/SITL/init/rcS_gazebo_standard_vtol
+++ b/posix-configs/SITL/init/rcS_gazebo_standard_vtol
@@ -39,7 +39,15 @@ param set NAV_DLL_ACT 2
param set NAV_ACC_RAD 3.0
param set MPC_TKO_SPEED 1.0
param set MIS_YAW_TMT 10
-simulator start -s
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set COM_DISARM_LAND 5
+param set COM_DL_LOSS_EN 1
+param set MPC_ACC_HOR_MAX 2
+param set GF_ACTION 3
+param set GF_MAX_HOR_DIST 500
+param set GF_MAX_VER_DIST 500
rgbledsim start
tone_alarm start
gyrosim start