diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 24a26a057e..80ff8010b9 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -23,10 +23,12 @@ void GCS::get_sensor_status_flags(uint32_t &present, MissionItemProtocol_Waypoints *GCS::_missionitemprotocol_waypoints; MissionItemProtocol_Rally *GCS::_missionitemprotocol_rally; +MissionItemProtocol_Fence *GCS::_missionitemprotocol_fence; const MAV_MISSION_TYPE GCS_MAVLINK::supported_mission_types[] = { MAV_MISSION_TYPE_MISSION, MAV_MISSION_TYPE_RALLY, + MAV_MISSION_TYPE_FENCE, }; /* diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index d66daed9a5..fd091ddf43 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -18,6 +18,7 @@ #include "MissionItemProtocol_Waypoints.h" #include "MissionItemProtocol_Rally.h" +#include "MissionItemProtocol_Fence.h" #include "ap_message.h" #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0 @@ -85,7 +86,7 @@ public: mission_type); } - static const MAV_MISSION_TYPE supported_mission_types[2]; + static const MAV_MISSION_TYPE supported_mission_types[3]; // packetReceived is called on any successful decode of a mavlink message virtual void packetReceived(const mavlink_status_t &status, @@ -742,6 +743,7 @@ public: static MissionItemProtocol_Waypoints *_missionitemprotocol_waypoints; static MissionItemProtocol_Rally *_missionitemprotocol_rally; + static MissionItemProtocol_Fence *_missionitemprotocol_fence; MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const; void try_send_queued_message_for_type(MAV_MISSION_TYPE type); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 53f903faef..210fd01787 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -16,6 +16,7 @@ */ #include "GCS.h" +#include #include #include #include @@ -432,6 +433,8 @@ MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE missi return _missionitemprotocol_waypoints; case MAV_MISSION_TYPE_RALLY: return _missionitemprotocol_rally; + case MAV_MISSION_TYPE_FENCE: + return _missionitemprotocol_fence; default: return nullptr; } @@ -1858,6 +1861,10 @@ void GCS::update_send() if (rally != nullptr) { _missionitemprotocol_rally = new MissionItemProtocol_Rally(*rally); } + AC_Fence *fence = AP::fence(); + if (fence != nullptr) { + _missionitemprotocol_fence = new MissionItemProtocol_Fence(*fence); + } } if (_missionitemprotocol_waypoints != nullptr) { _missionitemprotocol_waypoints->update(); @@ -1865,6 +1872,9 @@ void GCS::update_send() if (_missionitemprotocol_rally != nullptr) { _missionitemprotocol_rally->update(); } + if (_missionitemprotocol_fence != nullptr) { + _missionitemprotocol_fence->update(); + } for (uint8_t i=0; iupdate_send(); } @@ -3910,6 +3920,11 @@ bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id) gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_RALLY); ret = true; break; + case MSG_NEXT_MISSION_REQUEST_FENCE: + CHECK_PAYLOAD_SIZE(MISSION_REQUEST); + gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_FENCE); + ret = true; + break; default: ret = true; break; @@ -4145,6 +4160,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) case MSG_MISSION_ITEM_REACHED: case MSG_NEXT_MISSION_REQUEST_WAYPOINTS: case MSG_NEXT_MISSION_REQUEST_RALLY: + case MSG_NEXT_MISSION_REQUEST_FENCE: ret = try_send_mission_message(id); break; @@ -4664,6 +4680,10 @@ uint64_t GCS_MAVLINK::capabilities() const if (AP::rally()) { ret |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY; } + if (AP::fence()) { + // FIXME: plane also supports this... + ret |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE; + } return ret; } diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index b8e8f8fc53..c54af0d986 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -69,4 +69,3 @@ void GCS_MAVLINK::send_fence_status() const mavlink_breach_type, fence->get_breach_time()); } - diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.cpp b/libraries/GCS_MAVLink/MissionItemProtocol.cpp index 52ee0a5a00..84b711da25 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol.cpp @@ -63,19 +63,28 @@ void MissionItemProtocol::handle_mission_count( send_mission_ack(_link, msg, MAV_MISSION_DENIED); return; } + // the upload count may have changed; free resources and + // allocate them again: + free_upload_resources(); } if (packet.count > max_items()) { + // FIXME: different items take up different storage space! send_mission_ack(_link, msg, MAV_MISSION_NO_SPACE); return; } + MAV_MISSION_RESULT ret_alloc = allocate_receive_resources(packet.count); + if (ret_alloc != MAV_MISSION_ACCEPTED) { + send_mission_ack(_link, msg, ret_alloc); + return; + } + truncate(packet); if (packet.count == 0) { // no requests to send... - const MAV_MISSION_RESULT result = complete(_link); - send_mission_ack(_link, msg, result); + transfer_is_complete(_link, msg); return; } @@ -193,6 +202,12 @@ void MissionItemProtocol::handle_mission_write_partial_list(GCS_MAVLINK &_link, return; } + MAV_MISSION_RESULT ret_alloc = allocate_update_resources(); + if (ret_alloc != MAV_MISSION_ACCEPTED) { + send_mission_ack(_link, msg, ret_alloc); + return; + } + init_send_requests(_link, msg, packet.start_index, packet.end_index); } @@ -235,6 +250,7 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons send_mission_ack(msg, result); receiving = false; link = nullptr; + free_upload_resources(); return; } @@ -243,10 +259,7 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons request_i++; if (request_i > request_last) { - const MAV_MISSION_RESULT complete_result = complete(*link); - send_mission_ack(msg, complete_result); - receiving = false; - link = nullptr; + transfer_is_complete(*link, msg); return; } // if we have enough space, then send the next WP request immediately @@ -257,6 +270,15 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons } } +void MissionItemProtocol::transfer_is_complete(const GCS_MAVLINK &_link, const mavlink_message_t &msg) +{ + const MAV_MISSION_RESULT result = complete(_link); + send_mission_ack(_link, msg, result); + free_upload_resources(); + receiving = false; + link = nullptr; +} + void MissionItemProtocol::send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_RESULT result) const { @@ -317,7 +339,13 @@ void MissionItemProtocol::update() if (tnow - timelast_receive_ms > upload_timeout_ms) { receiving = false; timeout(); + mavlink_msg_mission_ack_send(link->get_chan(), + dest_sysid, + dest_compid, + MAV_MISSION_OPERATION_CANCELLED, + mission_type()); link = nullptr; + free_upload_resources(); return; } // resend request if we haven't gotten one: diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.h b/libraries/GCS_MAVLink/MissionItemProtocol.h index 8a3e7b1455..30a84b12c1 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol.h @@ -96,6 +96,13 @@ private: const mavlink_message_t &msg, const int16_t _request_first, const int16_t _request_last); + virtual MAV_MISSION_RESULT allocate_receive_resources(const uint16_t count) WARN_IF_UNUSED { + return MAV_MISSION_ACCEPTED; + } + virtual MAV_MISSION_RESULT allocate_update_resources() WARN_IF_UNUSED { + return MAV_MISSION_ACCEPTED; + } + virtual void free_upload_resources() { } void send_mission_ack(const mavlink_message_t &msg, MAV_MISSION_RESULT result) const; void send_mission_ack(const GCS_MAVLINK &link, const mavlink_message_t &msg, MAV_MISSION_RESULT result) const; @@ -106,9 +113,19 @@ 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; + // complete - method called when transfer is complete - backends + // are expected to override this method to do any required tidy up. virtual MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) { return MAV_MISSION_ACCEPTED; }; + // transfer_is_complete - tidy up after a transfer is complete; + // this method will call complete() so the backends can do their + // bit. + void transfer_is_complete(const GCS_MAVLINK &_link, const mavlink_message_t &msg); + + // timeout - called if the GCS fails to continue to supply items + // in a transfer. Backends are expected to tidy themselves up in + // this routine virtual void timeout() {}; bool mavlink2_requirement_met(const GCS_MAVLINK &_link, const mavlink_message_t &msg) const; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp new file mode 100644 index 0000000000..7eab1ccc99 --- /dev/null +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp @@ -0,0 +1,211 @@ +#include "MissionItemProtocol_Fence.h" + +#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) +{ + const uint8_t num_stored_items = _fence.polyfence().num_stored_items(); + if (packet.seq > num_stored_items) { + return MAV_MISSION_INVALID_SEQUENCE; + } + + AC_PolyFenceItem fenceitem; + + if (!_fence.polyfence().get_item(packet.seq, fenceitem)) { + return MAV_MISSION_ERROR; + } + + MAV_CMD ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION; // initialised to avoid compiler warning + float p1 = 0; + switch (fenceitem.type) { + case AC_PolyFenceType::POLYGON_INCLUSION: + ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION; + p1 = fenceitem.vertex_count; + break; + case AC_PolyFenceType::POLYGON_EXCLUSION: + ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION; + p1 = fenceitem.vertex_count; + break; + case AC_PolyFenceType::RETURN_POINT: + ret_cmd = MAV_CMD_NAV_FENCE_RETURN_POINT; + break; + case AC_PolyFenceType::CIRCLE_EXCLUSION: + ret_cmd = MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION; + p1 = fenceitem.radius; + break; + case AC_PolyFenceType::CIRCLE_INCLUSION: + ret_cmd = MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION; + p1 = fenceitem.radius; + break; + case AC_PolyFenceType::END_OF_STORAGE: + return MAV_MISSION_ERROR; + } + + ret_packet.command = ret_cmd; + ret_packet.param1 = p1; + ret_packet.x = fenceitem.loc.x; + ret_packet.y = fenceitem.loc.y; + ret_packet.z = 0; + + return MAV_MISSION_ACCEPTED; +} + +uint16_t MissionItemProtocol_Fence::item_count() const +{ + if (receiving) { + return _new_items_count; + } + return _fence.polyfence().num_stored_items(); +} + +static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(const mavlink_mission_item_int_t &mission_item_int, AC_PolyFenceItem &ret) +{ + if (mission_item_int.frame != MAV_FRAME_GLOBAL && + mission_item_int.frame != MAV_FRAME_GLOBAL_INT) { + return MAV_MISSION_UNSUPPORTED_FRAME; + } + + switch (mission_item_int.command) { + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION: + ret.type = AC_PolyFenceType::POLYGON_INCLUSION; + ret.vertex_count = mission_item_int.param1; + break; + case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: + ret.type = AC_PolyFenceType::POLYGON_EXCLUSION; + ret.vertex_count = mission_item_int.param1; + break; + case MAV_CMD_NAV_FENCE_RETURN_POINT: + ret.type = AC_PolyFenceType::RETURN_POINT; + break; + case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION: + ret.type = AC_PolyFenceType::CIRCLE_EXCLUSION; + ret.radius = mission_item_int.param1; + break; + case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION: + ret.type = AC_PolyFenceType::CIRCLE_INCLUSION; + ret.radius = mission_item_int.param1; + break; + default: + return MAV_MISSION_UNSUPPORTED; + } + ret.loc.x = mission_item_int.x; + ret.loc.y = mission_item_int.y; + return MAV_MISSION_ACCEPTED; +} + +MAV_MISSION_RESULT MissionItemProtocol_Fence::replace_item(const mavlink_mission_item_int_t &mission_item_int) +{ + if (_new_items == nullptr) { + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return MAV_MISSION_ERROR; + } + if (mission_item_int.seq >= _new_items_count) { + return MAV_MISSION_INVALID_SEQUENCE; + } + + const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(mission_item_int, _new_items[mission_item_int.seq]); + if (ret != MAV_MISSION_ACCEPTED) { + return ret; + } + if (_updated_mask != nullptr) { + _updated_mask[mission_item_int.seq/8] |= (1U<<(mission_item_int.seq%8)); + } + return MAV_MISSION_ACCEPTED; +} + +MAV_MISSION_RESULT MissionItemProtocol_Fence::append_item(const mavlink_mission_item_int_t &mission_item_int) +{ + return replace_item(mission_item_int); +} + +void MissionItemProtocol_Fence::free_upload_resources() +{ + free(_new_items); + _new_items = nullptr; + delete[] _updated_mask; + _updated_mask = nullptr; +} + +MAV_MISSION_RESULT MissionItemProtocol_Fence::complete(const GCS_MAVLINK &_link) +{ + if (_updated_mask != nullptr) { + // get any points that weren't filled in + for (uint16_t i=0; i<_new_items_count; i++) { + if (!(_updated_mask[i/8] & (1U<<(i%8)))) { + if (!_fence.polyfence().get_item(i, _new_items[i])) { + _link.send_text(MAV_SEVERITY_INFO, "Error replacing item (%u)", i); + return MAV_MISSION_ERROR; + } + } + } + } + + bool success = _fence.polyfence().write_fence(_new_items, _new_items_count); + if (!success) { + return MAV_MISSION_ERROR; + } + + // AP::logger().Write_Fence(); + return MAV_MISSION_ACCEPTED; +} +void MissionItemProtocol_Fence::timeout() +{ + link->send_text(MAV_SEVERITY_WARNING, "Fence upload timeout"); +} + +uint16_t MissionItemProtocol_Fence::max_items() const +{ + return _fence.polyfence().max_items(); +} + +void MissionItemProtocol_Fence::truncate(const mavlink_mission_count_t &packet) +{ + // FIXME: validate packet.count is same as allocated number of items +} + +bool MissionItemProtocol_Fence::clear_all_items() +{ + return _fence.polyfence().write_fence(nullptr, 0); +} + +MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_receive_resources(const uint16_t count) +{ + if (_new_items != nullptr) { + // this is an error - the base class should have called + // free_upload_resources first + AP::internalerror().error(AP_InternalError::error_t::flow_of_control); + return MAV_MISSION_ERROR; + } + + const uint16_t allocation_size = count * sizeof(AC_PolyFenceItem); + gcs().send_text(MAV_SEVERITY_DEBUG, "Allocating %u bytes for fence upload", allocation_size); + if (allocation_size != 0) { + _new_items = (AC_PolyFenceItem*)malloc(allocation_size); + if (_new_items == nullptr) { + gcs().send_text(MAV_SEVERITY_WARNING, "Out of memory for upload"); + return MAV_MISSION_ERROR; + } + } + _new_items_count = count; + return MAV_MISSION_ACCEPTED; +} + +MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_update_resources() +{ + const uint16_t _item_count = _fence.polyfence().num_stored_items(); + _updated_mask = new uint8_t[(_item_count+7/8)]; + if (_updated_mask == nullptr) { + return MAV_MISSION_ERROR; + } + MAV_MISSION_RESULT ret = allocate_receive_resources(_item_count); + if (ret != MAV_MISSION_ACCEPTED) { + delete[] _updated_mask; + _updated_mask = nullptr; + return ret; + } + _new_items_count = _item_count; + return MAV_MISSION_ACCEPTED; +} diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h new file mode 100644 index 0000000000..91ad93dbab --- /dev/null +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h @@ -0,0 +1,48 @@ +#pragma once + +#include "MissionItemProtocol.h" + +class AC_PolyFence_loader; + +class MissionItemProtocol_Fence : public MissionItemProtocol { +public: + MissionItemProtocol_Fence(class AC_Fence &fence) : + _fence(fence) {} + + MAV_MISSION_TYPE mission_type() const override { + return MAV_MISSION_TYPE_FENCE; + } + + void truncate(const mavlink_mission_count_t &packet) override; + MAV_MISSION_RESULT complete(const GCS_MAVLINK &_link) override; + void timeout() override; + +protected: + + ap_message next_item_ap_message_id() const override { + return MSG_NEXT_MISSION_REQUEST_FENCE; + } + bool clear_all_items() override WARN_IF_UNUSED; + +private: + class AC_Fence &_fence; + + uint16_t item_count() const override; + uint16_t max_items() const override; + + MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; + MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; + + MAV_MISSION_RESULT 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) override WARN_IF_UNUSED; + + void free_upload_resources() override; + MAV_MISSION_RESULT allocate_receive_resources(const uint16_t count) override WARN_IF_UNUSED; + MAV_MISSION_RESULT allocate_update_resources() override WARN_IF_UNUSED; + + class AC_PolyFenceItem *_new_items; + uint16_t _new_items_count; + uint8_t *_updated_mask; +}; diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 10f19a1fc7..a50041162a 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -35,6 +35,7 @@ enum ap_message : uint8_t { MSG_SERVO_OUT, MSG_NEXT_MISSION_REQUEST_WAYPOINTS, MSG_NEXT_MISSION_REQUEST_RALLY, + MSG_NEXT_MISSION_REQUEST_FENCE, MSG_NEXT_PARAM, MSG_FENCE_STATUS, MSG_AHRS,