diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp index dd824505af..a19fe780d8 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp @@ -2,20 +2,25 @@ #include -MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) +/* + public function to format mission item as mavlink_mission_item_int_t + */ +bool MissionItemProtocol_Fence::get_item_as_mission_item(uint16_t seq, + mavlink_mission_item_int_t &ret_packet) { - const uint8_t num_stored_items = _fence.polyfence().num_stored_items(); - if (packet.seq > num_stored_items) { - return MAV_MISSION_INVALID_SEQUENCE; + AC_Fence *fence = AP::fence(); + if (fence == nullptr) { + return false; + } + const uint8_t num_stored_items = fence->polyfence().num_stored_items(); + if (seq > num_stored_items) { + return false; } AC_PolyFenceItem fenceitem; - if (!_fence.polyfence().get_item(packet.seq, fenceitem)) { - return MAV_MISSION_ERROR; + if (!fence->polyfence().get_item(seq, fenceitem)) { + return false; } MAV_CMD ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION; // initialised to avoid compiler warning @@ -41,7 +46,7 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link, p1 = fenceitem.radius; break; case AC_PolyFenceType::END_OF_STORAGE: - return MAV_MISSION_ERROR; + return false; } ret_packet.command = ret_cmd; @@ -50,6 +55,23 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link, ret_packet.y = fenceitem.loc.y; ret_packet.z = 0; + return true; +} + +MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const mavlink_mission_request_int_t &packet, + mavlink_mission_item_int_t &ret_packet) +{ + const uint8_t num_stored_items = _fence.polyfence().num_stored_items(); + if (packet.seq > num_stored_items) { + return MAV_MISSION_INVALID_SEQUENCE; + } + + if (!get_item_as_mission_item(packet.seq, ret_packet)) { + return MAV_MISSION_ERROR; + } + return MAV_MISSION_ACCEPTED; } diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h index 91ad93dbab..56cadc02eb 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h @@ -17,6 +17,11 @@ public: MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override; void timeout() override; + /* + static function to format mission item as mavlink_mission_item_int_t + */ + static bool get_item_as_mission_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet); + protected: ap_message next_item_ap_message_id() const override { diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp index a93383aaef..d2fbdd4c69 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp @@ -73,15 +73,18 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::convert_MISSION_ITEM_INT_to_RallyL return MAV_MISSION_ACCEPTED; } -MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) +/* + static function to get rally item as mavlink_mission_item_int_t + */ +bool MissionItemProtocol_Rally::get_item_as_mission_item(uint16_t seq, + mavlink_mission_item_int_t &ret_packet) { + auto *rallyp = AP::rally(); + RallyLocation rallypoint; - if (!rally.get_rally_point_with_index(packet.seq, rallypoint)) { - return MAV_MISSION_INVALID_SEQUENCE; + if (rallyp == nullptr || !rallyp->get_rally_point_with_index(seq, rallypoint)) { + return false; } ret_packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; @@ -90,6 +93,17 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link, ret_packet.y = rallypoint.lng; ret_packet.z = rallypoint.alt; + return true; +} + +MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link, + const mavlink_message_t &msg, + const mavlink_mission_request_int_t &packet, + mavlink_mission_item_int_t &ret_packet) +{ + if (!get_item_as_mission_item(packet.seq, ret_packet)) { + return MAV_MISSION_INVALID_SEQUENCE; + } return MAV_MISSION_ACCEPTED; } diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h index 379bcdcebc..cb8fff6ef8 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h @@ -12,6 +12,11 @@ public: MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override; void timeout() override; + /* + static function to get rally item as mavlink_mission_item_int_t + */ + static bool get_item_as_mission_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet); + protected: ap_message next_item_ap_message_id() const override {