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

@@ -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);

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);
 			}
 		}

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;