Line data    Source code

       1             : #include "GCS_config.h"
       2             : #include <AC_Fence/AC_Fence_config.h>
       3             : 
       4             : #if HAL_GCS_ENABLED && AP_FENCE_ENABLED
       5             : 
       6             : #include "MissionItemProtocol_Fence.h"
       7             : 
       8             : #include <AC_Fence/AC_Fence.h>
       9             : #include <AP_InternalError/AP_InternalError.h>
      10             : #include <GCS_MAVLink/GCS.h>
      11             : 
      12             : /*
      13             :   public function to format mission item as mavlink_mission_item_int_t
      14             :  */
      15         943 : bool MissionItemProtocol_Fence::get_item_as_mission_item(uint16_t seq,
      16             :                                                          mavlink_mission_item_int_t &ret_packet)
      17             : {
      18         943 :     AC_Fence *fence = AP::fence();
      19         943 :     if (fence == nullptr) {
      20           0 :         return false;
      21             :     }
      22         943 :     const auto num_stored_items = fence->polyfence().num_stored_items();
      23         943 :     if (seq > num_stored_items) {
      24           0 :         return false;
      25             :     }
      26             : 
      27         943 :     AC_PolyFenceItem fenceitem;
      28             : 
      29         943 :     if (!fence->polyfence().get_item(seq, fenceitem)) {
      30           0 :         return false;
      31             :     }
      32             : 
      33         943 :     MAV_CMD ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION; // initialised to avoid compiler warning
      34         943 :     float p1 = 0;
      35         943 :     switch (fenceitem.type) {
      36         264 :     case AC_PolyFenceType::POLYGON_INCLUSION:
      37         264 :         ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION;
      38         264 :         p1 = fenceitem.vertex_count;
      39         264 :         break;
      40         662 :     case AC_PolyFenceType::POLYGON_EXCLUSION:
      41         662 :         ret_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION;
      42         662 :         p1 = fenceitem.vertex_count;
      43         662 :         break;
      44           0 :     case AC_PolyFenceType::RETURN_POINT:
      45           0 :         ret_cmd = MAV_CMD_NAV_FENCE_RETURN_POINT;
      46           0 :         break;
      47           6 :     case AC_PolyFenceType::CIRCLE_EXCLUSION:
      48           6 :         ret_cmd = MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION;
      49           6 :         p1 = fenceitem.radius;
      50           6 :         break;
      51           4 :     case AC_PolyFenceType::CIRCLE_INCLUSION:
      52           4 :         ret_cmd = MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION;
      53           4 :         p1 = fenceitem.radius;
      54           4 :         break;
      55           7 :     case AC_PolyFenceType::HOME_CIRCLE_INCLUSION:
      56           7 :         ret_cmd = MAV_CMD_NAV_FENCE_HOME_CIRCLE_INCLUSION;
      57           7 :         p1 = fenceitem.radius;
      58           7 :         break;
      59             : #if AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
      60           0 :     case AC_PolyFenceType::CIRCLE_EXCLUSION_INT:
      61             :     case AC_PolyFenceType::CIRCLE_INCLUSION_INT:
      62             :         // should never have an AC_PolyFenceItem with these types
      63           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      64             :         FALLTHROUGH;
      65             : #endif  // AC_POLYFENCE_CIRCLE_INT_SUPPORT_ENABLED
      66           0 :     case AC_PolyFenceType::END_OF_STORAGE:
      67           0 :         return false;
      68             :     }
      69             : 
      70         943 :     ret_packet = {
      71             :         param1: p1,
      72         943 :         x: fenceitem.loc.x,
      73         943 :         y: fenceitem.loc.y,
      74             :         z: 0,
      75             :         seq: seq,
      76             :         command: uint16_t(ret_cmd),
      77             :         mission_type: MAV_MISSION_TYPE_FENCE,
      78             :     };
      79             : 
      80         943 :     return true;
      81             : }
      82             : 
      83         944 : MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet)
      84             : {
      85         944 :     if (seq >= _fence.polyfence().num_stored_items()) {
      86           1 :         return MAV_MISSION_INVALID_SEQUENCE;
      87             :     }
      88             : 
      89         943 :     if (!get_item_as_mission_item(seq, ret_packet)) {
      90           0 :         return MAV_MISSION_ERROR;
      91             :     }
      92             : 
      93         943 :     return MAV_MISSION_ACCEPTED;
      94             : }
      95             : 
      96        1006 : uint16_t MissionItemProtocol_Fence::item_count() const
      97             : {
      98        1006 :     if (receiving) {
      99         958 :         return _new_items_count;
     100             :     }
     101          48 :     return _fence.polyfence().num_stored_items();
     102             : }
     103             : 
     104         958 : MAV_MISSION_RESULT MissionItemProtocol_Fence::convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(const mavlink_mission_item_int_t &mission_item_int, AC_PolyFenceItem &ret)
     105             : {
     106         958 :     if (mission_item_int.frame != MAV_FRAME_GLOBAL &&
     107           6 :         mission_item_int.frame != MAV_FRAME_GLOBAL_INT &&
     108           0 :         mission_item_int.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
     109           0 :         mission_item_int.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
     110           0 :         mission_item_int.frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
     111           0 :         mission_item_int.frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
     112           0 :         return MAV_MISSION_UNSUPPORTED_FRAME;
     113             :     }
     114             : 
     115         958 :     switch (mission_item_int.command) {
     116         279 :     case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
     117         279 :         ret.type = AC_PolyFenceType::POLYGON_INCLUSION;
     118         279 :         if (mission_item_int.param1 > 255) {
     119           0 :             return MAV_MISSION_INVALID_PARAM1;
     120             :         }
     121         279 :         ret.vertex_count = mission_item_int.param1;
     122         279 :         break;
     123         655 :     case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
     124         655 :         ret.type = AC_PolyFenceType::POLYGON_EXCLUSION;
     125         655 :         if (mission_item_int.param1 > 255) {
     126           0 :             return MAV_MISSION_INVALID_PARAM1;
     127             :         }
     128         655 :         ret.vertex_count = mission_item_int.param1;
     129         655 :         break;
     130           1 :     case MAV_CMD_NAV_FENCE_RETURN_POINT:
     131           1 :         ret.type = AC_PolyFenceType::RETURN_POINT;
     132           1 :         break;
     133          10 :     case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION:
     134          10 :         ret.type = AC_PolyFenceType::CIRCLE_EXCLUSION;
     135          10 :         ret.radius = mission_item_int.param1;
     136          10 :         break;
     137           6 :     case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION:
     138           6 :         ret.type = AC_PolyFenceType::CIRCLE_INCLUSION;
     139           6 :         ret.radius = mission_item_int.param1;
     140           6 :         break;
     141           7 :     case MAV_CMD_NAV_FENCE_HOME_CIRCLE_INCLUSION:
     142           7 :         ret.type = AC_PolyFenceType::HOME_CIRCLE_INCLUSION;
     143           7 :         ret.radius = mission_item_int.param1;
     144           7 :         break;
     145           0 :     default:
     146           0 :         return MAV_MISSION_UNSUPPORTED;
     147             :     }
     148         958 :     ret.loc.x = mission_item_int.x;
     149         958 :     ret.loc.y = mission_item_int.y;
     150         958 :     return MAV_MISSION_ACCEPTED;
     151             : }
     152             : 
     153         958 : MAV_MISSION_RESULT MissionItemProtocol_Fence::replace_item(const mavlink_mission_item_int_t &mission_item_int)
     154             : {
     155         958 :     if (_new_items == nullptr) {
     156           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     157           0 :         return MAV_MISSION_ERROR;
     158             :     }
     159         958 :     if (mission_item_int.seq >= _new_items_count) {
     160           0 :         return MAV_MISSION_INVALID_SEQUENCE;
     161             :     }
     162             : 
     163         958 :     const MAV_MISSION_RESULT ret = convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(mission_item_int, _new_items[mission_item_int.seq]);
     164         958 :     if (ret != MAV_MISSION_ACCEPTED) {
     165           0 :         return ret;
     166             :     }
     167         958 :     if (_updated_mask != nullptr) {
     168           0 :         _updated_mask[mission_item_int.seq/8] |= (1U<<(mission_item_int.seq%8));
     169             :     }
     170         958 :     return MAV_MISSION_ACCEPTED;
     171             : }
     172             : 
     173           0 : MAV_MISSION_RESULT MissionItemProtocol_Fence::append_item(const mavlink_mission_item_int_t &mission_item_int)
     174             : {
     175           0 :     return replace_item(mission_item_int);
     176             : }
     177             : 
     178        1006 : void MissionItemProtocol_Fence::free_upload_resources()
     179             : {
     180        1006 :     free(_new_items);
     181        1006 :     _new_items = nullptr;
     182        1006 :     delete[] _updated_mask;
     183        1006 :     _updated_mask = nullptr;
     184        1006 : }
     185             : 
     186        1001 : MAV_MISSION_RESULT MissionItemProtocol_Fence::complete(const GCS_MAVLINK &_link)
     187             : {
     188        1001 :     if (_updated_mask != nullptr) {
     189             :         // get any points that weren't filled in
     190           0 :         for (uint16_t i=0; i<_new_items_count; i++) {
     191           0 :             if (!(_updated_mask[i/8] & (1U<<(i%8)))) {
     192           0 :                 if (!_fence.polyfence().get_item(i, _new_items[i])) {
     193           0 :                     _link.send_text(MAV_SEVERITY_INFO, "Error replacing item (%u)", i);
     194           0 :                     return MAV_MISSION_ERROR;
     195             :                 }
     196             :             }
     197             :         }
     198             :     }
     199             : 
     200        1001 :     bool success = _fence.polyfence().write_fence(_new_items, _new_items_count);
     201        1001 :     if (!success) {
     202           0 :         return MAV_MISSION_ERROR;
     203             :     }
     204             : 
     205             :     // AP::logger().Write_Fence();
     206        1001 :     return MAV_MISSION_ACCEPTED;
     207             : }
     208           4 : void MissionItemProtocol_Fence::timeout()
     209             : {
     210           4 :     link->send_text(MAV_SEVERITY_WARNING, "Fence upload timeout");
     211           4 : }
     212             : 
     213        1008 : uint16_t MissionItemProtocol_Fence::max_items() const
     214             : {
     215        1008 :     return _fence.polyfence().max_items();
     216             : }
     217             : 
     218        1006 : void MissionItemProtocol_Fence::truncate(const mavlink_mission_count_t &packet)
     219             : {
     220             :     // FIXME: validate packet.count is same as allocated number of items
     221        1006 : }
     222             : 
     223           0 : bool MissionItemProtocol_Fence::clear_all_items()
     224             : {
     225           0 :     return _fence.polyfence().write_fence(nullptr, 0);
     226             : }
     227             : 
     228        1006 : MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_receive_resources(const uint16_t count)
     229             : {
     230        1006 :     if (_new_items != nullptr) {
     231             :         // this is an error - the base class should have called
     232             :         // free_upload_resources first
     233           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     234           0 :         return MAV_MISSION_ERROR;
     235             :     }
     236             : 
     237        1006 :     const uint32_t allocation_size = count * sizeof(AC_PolyFenceItem);
     238        1006 :     if (allocation_size != 0) {
     239          53 :         _new_items = (AC_PolyFenceItem*)malloc(allocation_size);
     240          53 :         if (_new_items == nullptr) {
     241           0 :             GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Out of memory for upload");
     242           0 :             return MAV_MISSION_ERROR;
     243             :         }
     244             :     }
     245        1006 :     _new_items_count = count;
     246        1006 :     return MAV_MISSION_ACCEPTED;
     247             : }
     248             : 
     249           0 : MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_update_resources()
     250             : {
     251           0 :     const uint16_t _item_count = _fence.polyfence().num_stored_items();
     252           0 :     _updated_mask = NEW_NOTHROW uint8_t[(_item_count+7)/8];
     253           0 :     if (_updated_mask == nullptr) {
     254           0 :         return MAV_MISSION_ERROR;
     255             :     }
     256           0 :     MAV_MISSION_RESULT ret = allocate_receive_resources(_item_count);
     257           0 :     if (ret != MAV_MISSION_ACCEPTED) {
     258           0 :         delete[] _updated_mask;
     259           0 :         _updated_mask = nullptr;
     260           0 :         return ret;
     261             :     }
     262           0 :     _new_items_count = _item_count;
     263           0 :     return MAV_MISSION_ACCEPTED;
     264             : }
     265             : 
     266             : #endif // HAL_GCS_ENABLED && AP_FENCE_ENABLED