Line data    Source code

       1             : #include "Plane.h"
       2             : 
       3             : // Code to integrate AC_Fence library with main ArduPlane code
       4             : 
       5             : #if AP_FENCE_ENABLED
       6             : 
       7             : // async fence checking io callback at 1Khz
       8    79445325 : void Plane::fence_checks_async()
       9             : {
      10    79445325 :     const uint32_t now = AP_HAL::millis();
      11             : 
      12    79445325 :     if (!AP_HAL::timeout_expired(fence_breaches.last_check_ms, now, 333U)) { // 3Hz update rate
      13    79053987 :         return;
      14             :     }
      15             : 
      16      391338 :     if (fence_breaches.have_updates) {
      17      226522 :         return; // wait for the main loop to pick up the new breaches before checking again
      18             :     }
      19             : 
      20      164816 :     fence_breaches.last_check_ms = now;
      21      164816 :     orig_breaches = fence.get_breaches();
      22      164816 :     const bool armed = arming.is_armed();
      23             : 
      24      164816 :     uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
      25      164816 :     bool landing_or_landed = plane.flight_stage == AP_FixedWing::FlightStage::LAND
      26      153961 :                          || !armed
      27             : #if HAL_QUADPLANE_ENABLED
      28      113489 :                          || control_mode->mode_number() == Mode::Number::QLAND
      29      110870 :                          || quadplane.in_vtol_land_descent()
      30             : #endif
      31      318777 :                          || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING);
      32             : 
      33             :     // check for new breaches; new_breaches is bitmask of fence types breached
      34      164816 :     fence_breaches.new_breaches = fence.check(landing_or_landed);
      35      164816 :     fence_breaches.have_updates = true; // new breach status latched so main loop will now pick it up
      36             : }
      37             : 
      38             : // fence_check - ask fence library to check for breaches and initiate the response
      39      167812 : void Plane::fence_check()
      40             : {
      41      167812 :     if (!fence_breaches.have_updates) {
      42        3625 :         return;
      43             :     }
      44             :     /*
      45             :       if we are either disarmed or we are currently not in breach and
      46             :       we are not flying then clear the state associated with the
      47             :       previous mode breach handling. This allows the fence state
      48             :       machine to reset at the end of a fence breach action such as an
      49             :       RTL and autoland
      50             :      */
      51      164187 :     const bool armed = arming.is_armed();
      52             : 
      53      164187 :     if (plane.previous_mode_reason == ModeReason::FENCE_BREACHED) {
      54        1400 :         if (!armed || ((fence_breaches.new_breaches == 0 && orig_breaches == 0) && !plane.is_flying())) {
      55           5 :             plane.previous_mode_reason = ModeReason::FENCE_REENABLE;
      56             :         }
      57             :     }
      58             : 
      59      164187 :     if (!fence.enabled()) {
      60             :         // Switch back to the chosen control mode if still in
      61             :         // GUIDED to the return point
      62      157972 :         switch(fence.get_action()) {
      63      157600 :             case AC_Fence::Action::GUIDED:
      64             :             case AC_Fence::Action::GUIDED_THROTTLE_PASS:
      65             :             case AC_Fence::Action::RTL_AND_LAND:
      66             :             case AC_Fence::Action::AUTOLAND_OR_RTL:
      67      157619 :                 if (plane.control_mode_reason == ModeReason::FENCE_BREACHED &&
      68          19 :                     control_mode->is_guided_mode()) {
      69           6 :                     set_mode(*previous_mode, ModeReason::FENCE_RETURN_PREVIOUS_MODE);
      70             :                 }
      71      157600 :                 break;
      72         372 :             default:
      73             :                 // No returning to a previous mode, unless our action allows it
      74         372 :                 break;
      75             :         }
      76             :         /*
      77             :           clear mode reasons if they are FENCE_BREACHED to allow AUX
      78             :           switch fence disable/enable to re-enable the fence after a breach
      79             :          */
      80      157972 :         if (plane.previous_mode_reason == ModeReason::FENCE_BREACHED) {
      81          10 :             plane.previous_mode_reason = ModeReason::FENCE_REENABLE;
      82             :         }
      83      157972 :         if (plane.control_mode_reason == ModeReason::FENCE_BREACHED) {
      84          13 :             plane.control_mode_reason = ModeReason::FENCE_REENABLE;
      85             :         }
      86      157972 :         goto fence_check_complete;
      87             :     }
      88             : 
      89             :     // we still don't do anything when disarmed, but we do check for fence breaches.
      90             :     // fence pre-arm check actually checks if any fence has been breached
      91             :     // that's not ever going to be true if we don't call check on AP_Fence while disarmed
      92        6215 :     if (!armed) {
      93         527 :         goto fence_check_complete;
      94             :     }
      95             : 
      96             :     // Never trigger a fence breach in the final stage of landing
      97        5688 :     if (landing.is_expecting_impact()) {
      98         488 :         goto fence_check_complete;
      99             :     }
     100             : 
     101        5200 :     if (in_fence_recovery()) {
     102             :         // we have already triggered, don't trigger again until the
     103             :         // user disables/re-enables using the fence channel switch
     104        1149 :         goto fence_check_complete;
     105             :     }
     106             : 
     107        4051 :     if (fence_breaches.new_breaches) {
     108          40 :         fence.print_fence_message("breached", fence_breaches.new_breaches);
     109             : 
     110             :         // if the user wants some kind of response and motors are armed
     111          40 :         const auto fence_act = fence.get_action();
     112          40 :         switch (fence_act) {
     113           3 :         case AC_Fence::Action::REPORT_ONLY:
     114          40 :             break;
     115             : 
     116           0 :         case AC_Fence::Action::ALWAYS_LAND:
     117             :         case AC_Fence::Action::SMART_RTL:
     118             :         case AC_Fence::Action::SMART_RTL_OR_LAND:
     119             :         case AC_Fence::Action::BRAKE:
     120             :             // invalid enumeration value for Plane
     121           0 :             break;
     122             : 
     123          30 :         case AC_Fence::Action::AUTOLAND_OR_RTL:
     124             :         case AC_Fence::Action::RTL_AND_LAND:
     125           2 :             if (control_mode == &mode_auto &&
     126          32 :                 mission.get_in_landing_sequence_flag() &&
     127           0 :                 (g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START ||
     128           0 :                     g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START)) {
     129             :                 // already landing
     130           0 :                 goto fence_check_complete;
     131             :             }
     132             : #if MODE_AUTOLAND_ENABLED
     133          30 :             if (control_mode == &mode_autoland) {
     134             :                 // Already landing
     135           0 :                 return;
     136             :             }
     137          30 :             if ((fence_act == AC_Fence::Action::AUTOLAND_OR_RTL) && set_mode(mode_autoland, ModeReason::FENCE_BREACHED)) {
     138           1 :                 break;
     139             :             }
     140             : #endif
     141          29 :             set_mode(mode_rtl, ModeReason::FENCE_BREACHED);
     142          29 :             break;
     143             : 
     144           7 :         case AC_Fence::Action::GUIDED:
     145             :         case AC_Fence::Action::GUIDED_THROTTLE_PASS:
     146           7 :             set_mode(mode_guided, ModeReason::FENCE_BREACHED);
     147             : 
     148           7 :             Location loc;
     149           7 :             if (fence.get_return_rally() != 0) {
     150           1 :                 loc = calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm());
     151             :             } else {
     152             :                 //return to fence return point, not a rally point
     153           6 :                 if (fence.get_return_altitude() > 0) {
     154             :                     // fly to the return point using _retalt
     155           1 :                     loc.set_alt_cm(home.alt + 100.0 * fence.get_return_altitude(),
     156             :                                    Location::AltFrame::ABSOLUTE);
     157           5 :                 } else if (fence.get_safe_alt_min_m() >= fence.get_safe_alt_max_m()) {
     158             :                     // invalid min/max, use RTL_altitude
     159           0 :                     loc.set_alt_cm(home.alt + g.RTL_altitude*100,
     160             :                                    Location::AltFrame::ABSOLUTE);
     161             :                 } else {
     162             :                     // fly to the return point, with an altitude half way between
     163             :                     // min and max
     164           5 :                     loc.set_alt_cm(home.alt + 100.0f * (fence.get_safe_alt_min_m() + fence.get_safe_alt_max_m()) / 2,
     165             :                                    Location::AltFrame::ABSOLUTE);
     166             :                 }
     167             : 
     168           6 :                 Vector2l return_point;
     169           6 :                 if(fence.polyfence().get_return_point(return_point)) {
     170           3 :                     loc.lat = return_point[0];
     171           3 :                     loc.lng = return_point[1];
     172             :                 } else {
     173             :                     // When no fence return point is found (ie. no inclusion fence uploaded, but exclusion is)
     174             :                     // we fail to obtain a valid fence return point. In this case, home is considered a safe
     175             :                     // return point.
     176           3 :                     loc.lat = home.lat;
     177           3 :                     loc.lng = home.lng;
     178             :                 }
     179             :             }
     180             : 
     181           7 :             setup_terrain_target_alt(loc);
     182           7 :             set_guided_WP(loc);
     183             : 
     184           7 :             if (fence.get_action() == AC_Fence::Action::GUIDED_THROTTLE_PASS) {
     185           1 :                 guided_throttle_passthru = true;
     186             :             }
     187           7 :             break;
     188             :         }
     189             : 
     190          40 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(fence_breaches.new_breaches));
     191        4011 :     } else if (orig_breaches && fence.get_breaches() == 0) {
     192           3 :         GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared");
     193             :         // record clearing of breach
     194           3 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
     195             :     }
     196             : 
     197        4008 : fence_check_complete:
     198      164187 :     fence_breaches.have_updates = false;
     199             : }
     200             : 
     201    15396868 : bool Plane::fence_stickmixing(void) const
     202             : {
     203    15712574 :     if (fence.enabled() &&
     204    15712574 :         fence.get_breaches() &&
     205       53102 :         in_fence_recovery())
     206             :     {
     207             :         // don't mix in user input
     208       38590 :         return false;
     209             :     }
     210             :     // normal mixing rules
     211    15358278 :     return true;
     212             : }
     213             : 
     214       58366 : bool Plane::in_fence_recovery() const
     215             : {
     216       58366 :     if (control_mode == &mode_auto && !mission.get_in_landing_sequence_flag()) {
     217             :         // the user may have changed target WP to be outside the
     218             :         // landing sequence
     219        3683 :         return false;
     220             :     }
     221       54683 :     const bool current_mode_breach = plane.control_mode_reason == ModeReason::FENCE_BREACHED;
     222       54683 :     const bool previous_mode_breach = plane.previous_mode_reason ==  ModeReason::FENCE_BREACHED;
     223      164049 :     const bool previous_mode_complete = (plane.control_mode_reason == ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL) ||
     224       54683 :                                         (plane.control_mode_reason == ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND) ||
     225      162306 :                                         (plane.control_mode_reason == ModeReason::QRTL_INSTEAD_OF_RTL) ||
     226       52940 :                                         (plane.control_mode_reason == ModeReason::QLAND_INSTEAD_OF_RTL);
     227             : 
     228       54683 :     return current_mode_breach || (previous_mode_breach && previous_mode_complete);
     229             : }
     230             : 
     231             : #endif