diff --git a/integrationtests/run_tests.bash b/integrationtests/run_tests.bash index 76793f7fd77f..1bfb85eb67bd 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 000000000000..68ecf90ebecc --- /dev/null +++ b/launch/mavros_posix_tests_iris_opt_flow.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 315ec8a4145c..450f0f28c491 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3803,6 +3803,100 @@ class MavlinkStreamHighLatency : public MavlinkStream } }; + +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 };