Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : // Code to integrate AC_Fence library with main ArduCopter code
       4             : 
       5             : #if AP_FENCE_ENABLED
       6             : 
       7             : // async fence checking io callback at 1Khz
       8   128998846 : void Copter::fence_checks_async()
       9             : {
      10   128998846 :     const uint32_t now = AP_HAL::millis();
      11             : 
      12   128998846 :     if (!AP_HAL::timeout_expired(fence_breaches.last_check_ms, now, 40U)) { // 25Hz update rate
      13   126805404 :         return;
      14             :     }
      15             : 
      16     2193442 :     if (fence_breaches.have_updates) {
      17      292191 :         return; // wait for the main loop to pick up the new breaches before checking again
      18             :     }
      19             : 
      20     1901251 :     fence_breaches.last_check_ms = now;
      21     1901251 :     const uint8_t orig_breaches = fence.get_breaches();
      22     1901251 :     bool is_landing_or_landed = flightmode->is_landing() || ap.land_complete  || !motors->armed();
      23             : 
      24             :     // check for new breaches; new_breaches is bitmask of fence types breached
      25     1901251 :     fence_breaches.new_breaches = fence.check(is_landing_or_landed);
      26             : 
      27     1901251 :     if (!fence_breaches.new_breaches && orig_breaches && fence.get_breaches() == 0) {
      28          44 :         if (!copter.ap.land_complete) {
      29          27 :             GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared");
      30             :         }
      31             :         // record clearing of breach
      32          44 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
      33             :     }
      34     1901251 :     fence_breaches.have_updates = true; // new breach status latched so main loop will now pick it up
      35             : }
      36             : 
      37             : // fence_check - ask fence library to check for breaches and initiate the response
      38             : // called at 25hz
      39     1900265 : void Copter::fence_check()
      40             : {
      41             :     // only take action if there is a new breach
      42     1900265 :     if (!fence_breaches.have_updates) {
      43           0 :         return;
      44             :     }
      45             : 
      46             :     // we still don't do anything when disarmed, but we do check for fence breaches.
      47             :     // fence pre-arm check actually checks if any fence has been breached 
      48             :     // that's not ever going to be true if we don't call check on AP_Fence while disarmed.
      49     1900265 :     if (!motors->armed()) {
      50      666128 :         fence_breaches.have_updates = false; // fence checking can now be processed again
      51      666128 :         return;
      52             :     }
      53             : 
      54     1234137 :     if (fence_breaches.new_breaches) {
      55             : 
      56          38 :         if (!copter.ap.land_complete) {
      57          38 :             fence.print_fence_message("breached", fence_breaches.new_breaches);
      58             :         }
      59             : 
      60             :         // if the user wants some kind of response and motors are armed
      61          38 :         const auto fence_act = fence.get_action();
      62          38 :         if (fence_act != AC_Fence::Action::REPORT_ONLY ) {
      63             : 
      64             :             // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
      65             :             // 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
      66          36 :             if (ap.land_complete || (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
      67           0 :                 arming.disarm(AP_Arming::Method::FENCEBREACH);
      68             : 
      69             :             } else {
      70             : 
      71             :                 // if more than 100m outside the fence just force a land
      72          36 :                 if (fence.get_breach_distance(fence_breaches.new_breaches) > AC_FENCE_GIVE_UP_DISTANCE) {
      73           0 :                     set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
      74             :                 } else {
      75          36 :                     switch (fence_act) {
      76          36 :                     case AC_Fence::Action::RTL_AND_LAND:
      77             :                     default:
      78             :                         // switch to RTL, if that fails then Land
      79          36 :                         if (!set_mode(Mode::Number::RTL, ModeReason::FENCE_BREACHED)) {
      80           0 :                             set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
      81             :                         }
      82          36 :                         break;
      83           0 :                     case AC_Fence::Action::ALWAYS_LAND:
      84             :                         // if always land option mode is specified, land
      85           0 :                         set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
      86           0 :                         break;
      87           0 :                     case AC_Fence::Action::SMART_RTL:
      88             :                         // Try SmartRTL, if that fails, RTL, if that fails Land
      89           0 :                         if (!set_mode(Mode::Number::SMART_RTL, ModeReason::FENCE_BREACHED)) {
      90           0 :                             if (!set_mode(Mode::Number::RTL, ModeReason::FENCE_BREACHED)) {
      91           0 :                                 set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
      92             :                             }
      93             :                         }
      94           0 :                         break;
      95           0 :                     case AC_Fence::Action::BRAKE:
      96             :                         // Try Brake, if that fails Land
      97           0 :                         if (!set_mode(Mode::Number::BRAKE, ModeReason::FENCE_BREACHED)) {
      98           0 :                             set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
      99             :                         }
     100           0 :                         break;
     101           0 :                     case AC_Fence::Action::SMART_RTL_OR_LAND:
     102             :                         // Try SmartRTL, if that fails, Land
     103           0 :                         if (!set_mode(Mode::Number::SMART_RTL, ModeReason::FENCE_BREACHED)) {
     104           0 :                             set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
     105             :                         }
     106           0 :                         break;
     107             :                     }
     108             :                 }
     109             :             }
     110             :         }
     111             : 
     112          38 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(fence_breaches.new_breaches));
     113             :     }
     114     1234137 :     fence_breaches.have_updates = false; // fence checking can now be processed again
     115             : }
     116             : 
     117             : #endif