GCS_MAVLink: allow upload of fence using mission item protocol

GCS_MAVLink: add support for fence inclusion circles

GCS_MAVLink: factor out a transfer_is_complete; start commenting properly

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,
 };
 
 /*

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);
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -16,6 +16,7 @@
  */
 #include "GCS.h"
 
+#include <AC_Fence/AC_Fence.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Arming/AP_Arming.h>
@@ -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; i<num_gcs(); i++) {
         chan(i)->update_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;
 }
 

libraries/GCS_MAVLink/GCS_Fence.cpp

@@ -69,4 +69,3 @@ void GCS_MAVLINK::send_fence_status() const
                                   mavlink_breach_type,
                                   fence->get_breach_time());
 }
-

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);
+        transfer_is_complete(_link, msg);
-        send_mission_ack(_link, msg, result);
         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);
+        transfer_is_complete(*link, msg);
-        send_mission_ack(msg, complete_result);
-        receiving = false;
-        link = nullptr;
         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:

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;

libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp

@@ -0,0 +1,211 @@
+#include "MissionItemProtocol_Fence.h"
+
+#include <AC_Fence/AC_Fence.h>
+
+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;
+}

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;
+};

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,