Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : // Code to integrate AC_Fence library with main ArduSub code
       4             : 
       5             : #if AP_FENCE_ENABLED
       6             : 
       7             : // fence_check - ask fence library to check for breaches and initiate the response
       8             : // async fence checking io callback at 1Khz
       9     9246080 : void Sub::fence_checks_async()
      10             : {
      11     9246080 :     const uint32_t now = AP_HAL::millis();
      12             :     // ignore any fence activity when not armed
      13     9246080 :     if (!motors.armed()) {
      14     4013193 :         return;
      15             :     }
      16             : 
      17     5232887 :     if (!AP_HAL::timeout_expired(fence_breaches.last_check_ms, now, 333U)) { // 3Hz update rate
      18     5223577 :         return;
      19             :     }
      20             : 
      21        9310 :     fence_breaches.last_check_ms = now;
      22        9310 :     const uint8_t orig_breaches = fence.get_breaches();
      23             :     // check for new breaches; new_breaches is bitmask of fence types breached
      24        9310 :     const uint8_t new_breaches = fence.check();
      25             : 
      26             :     // if the user wants some kind of response and motors are armed
      27        9310 :     if (new_breaches) {
      28           0 :         if (fence.get_action() != AC_Fence::Action::REPORT_ONLY) {
      29             :             //
      30             :             //            // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
      31             :             //            // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
      32             :             //            if (ap.land_complete || (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.manual_control && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
      33             :             //                init_disarm_motors();
      34             :             //            }else{
      35             :             //                // if we are within 100m of the fence, RTL
      36             :             //                if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
      37             :             //                    if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
      38             :             //                        set_mode(LAND, MODE_REASON_FENCE_BREACH);
      39             :             //                    }
      40             :             //                }else{
      41             :             //                    // if more than 100m outside the fence just force a land
      42             :             //                    set_mode(LAND, MODE_REASON_FENCE_BREACH);
      43             :             //                }
      44             :             //            }
      45             :         }
      46             : 
      47           0 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(fence_breaches.new_breaches));
      48             : 
      49        9310 :     } else if (orig_breaches) {
      50             :         // record clearing of breach
      51           0 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
      52             :     }
      53             : }
      54             : 
      55             : #endif