navigator/mavlink: fixes for mission item reached message (#5486)

* navigator/mavlink: always send last reached item

Since we can't rely on mavlink that every message arrives, it makes
sense to continuously send the last reached waypoint.

* navigator: don't report reached for takeoff

If a takeoff waypoint has been inserted, we should not report that we
reached a mavlink mission item because we actually have not.

src/modules/mavlink/mavlink_mission.cpp

@@ -63,6 +63,7 @@ int MavlinkMissionManager::_dataman_id = 0;
 bool MavlinkMissionManager::_dataman_init = false;
 unsigned MavlinkMissionManager::_count = 0;
 int MavlinkMissionManager::_current_seq = 0;
+int MavlinkMissionManager::_last_reached = -1;
 bool MavlinkMissionManager::_transfer_in_progress = false;
 
 #define CHECK_SYSID_COMPID_MISSION(_msg)		(_msg.target_system == mavlink_system.sysid && \
@@ -327,7 +328,10 @@ MavlinkMissionManager::send(const hrt_abstime now)
 		if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); }
 
 		if (mission_result.reached) {
+			_last_reached = mission_result.seq_reached;
 			send_mission_item_reached((uint16_t)mission_result.seq_reached);
+		} else {
+			_last_reached = -1;
 		}
 
 		send_mission_current(_current_seq);
@@ -341,6 +345,9 @@ MavlinkMissionManager::send(const hrt_abstime now)
 	} else {
 		if (_slow_rate_limiter.check(now)) {
 			send_mission_current(_current_seq);
+			if (_last_reached >= 0) {
+				send_mission_item_reached((uint16_t)_last_reached);
+			}
 		}
 	}
 

src/modules/mavlink/mavlink_mission.h

@@ -118,6 +118,8 @@ private:
 	static unsigned		_count;					///< Count of items in active mission
 	static int		_current_seq;				///< Current item sequence in active mission
 
+	static int		_last_reached;				///< Last reached waypoint in active mission (-1 means nothing reached)
+
 	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
 	unsigned		_transfer_count;			///< Items count in current transmission
 	unsigned		_transfer_seq;				///< Item sequence in current transmission

src/modules/navigator/mission.cpp

@@ -204,7 +204,12 @@ Mission::on_active()
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
-		set_mission_item_reached();
+
+		/* If we just completed a takeoff which was inserted before the right waypoint,
+		   there is no need to report that we reached it because we didn't. */
+		if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
+			set_mission_item_reached();
+		}
 
 		if (_mission_item.autocontinue) {
 			/* switch to next waypoint if 'autocontinue' flag set */
@@ -266,6 +271,11 @@ Mission::update_onboard_mission()
 		_navigator->get_mission_result()->valid = true;
 		/* reset mission failure if we have an updated valid mission */
 		_navigator->get_mission_result()->mission_failure = false;
+
+		/* reset reached info as well */
+		_navigator->get_mission_result()->reached = false;
+		_navigator->get_mission_result()->seq_reached = 0;
+
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
 
@@ -310,6 +320,10 @@ Mission::update_offboard_mission()
 		if (!failed) {
 			/* reset mission failure if we have an updated valid mission */
 			_navigator->get_mission_result()->mission_failure = false;
+
+			/* reset reached info as well */
+			_navigator->get_mission_result()->reached = false;
+			_navigator->get_mission_result()->seq_reached = 0;
 		}
 
 	} else {
@@ -1206,7 +1220,6 @@ Mission::set_mission_item_reached()
 void
 Mission::set_current_offboard_mission_item()
 {
-	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
 	_navigator->set_mission_result_updated();

src/modules/navigator/navigator_main.cpp

@@ -878,7 +878,6 @@ Navigator::publish_mission_result()
 	}
 
 	/* reset some of the flags */
-	_mission_result.seq_reached = false;
 	_mission_result.seq_current = 0;
 	_mission_result.item_do_jump_changed = false;
 	_mission_result.item_changed_index = 0;