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 "GCS.h"
       7             : 
       8             : #include <AC_Fence/AC_Fence.h>
       9             : #include <AC_Avoidance/AC_Avoid.h>
      10             : #include <AP_Vehicle/AP_Vehicle_Type.h>
      11             : 
      12          95 : MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_int_t &packet)
      13             : {
      14          95 :     AC_Fence *fence = AP::fence();
      15          95 :     if (fence == nullptr) {
      16           0 :         return MAV_RESULT_UNSUPPORTED;
      17             :     }
      18             : 
      19          95 :     uint8_t fences = AC_FENCE_ALL_FENCES;
      20          95 :     if (uint8_t(packet.param2)) {
      21           0 :         fences = uint8_t(packet.param2);
      22             :     }
      23             : 
      24          95 :     switch (AC_Fence::MavlinkFenceActions(packet.param1)) {
      25          35 :     case AC_Fence::MavlinkFenceActions::DISABLE_FENCE:
      26          35 :         fence->enable(false, fences);
      27          35 :         return MAV_RESULT_ACCEPTED;
      28          60 :     case AC_Fence::MavlinkFenceActions::ENABLE_FENCE:
      29          60 :         if (!(fence->present() & fences)) {
      30           1 :             return MAV_RESULT_FAILED;
      31             :         }
      32             : 
      33          59 :         fence->enable(true, fences);
      34          59 :         return MAV_RESULT_ACCEPTED;
      35           0 :     case AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE:
      36           0 :         fence->enable(false, AC_FENCE_TYPE_ALT_MIN);
      37           0 :         return MAV_RESULT_ACCEPTED;
      38           0 :     default:
      39           0 :         return MAV_RESULT_FAILED;
      40             :     }
      41             : }
      42             : 
      43             : // fence_send_mavlink_status - send fence status to ground station
      44     1117208 : void GCS_MAVLINK::send_fence_status() const
      45             : {
      46     1117208 :     const AC_Fence *fence = AP::fence();
      47     1117208 :     if (fence == nullptr) {
      48        1760 :         return;
      49             :     }
      50     1115448 :     if (!fence->enabled()) {
      51     1060948 :         return;
      52             :     }
      53             : 
      54             :     // traslate fence library breach types to mavlink breach types
      55       54500 :     uint8_t mavlink_breach_type = FENCE_BREACH_NONE;
      56       54500 :     const uint8_t breaches = fence->get_breaches();
      57       54500 :     if ((breaches & AC_FENCE_TYPE_ALT_MIN) != 0) {
      58        2028 :         mavlink_breach_type = FENCE_BREACH_MINALT;
      59             :     }
      60       54500 :     if ((breaches & AC_FENCE_TYPE_ALT_MAX) != 0) {
      61        1312 :         mavlink_breach_type = FENCE_BREACH_MAXALT;
      62             :     }
      63       54500 :     if ((breaches & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) != 0) {
      64        4309 :         mavlink_breach_type = FENCE_BREACH_BOUNDARY;
      65             :     }
      66             : 
      67             :     // report on Avoidance limiting
      68       54500 :     uint8_t breach_mitigation = FENCE_MITIGATE_UNKNOWN;
      69             : #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
      70       33766 :     const AC_Avoid* avoid =  AC_Avoid::get_singleton();
      71       33766 :     if (avoid != nullptr) {
      72       33766 :         if (avoid->limits_active()) {
      73        2156 :             breach_mitigation = FENCE_MITIGATE_VEL_LIMIT;
      74             :         } else {
      75       31610 :             breach_mitigation = FENCE_MITIGATE_NONE;
      76             :         }
      77             :     }
      78             : #endif
      79             : 
      80             :     // send status
      81      109000 :     mavlink_msg_fence_status_send(chan,
      82       54500 :                                   static_cast<int8_t>(fence->get_breaches() != 0),
      83       54500 :                                   fence->get_breach_count(),
      84             :                                   mavlink_breach_type,
      85             :                                   fence->get_breach_time(),
      86             :                                   breach_mitigation);
      87             : }
      88             : 
      89             : #endif  // HAL_GCS_ENABLED && AP_FENCE_ENABLED