Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : #if AP_FENCE_ENABLED
       4             : 
       5             : // async fence checking io callback at 1Khz
       6    16950630 : void Rover::fence_checks_async()
       7             : {
       8    16950630 :     const uint32_t now = AP_HAL::millis();
       9             : 
      10    16950630 :     if (!AP_HAL::timeout_expired(fence_breaches.last_check_ms, now, 100U)) { // 10Hz update rate
      11    16787779 :         return;
      12             :     }
      13             : 
      14      162851 :     if (fence_breaches.have_updates) {
      15       36856 :         return; // wait for the main loop to pick up the new breaches before checking again
      16             :     }
      17             : 
      18      125995 :     fence_breaches.last_check_ms = now;
      19      125995 :     const uint8_t orig_breaches = fence.get_breaches();
      20             :     // check for new breaches; new_breaches is bitmask of fence types breached
      21      125995 :     fence_breaches.new_breaches = fence.check();
      22             : 
      23      125995 :     if (!fence_breaches.new_breaches && orig_breaches && fence.get_breaches() == 0) {
      24             :         // record clearing of breach
      25          16 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
      26             :     }
      27      125995 :     fence_breaches.have_updates = true; // new breaches latched so main loop will now pick it up
      28             : }
      29             : 
      30             : // fence_check - ask fence library to check for breaches and initiate the response
      31      125720 : void Rover::fence_check()
      32             : {
      33             :     // only take action if there is a new breach
      34      125720 :     if (!fence_breaches.have_updates) {
      35           0 :         return;
      36             :     }
      37             : 
      38             :     // return immediately if motors are not armed
      39      125720 :     if (!arming.is_armed()) {
      40       75508 :         fence_breaches.have_updates = false;
      41       75508 :         return;
      42             :     }
      43             : 
      44       50212 :     if (fence_breaches.new_breaches) {
      45             :         // if the user wants some kind of response and motors are armed
      46          11 :         if ((FailsafeAction)fence.get_action() != FailsafeAction::None) {
      47             :             // if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter
      48           9 :             if (fence.get_breach_distance(fence_breaches.new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
      49           9 :                 switch ((FailsafeAction)fence.get_action()) {
      50           0 :                 case FailsafeAction::None:
      51           0 :                     break;
      52           0 :                 case FailsafeAction::SmartRTL:
      53           0 :                     if (set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {
      54           0 :                         break;
      55             :                     }
      56             :                     FALLTHROUGH;
      57             :                 case FailsafeAction::RTL:
      58           9 :                     if (set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) {
      59           9 :                         break;
      60             :                     }
      61             :                     FALLTHROUGH;
      62             :                 case FailsafeAction::Hold:
      63           0 :                     set_mode(mode_hold, ModeReason::FENCE_BREACHED);
      64           0 :                     break;
      65           0 :                 case FailsafeAction::SmartRTL_Hold:
      66           0 :                     if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {
      67           0 :                         set_mode(mode_hold, ModeReason::FENCE_BREACHED);
      68             :                     }
      69           0 :                     break;
      70           0 :                 case FailsafeAction::Loiter_Hold:
      71           0 :                     if (!set_mode(mode_loiter, ModeReason::FENCE_BREACHED)) {
      72           0 :                         set_mode(mode_hold, ModeReason::FENCE_BREACHED);
      73             :                     }
      74           0 :                     break;
      75           0 :                 case FailsafeAction::Terminate:
      76           0 :                     arming.disarm(AP_Arming::Method::FENCEBREACH);
      77           0 :                     break;
      78             :                 }
      79             :             } else {
      80             :                 // if more than 100m outside the fence just force to HOLD
      81           0 :                 set_mode(mode_hold, ModeReason::FENCE_BREACHED);
      82             :             }
      83             :         }
      84          11 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(fence_breaches.new_breaches));
      85             :     }
      86       50212 :     fence_breaches.have_updates = false;
      87             : }
      88             : 
      89             : #endif // AP_FENCE_ENABLED