From 737e18dccb8d065c536e4384ee967ad90c422330 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Dec 2016 17:07:34 +0100 Subject: [PATCH] MAVLink app: Fix VTOL reporting and prevent mission reached spam The VTOL status reporting and the mission status reporting were both suboptimal. VTOL was too slow, mission reporting too fast --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/mavlink/mavlink_mission.cpp | 6 +++++- src/modules/mavlink/mavlink_mission.h | 1 + 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8cd24bb7f5..f4473bbcf6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2011,8 +2011,8 @@ Mavlink::task_main(int argc, char *argv[]) break; case MAVLINK_MODE_ONBOARD: - configure_stream("SYS_STATUS", 1.0f); - configure_stream("EXTENDED_SYS_STATE", 2.0f); + configure_stream("SYS_STATUS", 5.0f); + configure_stream("EXTENDED_SYS_STATE", 5.0f); configure_stream("HIGHRES_IMU", 50.0f); configure_stream("ATTITUDE", 250.0f); configure_stream("RC_CHANNELS", 20.0f); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 985c659cea..e7147f70f7 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -70,6 +70,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _state(MAVLINK_WPM_STATE_IDLE), _time_last_recv(0), _time_last_sent(0), + _time_last_reached(0), _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), _int_mode(true), @@ -358,6 +359,7 @@ MavlinkMissionManager::send(const hrt_abstime now) if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } if (mission_result.reached) { + _time_last_reached = now; _last_reached = mission_result.seq_reached; send_mission_item_reached((uint16_t)mission_result.seq_reached); } else { @@ -375,7 +377,9 @@ MavlinkMissionManager::send(const hrt_abstime now) } else { if (_slow_rate_limiter.check(now)) { send_mission_current(_current_seq); - if (_last_reached >= 0) { + + // send the reached message a couple of times after reaching the waypoint + if (_last_reached >= 0 && (now - _time_last_reached) < 300 * 1000) { send_mission_item_reached((uint16_t)_last_reached); } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 33d393377b..ad9c9ebc6e 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -105,6 +105,7 @@ private: uint64_t _time_last_recv; uint64_t _time_last_sent; + uint64_t _time_last_reached; ///< last time when the vehicle reached a waypoint uint32_t _action_timeout; uint32_t _retry_timeout;