{"action":"created","issue":{"url":"https://api.github.com/repos/PX4/Firmware/issues/2641","labels_url":"https://api.github.com/repos/PX4/Firmware/issues/2641/labels{/name}","comments_url":"https://api.github.com/repos/PX4/Firmware/issues/2641/comments","events_url":"https://api.github.com/repos/PX4/Firmware/issues/2641/events","html_url":"https://github.com/PX4/Firmware/issues/2641","id":99231411,"number":2641,"title":"Sending Waypoints over different mavlink links will mess up system","user":{"login":"oberion","id":922449,"avatar_url":"https://avatars.githubusercontent.com/u/922449?v=3","gravatar_id":"","url":"https://api.github.com/users/oberion","html_url":"https://github.com/oberion","followers_url":"https://api.github.com/users/oberion/followers","following_url":"https://api.github.com/users/oberion/following{/other_user}","gists_url":"https://api.github.com/users/oberion/gists{/gist_id}","starred_url":"https://api.github.com/users/oberion/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/oberion/subscriptions","organizations_url":"https://api.github.com/users/oberion/orgs","repos_url":"https://api.github.com/users/oberion/repos","events_url":"https://api.github.com/users/oberion/events{/privacy}","received_events_url":"https://api.github.com/users/oberion/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"milestone":null,"comments":1,"created_at":"2015-08-05T15:28:02Z","updated_at":"2015-08-06T08:10:42Z","closed_at":null,"body":"The px4 code allows for multiple mavlink link. Setting some waypoints over one mavlink link works fine, even the navigator has no problem with it. However, requesting the current waypoints over another mavlink link will fail. More crucially, sending new waypoints over the other mavlink link will write on the active mission list.\r\nThe main issue is the _dataman_id variable in the MavlinkMissionManager class storing the active mission list. This variable is defined as an instance variable which is only synchronized in the constructor. If a mavlink link is updating the mission list, it will only update its instance variables. the other instances are not aware of the updated mission!\r\nA dirty hack trying to solve the issue is attached to the end. However i am not quite happy with it, thus i only attach a patchfile. Things that need to be changed:\r\n- All mavlink instances should be aware of the current active list.\r\n  * I have changed the _dataman_id variable from instance type to class type together with the _count and _current_seq variables.\r\n\r\n- Only one instance at a time should be able to write a new mission list.\r\n  * another class variable _transfer_in_progress checks for active transfers from other instance\r\n  * This is not really threadsafe, should be guarded.\r\n\r\n- The mavlink instances should be aware of mission changes.\r\n  * In the patch each instance is checking for _dataman_id changes\r\n  * There might be a more elegant solution using the uorb system \r\n\r\n- the mavlink instances should inform over their link that the mission has been changed.\r\n  * Here i see the biggest issue. At the moment the mavlink mission messages are not really defined in such a way, that the onboard system can update the receiver about mission changes see http://qgroundcontrol.org/mavlink/waypoint_protocol. Thus i will only send a waypoint_count message as soon as the mission is updated.\r\n   * The proper way would be to define a new mavlink message for telling the receiver that the onboard mission has changed and he shall update it. \r\n   * If the waypoint_count message is used, QGC needs to be adapted. Until now the state machine on QGC is programmed such that it will ignore the message when it has not send a mission request before! But it will already work for the mavros bridge.\r\n\r\nI think the patch might be a good basis to start fixing the issue. the patch should work and was already flight tested. Feel free to integrate the patch into the firmware or modify it.\r\n\r\n```\r\nFrom a985b37c80ebf3827ba14dd518c7be959bc0bd42 Mon Sep 17 00:00:00 2001\r\nFrom: oberion \r\nDate: Wed, 5 Aug 2015 16:14:17 +0200\r\nSubject: [PATCH] mavlink/mavlink_mission: Dirty fix for setting waypoints over\r\n multiple mavlink link\r\n\r\n---\r\n .gitmodules                             |  2 +-\r\n src/modules/mavlink/mavlink_main.cpp    |  2 ++\r\n src/modules/mavlink/mavlink_mission.cpp | 58 +++++++++++++++++++++++++--------\r\n src/modules/mavlink/mavlink_mission.h   | 12 +++++--\r\n 4 files changed, 57 insertions(+), 17 deletions(-)\r\n\r\ndiff --git a/.gitmodules b/.gitmodules\r\nindex b05de95..3e6e75c 100644\r\n--- a/.gitmodules\r\n+++ b/.gitmodules\r\n@@ -4,7 +4,7 @@\r\n [submodule \"NuttX\"]\r\n \tpath = NuttX\r\n \turl = git://github.com/PX4/NuttX.git\r\n-[submodule \"libuavcan\"]\r\n+[submodule \"src/lib/uavcan\"]\r\n \tpath = src/lib/uavcan\r\n \turl = git://github.com/UAVCAN/libuavcan.git\r\n [submodule \"Tools/genmsg\"]\r\ndiff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp\r\nindex 8901dd0..dfc1afe 100644\r\n--- a/src/modules/mavlink/mavlink_main.cpp\r\n+++ b/src/modules/mavlink/mavlink_main.cpp\r\n@@ -1710,6 +1710,8 @@ Mavlink::task_main(int argc, char *argv[])\r\n \t\thrt_abstime t = hrt_absolute_time();\r\n \r\n \t\tupdate_rate_mult();\r\n+\t\t\r\n+\t\t_mission_manager->check_active_mission();\r\n \r\n \t\tif (param_sub->update(&param_time, nullptr)) {\r\n \t\t\t/* parameters updated */\r\ndiff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp\r\nindex bc0b269..2086edb 100644\r\n--- a/src/modules/mavlink/mavlink_mission.cpp\r\n+++ b/src/modules/mavlink/mavlink_mission.cpp\r\n@@ -58,6 +58,12 @@\r\n #endif\r\n static const int ERROR = -1;\r\n \r\n+int MavlinkMissionManager::_dataman_id = 0;\r\n+bool MavlinkMissionManager::_dataman_init = false;\r\n+unsigned MavlinkMissionManager::_count = 0;\r\n+int MavlinkMissionManager::_current_seq = 0;\r\n+bool MavlinkMissionManager::_transfer_in_progress = false;\r\n+\r\n #define CHECK_SYSID_COMPID_MISSION(_msg)\t\t(_msg.target_system == mavlink_system.sysid && \\\r\n \t\t\t\t\t\t((_msg.target_component == mavlink_system.compid) || \\\r\n \t\t\t\t\t\t(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \\\r\n@@ -70,14 +76,12 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m\r\n \t_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),\r\n \t_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),\r\n \t_max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX),\r\n-\t_dataman_id(0),\r\n-\t_count(0),\r\n-\t_current_seq(0),\r\n+\t_my_dataman_id(0),\r\n \t_transfer_dataman_id(0),\r\n \t_transfer_count(0),\r\n \t_transfer_seq(0),\r\n \t_transfer_current_seq(0),\r\n-\t_transfer_partner_sysid(0),\r\n+\t_transfer_partner_sysid(255),\r\n \t_transfer_partner_compid(0),\r\n \t_offboard_mission_sub(-1),\r\n \t_mission_result_sub(-1),\r\n@@ -114,17 +118,21 @@ void\r\n MavlinkMissionManager::init_offboard_mission()\r\n {\r\n \tmission_s mission_state;\r\n-\tif (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {\r\n-\t\t_dataman_id = mission_state.dataman_id;\r\n-\t\t_count = mission_state.count;\r\n-\t\t_current_seq = mission_state.current_seq;\r\n+\tif (!_dataman_init) {\r\n+\t\t_dataman_init = true;\r\n+\t\tif (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {\r\n+\t\t\t_dataman_id = mission_state.dataman_id;\r\n+\t\t\t_count = mission_state.count;\r\n+\t\t\t_current_seq = mission_state.current_seq;\r\n \r\n-\t} else {\r\n-\t\t_dataman_id = 0;\r\n-\t\t_count = 0;\r\n-\t\t_current_seq = 0;\r\n-\t\twarnx(\"offboard mission init: ERROR\");\r\n+\t\t} else {\r\n+\t\t\t_dataman_id = 0;\r\n+\t\t\t_count = 0;\r\n+\t\t\t_current_seq = 0;\r\n+\t\t\twarnx(\"offboard mission init: ERROR\");\r\n+\t\t}\r\n \t}\r\n+\t_my_dataman_id = _dataman_id;\r\n }\r\n \r\n /**\r\n@@ -147,6 +155,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int\r\n \t\t_dataman_id = dataman_id;\r\n \t\t_count = count;\r\n \t\t_current_seq = seq;\r\n+\t\t_my_dataman_id = _dataman_id;\r\n \r\n \t\t/* mission state saved successfully, publish offboard_mission topic */\r\n \t\tif (_offboard_mission_pub == nullptr) {\r\n@@ -564,11 +573,19 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)\r\n \tif (CHECK_SYSID_COMPID_MISSION(wpc)) {\r\n \t\tif (_state == MAVLINK_WPM_STATE_IDLE) {\r\n \t\t\t_time_last_recv = hrt_absolute_time();\r\n+\t\t\t\r\n+\t\t\tif(_transfer_in_progress)\r\n+\t\t\t{\r\n+\t\t\t\tsend_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);\r\n+\t\t\t\treturn;\r\n+\t\t\t}\r\n+\t\t\t_transfer_in_progress = true;\r\n \r\n \t\t\tif (wpc.count > _max_count) {\r\n \t\t\t\tif (_verbose) { warnx(\"WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d\", wpc.count, _max_count); }\r\n \r\n \t\t\t\tsend_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE);\r\n+\t\t\t\t_transfer_in_progress = false;\r\n \t\t\t\treturn;\r\n \t\t\t}\r\n \r\n@@ -580,6 +597,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)\r\n \t\t\t\t_mavlink->send_statustext_info(\"WPM: COUNT 0: CLEAR MISSION\");\r\n \r\n \t\t\t\t// TODO send ACK?\r\n+\t\t\t\t_transfer_in_progress = false;\r\n \t\t\t\treturn;\r\n \t\t\t}\r\n \r\n@@ -661,6 +679,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)\r\n \r\n \t\t\tsend_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);\r\n \t\t\t_state = MAVLINK_WPM_STATE_IDLE;\r\n+\t\t\t_transfer_in_progress = false;\r\n \t\t\treturn;\r\n \t\t}\r\n \r\n@@ -672,6 +691,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)\r\n \t\t\tsend_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);\r\n \t\t\t_mavlink->send_statustext_critical(\"Unable to write on micro SD\");\r\n \t\t\t_state = MAVLINK_WPM_STATE_IDLE;\r\n+\t\t\t_transfer_in_progress = false;\r\n \t\t\treturn;\r\n \t\t}\r\n \r\n@@ -698,6 +718,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)\r\n \t\t\t} else {\r\n \t\t\t\tsend_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);\r\n \t\t\t}\r\n+\t\t\t_transfer_in_progress = false;\r\n \r\n \t\t} else {\r\n \t\t\t/* request next item */\r\n@@ -832,3 +853,14 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *\r\n \r\n \treturn OK;\r\n }\r\n+\r\n+\r\n+void MavlinkMissionManager::check_active_mission(void)\r\n+{\r\n+\tif(!(_my_dataman_id==_dataman_id))\r\n+\t{\r\n+\t\tif (_verbose) { warnx(\"WPM: New mission detected (possibly over different Mavlink instance) Updating\"); }\r\n+\t\t_my_dataman_id=_dataman_id;\r\n+\t\tthis->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count);\r\n+\t}\r\n+}\r\ndiff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h\r\nindex a7bb94c..54af7dc 100644\r\n--- a/src/modules/mavlink/mavlink_mission.h\r\n+++ b/src/modules/mavlink/mavlink_mission.h\r\n@@ -97,6 +97,8 @@ public:\r\n \r\n \tvoid set_verbose(bool v) { _verbose = v; }\r\n \r\n+\tvoid check_active_mission(void);\r\n+\r\n private:\r\n \tenum MAVLINK_WPM_STATES _state;\t\t\t\t\t///< Current state\r\n \r\n@@ -107,9 +109,12 @@ private:\r\n \tuint32_t\t\t_retry_timeout;\r\n \tunsigned\t\t_max_count;\t\t\t\t///< Maximum number of mission items\r\n \r\n-\tint\t\t\t_dataman_id;\t\t\t\t///< Dataman storage ID for active mission\r\n-\tunsigned\t\t_count;\t\t\t\t\t///< Count of items in active mission\r\n-\tint\t\t\t_current_seq;\t\t\t\t///< Current item sequence in active mission\r\n+\tstatic int\t\t_dataman_id;\t\t\t\t///< Global Dataman storage ID for active mission\r\n+\tint\t\t\t\t_my_dataman_id;\t\t\t\t///< class Dataman storage ID\r\n+\tstatic bool\t\t_dataman_init;\t\t\t\t///< Dataman initialized\r\n+\t\r\n+\tstatic unsigned\t\t_count;\t\t\t\t\t///< Count of items in active mission\r\n+\tstatic int\t\t\t_current_seq;\t\t\t\t///< Current item sequence in active mission\r\n \r\n \tint\t\t\t_transfer_dataman_id;\t\t\t///< Dataman storage ID for current transmission\r\n \tunsigned\t\t_transfer_count;\t\t\t///< Items count in current transmission\r\n@@ -117,6 +122,7 @@ private:\r\n \tunsigned\t\t_transfer_current_seq;\t\t\t///< Current item ID for current transmission (-1 means not initialized)\r\n \tunsigned\t\t_transfer_partner_sysid;\t\t///< Partner system ID for current transmission\r\n \tunsigned\t\t_transfer_partner_compid;\t\t///< Partner component ID for current transmission\r\n+\tstatic bool _transfer_in_progress;\t\t\t\t///< Global variable checking for current transmission\r\n \r\n \tint\t\t\t_offboard_mission_sub;\r\n \tint\t\t\t_mission_result_sub;\r\n-- \r\n1.9.5.msysgit.1\r\n\r\n\r\n```\r\n"},"comment":{"url":"https://api.github.com/repos/PX4/Firmware/issues/comments/128285852","html_url":"https://github.com/PX4/Firmware/issues/2641#issuecomment-128285852","issue_url":"https://api.github.com/repos/PX4/Firmware/issues/2641","id":128285852,"user":{"login":"LorenzMeier","id":1208119,"avatar_url":"https://avatars.githubusercontent.com/u/1208119?v=3","gravatar_id":"","url":"https://api.github.com/users/LorenzMeier","html_url":"https://github.com/LorenzMeier","followers_url":"https://api.github.com/users/LorenzMeier/followers","following_url":"https://api.github.com/users/LorenzMeier/following{/other_user}","gists_url":"https://api.github.com/users/LorenzMeier/gists{/gist_id}","starred_url":"https://api.github.com/users/LorenzMeier/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/LorenzMeier/subscriptions","organizations_url":"https://api.github.com/users/LorenzMeier/orgs","repos_url":"https://api.github.com/users/LorenzMeier/repos","events_url":"https://api.github.com/users/LorenzMeier/events{/privacy}","received_events_url":"https://api.github.com/users/LorenzMeier/received_events","type":"User","site_admin":false},"created_at":"2015-08-06T08:10:42Z","updated_at":"2015-08-06T08:10:42Z","body":"I would appreciate a pull request for this with the flight tested version. I think this is perfectly fine as a stopgap measure."}}