From a985b37c80ebf3827ba14dd518c7be959bc0bd42 Mon Sep 17 00:00:00 2001 From: oberion Date: Wed, 5 Aug 2015 16:14:17 +0200 Subject: [PATCH] mavlink/mavlink_mission: Dirty fix for setting waypoints over multiple mavlink link --- .gitmodules | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 + src/modules/mavlink/mavlink_mission.cpp | 58 +++++++++++++++++++------ src/modules/mavlink/mavlink_mission.h | 12 +++-- 4 files changed, 57 insertions(+), 17 deletions(-) diff --git a/.gitmodules b/.gitmodules index b05de95995..3e6e75cb0e 100644 --- a/.gitmodules +++ b/.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"] diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8901dd0672..dfc1afe91c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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(¶m_time, nullptr)) { /* parameters updated */ diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index bc0b269ea1..2086edbb18 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/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); + } +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index a7bb94c22d..54af7dc466 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/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;