You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
61 lines
2.4 KiB
61 lines
2.4 KiB
#include "Rover.h" |
|
|
|
// fence_check - ask fence library to check for breaches and initiate the response |
|
void Rover::fence_check() |
|
{ |
|
#if AP_FENCE_ENABLED |
|
uint8_t new_breaches; // the type of fence that has been breached |
|
const uint8_t orig_breaches = fence.get_breaches(); |
|
|
|
// check for a breach |
|
new_breaches = fence.check(); |
|
|
|
// 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 |
|
if (fence.get_action() != Failsafe_Action_None) { |
|
// if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter |
|
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { |
|
switch (fence.get_action()) { |
|
case Failsafe_Action_None: |
|
break; |
|
case Failsafe_Action_RTL: |
|
if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { |
|
set_mode(mode_hold, ModeReason::FENCE_BREACHED); |
|
} |
|
break; |
|
case Failsafe_Action_Hold: |
|
set_mode(mode_hold, ModeReason::FENCE_BREACHED); |
|
break; |
|
case Failsafe_Action_SmartRTL: |
|
if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { |
|
if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { |
|
set_mode(mode_hold, ModeReason::FENCE_BREACHED); |
|
} |
|
} |
|
break; |
|
case Failsafe_Action_SmartRTL_Hold: |
|
if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { |
|
set_mode(mode_hold, ModeReason::FENCE_BREACHED); |
|
} |
|
break; |
|
} |
|
} else { |
|
// if more than 100m outside the fence just force to HOLD |
|
set_mode(mode_hold, ModeReason::FENCE_BREACHED); |
|
} |
|
} |
|
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); |
|
|
|
} else if (orig_breaches) { |
|
// record clearing of breach |
|
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, |
|
LogErrorCode::ERROR_RESOLVED); |
|
} |
|
#endif // AP_FENCE_ENABLED |
|
}
|
|
|