ardupilot/Rover/fence.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

63 lines
2.4 KiB
C++
Raw Normal View History

2017-08-16 07:02:56 -03:00
#include "Rover.h"
// fence_check - ask fence library to check for breaches and initiate the response
void Rover::fence_check()
{
#if AP_FENCE_ENABLED
2017-08-16 07:02:56 -03:00
uint8_t new_breaches; // the type of fence that has been breached
2022-03-04 12:41:00 -04:00
const uint8_t orig_breaches = fence.get_breaches();
2017-08-16 07:02:56 -03:00
// check for a breach
2022-03-04 12:41:00 -04:00
new_breaches = fence.check();
2017-08-16 07:02:56 -03:00
// return immediately if motors are not armed
if (!arming.is_armed()) {
return;
}
// if there is a new breach take action
if (new_breaches) {
// if the user wants some kind of response and motors are armed
2023-09-21 11:22:37 -03:00
if ((FailsafeAction)fence.get_action() != FailsafeAction::None) {
// if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter
2022-03-04 12:41:00 -04:00
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
2023-09-21 11:22:37 -03:00
switch ((FailsafeAction)fence.get_action()) {
case FailsafeAction::None:
break;
2023-09-21 11:22:37 -03:00
case FailsafeAction::SmartRTL:
if (set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {
2023-09-21 11:22:37 -03:00
break;
}
2023-09-21 11:22:37 -03:00
FALLTHROUGH;
case FailsafeAction::RTL:
if (set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) {
2023-09-21 11:22:37 -03:00
break;
}
2023-09-21 11:22:37 -03:00
FALLTHROUGH;
case FailsafeAction::Hold:
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
break;
2023-09-21 11:22:37 -03:00
case FailsafeAction::SmartRTL_Hold:
if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) {
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
}
break;
case FailsafeAction::Terminate:
arming.disarm(AP_Arming::Method::FENCEBREACH);
break;
}
2017-08-16 07:02:56 -03:00
} else {
// if more than 100m outside the fence just force to HOLD
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
2017-08-16 07:02:56 -03:00
}
}
LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
2017-08-16 07:02:56 -03:00
} else if (orig_breaches) {
// record clearing of breach
LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE,
LogErrorCode::ERROR_RESOLVED);
2017-08-16 07:02:56 -03:00
}
#endif // AP_FENCE_ENABLED
2017-08-16 07:02:56 -03:00
}