diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index dc6a3cf907..737e3e069d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -14,6 +14,8 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ +#include "GCS.h" + #include #include #include @@ -36,8 +38,6 @@ #include #include -#include "GCS.h" - #include #if HAL_RCINPUT_WITH_AP_RADIO diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.cpp b/libraries/GCS_MAVLink/MissionItemProtocol.cpp index ea2bd8fe1f..52ee0a5a00 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol.cpp @@ -74,8 +74,8 @@ void MissionItemProtocol::handle_mission_count( if (packet.count == 0) { // no requests to send... - send_mission_ack(_link, msg, MAV_MISSION_ACCEPTED); - complete(_link); + const MAV_MISSION_RESULT result = complete(_link); + send_mission_ack(_link, msg, result); return; } @@ -233,6 +233,8 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons } if (result != MAV_MISSION_ACCEPTED) { send_mission_ack(msg, result); + receiving = false; + link = nullptr; return; } @@ -241,8 +243,8 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons request_i++; if (request_i > request_last) { - send_mission_ack(msg, MAV_MISSION_ACCEPTED); - complete(*link); + const MAV_MISSION_RESULT complete_result = complete(*link); + send_mission_ack(msg, complete_result); receiving = false; link = nullptr; return; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.h b/libraries/GCS_MAVLink/MissionItemProtocol.h index a7e6480bda..8a3e7b1455 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol.h @@ -106,7 +106,9 @@ private: virtual MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t &mission_item_int) = 0; virtual MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &mission_item_int) = 0; - virtual void complete(const GCS_MAVLINK &_link) {}; + virtual MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) { + return MAV_MISSION_ACCEPTED; + }; virtual void timeout() {}; bool mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp index b1f642f80a..b9e91b1941 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp @@ -36,10 +36,11 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_ return MAV_MISSION_ACCEPTED; } -void MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link) +MAV_MISSION_RESULT MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link) { _link.send_text(MAV_SEVERITY_INFO, "Rally points received"); AP::logger().Write_Rally(); + return MAV_MISSION_ACCEPTED; } bool MissionItemProtocol_Rally::clear_all_items() diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h index f1d5de514b..379bcdcebc 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h @@ -9,7 +9,7 @@ public: void truncate(const mavlink_mission_count_t &packet) override; MAV_MISSION_TYPE mission_type() const override { return MAV_MISSION_TYPE_RALLY; } - void complete(const GCS_MAVLINK &_link) override; + MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override; void timeout() override; protected: diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp index 72999d202b..b69ac5ead3 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp @@ -50,10 +50,11 @@ bool MissionItemProtocol_Waypoints::clear_all_items() return mission.clear(); } -void MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link) +MAV_MISSION_RESULT MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link) { _link.send_text(MAV_SEVERITY_INFO, "Flight plan received"); AP::logger().Write_EntireMission(); + return MAV_MISSION_ACCEPTED; } MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_link, diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h index f30c9cfcdf..5a7ee4e86e 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h @@ -16,7 +16,7 @@ public: // complete() is called by the base class after all waypoints have // been received. _link is the link which the last item was // transfered on. - void complete(const GCS_MAVLINK &_link) override; + MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override; // timeout() is called by the base class in the case that the GCS // does not transfer all waypoints to the vehicle. void timeout() override;