diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash
index 76793f7fd77fee0737c0cc69aa24d4c88ed15f24..1bfb85eb67bd9b922680fd513806e870f984fdd7 100755
--- a/integrationtests/run_tests.bash
+++ b/integrationtests/run_tests.bash
@@ -8,13 +8,32 @@ set -e
# TODO move to docker image
pip install px4tools pymavlink -q
-# handle cleaning command
+# A POSIX variable
+OPTIND=1 # Reset in case getopts has been used previously in the shell.
+
+# Initialize our own variables:
do_clean=true
-if [ "$1" = "-o" ]
-then
- echo not cleaning
- do_clean=false
-fi
+gui=false
+
+while getopts "h?og" opt; do
+ case "$opt" in
+ h|\?)
+ echo """
+ $0 [-h] [-o] [-g]
+ -h show help
+ -o don't clean before building (to save time)
+ -g run gazebo gui
+ """
+ exit 0
+ ;;
+ o) do_clean=false
+ echo "not cleaning"
+ ;;
+ g) gui=true
+ ;;
+ esac
+done
+
# determine the directory of the source given the directory of this script
pushd `dirname $0` > /dev/null
@@ -89,8 +108,13 @@ echo -e "TEST_RESULT_TARGET_DIR\t: $TEST_RESULT_TARGET_DIR"
# however, stop executing tests after the first failure
set +e
echo "=====> run tests"
-test $? -eq 0 && rostest px4 mavros_posix_tests_iris.launch
-test $? -eq 0 && rostest px4 mavros_posix_tests_standard_vtol.launch
+test $? -eq 0 && rostest px4 mavros_posix_tests_iris.launch gui:=$gui
+
+# commented out optical flow test for now since ci server has
+# an issue producing the simulated flow camera currently
+#test $? -eq 0 && rostest px4 mavros_posix_tests_iris_opt_flow.launch gui:=$gui
+
+test $? -eq 0 && rostest px4 mavros_posix_tests_standard_vtol.launch gui:=$gui
TEST_RESULT=$?
echo "<====="
diff --git a/launch/mavros_posix_tests_iris_opt_flow.launch b/launch/mavros_posix_tests_iris_opt_flow.launch
new file mode 100644
index 0000000000000000000000000000000000000000..68ecf90ebeccb2487869f48dce193eae6b8b4988
--- /dev/null
+++ b/launch/mavros_posix_tests_iris_opt_flow.launch
@@ -0,0 +1,23 @@
+<launch>
+ <!-- Posix SITL MAVROS integration tests -->
+
+ <arg name="ns" default="/"/>
+ <arg name="headless" default="true"/>
+ <arg name="gui" default="false"/>
+ <arg name="est" default="lpe"/>
+
+ <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="vehicle" value="iris_opt_flow"/>
+ <arg name="est" value="$(arg est)"/>
+ </include>
+
+ <group ns="$(arg ns)">
+ <test test-name="mavros_flow_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0" />
+ <test test-name="mavros_flow_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />
+ </group>
+</launch>
+
+<!-- vim: set ft=xml et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 315ec8a4145ceedf6d9fc25c9bdcd75525ec7cf2..450f0f28c4915b7cbb8a00f78833c837349b65d3 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -3803,6 +3803,100 @@ protected:
}
};
+
+class MavlinkStreamGroundTruth : public MavlinkStream
+{
+public:
+ const char *get_name() const
+ {
+ return MavlinkStreamGroundTruth::get_name_static();
+ }
+
+ static const char *get_name_static()
+ {
+ return "GROUND_TRUTH";
+ }
+
+ static uint16_t get_id_static()
+ {
+ return MAVLINK_MSG_ID_HIL_STATE_QUATERNION;
+ }
+
+ uint16_t get_id()
+ {
+ return get_id_static();
+ }
+
+ static MavlinkStream *new_instance(Mavlink *mavlink)
+ {
+ return new MavlinkStreamGroundTruth(mavlink);
+ }
+
+ unsigned get_size()
+ {
+ return (_att_time > 0 || _gpos_time > 0) ? MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN +
+ MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+ }
+
+private:
+ MavlinkOrbSubscription *_att_sub;
+ MavlinkOrbSubscription *_gpos_sub;
+ uint64_t _att_time;
+ uint64_t _gpos_time;
+ struct vehicle_attitude_s _att;
+ struct vehicle_global_position_s _gpos;
+
+ /* do not allow top copying this class */
+ MavlinkStreamGroundTruth(MavlinkStreamGroundTruth &);
+ MavlinkStreamGroundTruth &operator = (const MavlinkStreamGroundTruth &);
+
+protected:
+ explicit MavlinkStreamGroundTruth(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_groundtruth))),
+ _gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position_groundtruth))),
+ _att_time(0),
+ _gpos_time(0),
+ _att(),
+ _gpos()
+ {}
+
+ void send(const hrt_abstime t)
+ {
+ bool att_updated = _att_sub->update(&_att_time, &_att);
+ bool gpos_updated = _gpos_sub->update(&_gpos_time, &_gpos);
+
+ if (att_updated || gpos_updated) {
+
+ mavlink_hil_state_quaternion_t msg = {};
+
+ if (att_updated) {
+ msg.attitude_quaternion[0] = _att.q[0];
+ msg.attitude_quaternion[1] = _att.q[1];
+ msg.attitude_quaternion[2] = _att.q[2];
+ msg.attitude_quaternion[3] = _att.q[3];
+ msg.rollspeed = _att.rollspeed;
+ msg.pitchspeed = _att.pitchspeed;
+ msg.yawspeed = _att.yawspeed;
+ }
+
+ if (gpos_updated) {
+ msg.lat = _gpos.lat;
+ msg.lon = _gpos.lon;
+ msg.alt = _gpos.alt;
+ msg.vx = _gpos.vel_n;
+ msg.vy = _gpos.vel_e;
+ msg.vz = _gpos.vel_d;
+ msg.ind_airspeed = 0;
+ msg.true_airspeed = 0;
+ msg.xacc = 0;
+ msg.yacc = 0;
+ msg.zacc = 0;
+ }
+ mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg);
+ }
+ }
+};
+
const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
@@ -3850,5 +3944,6 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
new StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static),
new StreamListItem(&MavlinkStreamHighLatency::new_instance, &MavlinkStreamHighLatency::get_name_static, &MavlinkStreamWind::get_id_static),
+ new StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static),
nullptr
};