CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Rover/fence.cpp
Views: 1798
1
#include "Rover.h"

2


3
// fence_check - ask fence library to check for breaches and initiate the response

4
void Rover::fence_check()

5
{

6
#if AP_FENCE_ENABLED

7
    uint8_t new_breaches;  // the type of fence that has been breached

8
    const uint8_t orig_breaches = fence.get_breaches();

9


10
    // check for a breach

11
    new_breaches = fence.check();

12


13
    // return immediately if motors are not armed

14
    if (!arming.is_armed()) {

15
        return;

16
    }

17


18
    // if there is a new breach take action

19
    if (new_breaches) {

20
        // if the user wants some kind of response and motors are armed

21
        if ((FailsafeAction)fence.get_action() != FailsafeAction::None) {

22
            // if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter

23
            if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {

24
                switch ((FailsafeAction)fence.get_action()) {

25
                case FailsafeAction::None:

26
                    break;

27
                case FailsafeAction::SmartRTL:

28
                    if (set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {

29
                        break;

30
                    }

31
                    FALLTHROUGH;

32
                case FailsafeAction::RTL:

33
                    if (set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) {

34
                        break;

35
                    }

36
                    FALLTHROUGH;

37
                case FailsafeAction::Hold:

38
                    set_mode(mode_hold, ModeReason::FENCE_BREACHED);

39
                    break;

40
                case FailsafeAction::SmartRTL_Hold:

41
                    if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {

42
                        set_mode(mode_hold, ModeReason::FENCE_BREACHED);

43
                    }

44
                    break;

45
                case FailsafeAction::Terminate:

46
                    arming.disarm(AP_Arming::Method::FENCEBREACH);

47
                    break;

48
                }

49
            } else {

50
                // if more than 100m outside the fence just force to HOLD

51
                set_mode(mode_hold, ModeReason::FENCE_BREACHED);

52
            }

53
        }

54
        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));

55


56
    } else if (orig_breaches) {

57
        // record clearing of breach

58
        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE,

59
                                 LogErrorCode::ERROR_RESOLVED);

60
    }

61
#endif // AP_FENCE_ENABLED

62
}

63


64