mavlink/mavlink_mission: Dirty fix for setting waypoints over multiple mavlink link

.gitmodules

@@ -4,7 +4,7 @@
 [submodule "NuttX"]
 	path = NuttX
 	url = git://github.com/PX4/NuttX.git
-[submodule "libuavcan"]
+[submodule "src/lib/uavcan"]
 	path = src/lib/uavcan
 	url = git://github.com/UAVCAN/libuavcan.git
 [submodule "Tools/genmsg"]

src/modules/mavlink/mavlink_main.cpp

@@ -1710,6 +1710,8 @@ Mavlink::task_main(int argc, char *argv[])
 		hrt_abstime t = hrt_absolute_time();
 
 		update_rate_mult();
+		
+		_mission_manager->check_active_mission();
 
 		if (param_sub->update(&param_time, nullptr)) {
 			/* parameters updated */

src/modules/mavlink/mavlink_mission.cpp

@@ -58,6 +58,12 @@
 #endif
 static const int ERROR = -1;
 
+int MavlinkMissionManager::_dataman_id = 0;
+bool MavlinkMissionManager::_dataman_init = false;
+unsigned MavlinkMissionManager::_count = 0;
+int MavlinkMissionManager::_current_seq = 0;
+bool MavlinkMissionManager::_transfer_in_progress = false;
+
 #define CHECK_SYSID_COMPID_MISSION(_msg)		(_msg.target_system == mavlink_system.sysid && \
 						((_msg.target_component == mavlink_system.compid) || \
 						(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
@@ -70,14 +76,12 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
 	_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),
 	_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),
 	_max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX),
-	_dataman_id(0),
-	_count(0),
-	_current_seq(0),
+	_my_dataman_id(0),
 	_transfer_dataman_id(0),
 	_transfer_count(0),
 	_transfer_seq(0),
 	_transfer_current_seq(0),
-	_transfer_partner_sysid(0),
+	_transfer_partner_sysid(255),
 	_transfer_partner_compid(0),
 	_offboard_mission_sub(-1),
 	_mission_result_sub(-1),
@@ -114,17 +118,21 @@ void
 MavlinkMissionManager::init_offboard_mission()
 {
 	mission_s mission_state;
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
-		_dataman_id = mission_state.dataman_id;
-		_count = mission_state.count;
-		_current_seq = mission_state.current_seq;
+	if (!_dataman_init) {
+		_dataman_init = true;
+		if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
+			_dataman_id = mission_state.dataman_id;
+			_count = mission_state.count;
+			_current_seq = mission_state.current_seq;
 
-	} else {
-		_dataman_id = 0;
-		_count = 0;
-		_current_seq = 0;
-		warnx("offboard mission init: ERROR");
+		} else {
+			_dataman_id = 0;
+			_count = 0;
+			_current_seq = 0;
+			warnx("offboard mission init: ERROR");
+		}
 	}
+	_my_dataman_id = _dataman_id;
 }
 
 /**
@@ -147,6 +155,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
 		_dataman_id = dataman_id;
 		_count = count;
 		_current_seq = seq;
+		_my_dataman_id = _dataman_id;
 
 		/* mission state saved successfully, publish offboard_mission topic */
 		if (_offboard_mission_pub == nullptr) {
@@ -564,11 +573,19 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 	if (CHECK_SYSID_COMPID_MISSION(wpc)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			_time_last_recv = hrt_absolute_time();
+			
+			if(_transfer_in_progress)
+			{
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
+				return;
+			}
+			_transfer_in_progress = true;
 
 			if (wpc.count > _max_count) {
 				if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); }
 
 				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE);
+				_transfer_in_progress = false;
 				return;
 			}
 
@@ -580,6 +597,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 				_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
 
 				// TODO send ACK?
+				_transfer_in_progress = false;
 				return;
 			}
 
@@ -661,6 +679,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 
 			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
 			_state = MAVLINK_WPM_STATE_IDLE;
+			_transfer_in_progress = false;
 			return;
 		}
 
@@ -672,6 +691,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 			send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
 			_mavlink->send_statustext_critical("Unable to write on micro SD");
 			_state = MAVLINK_WPM_STATE_IDLE;
+			_transfer_in_progress = false;
 			return;
 		}
 
@@ -698,6 +718,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 			} else {
 				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
 			}
+			_transfer_in_progress = false;
 
 		} else {
 			/* request next item */
@@ -832,3 +853,14 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 	return OK;
 }
+
+
+void MavlinkMissionManager::check_active_mission(void)
+{
+	if(!(_my_dataman_id==_dataman_id))
+	{
+		if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); }
+		_my_dataman_id=_dataman_id;
+		this->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count);
+	}
+}

src/modules/mavlink/mavlink_mission.h

@@ -97,6 +97,8 @@ public:
 
 	void set_verbose(bool v) { _verbose = v; }
 
+	void check_active_mission(void);
+
 private:
 	enum MAVLINK_WPM_STATES _state;					///< Current state
 
@@ -107,9 +109,12 @@ private:
 	uint32_t		_retry_timeout;
 	unsigned		_max_count;				///< Maximum number of mission items
 
-	int			_dataman_id;				///< Dataman storage ID for active mission
-	unsigned		_count;					///< Count of items in active mission
-	int			_current_seq;				///< Current item sequence in active mission
+	static int		_dataman_id;				///< Global Dataman storage ID for active mission
+	int				_my_dataman_id;				///< class Dataman storage ID
+	static bool		_dataman_init;				///< Dataman initialized
+	
+	static unsigned		_count;					///< Count of items in active mission
+	static int			_current_seq;				///< Current item sequence in active mission
 
 	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
 	unsigned		_transfer_count;			///< Items count in current transmission
@@ -117,6 +122,7 @@ private:
 	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
 	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
 	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
+	static bool _transfer_in_progress;				///< Global variable checking for current transmission
 
 	int			_offboard_mission_sub;
 	int			_mission_result_sub;