|
|
|
@ -8,19 +8,18 @@
@@ -8,19 +8,18 @@
|
|
|
|
|
// called at 1hz
|
|
|
|
|
void Sub::fence_check() |
|
|
|
|
{ |
|
|
|
|
uint8_t new_breaches; // the type of fence that has been breached
|
|
|
|
|
uint8_t orig_breaches = fence.get_breaches(); |
|
|
|
|
|
|
|
|
|
// check for a breach
|
|
|
|
|
new_breaches = fence.check_fence(current_loc.alt/100.0f); |
|
|
|
|
|
|
|
|
|
// return immediately if motors are not armed
|
|
|
|
|
// ignore any fence activity when not armed
|
|
|
|
|
if (!motors.armed()) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
const uint8_t orig_breaches = fence.get_breaches(); |
|
|
|
|
|
|
|
|
|
// check for new breaches; new_breaches is bitmask of fence types breached
|
|
|
|
|
const uint8_t new_breaches = fence.check(); |
|
|
|
|
|
|
|
|
|
// if there is a new breach take action
|
|
|
|
|
if (new_breaches != AC_FENCE_TYPE_NONE) { |
|
|
|
|
if (new_breaches) { |
|
|
|
|
|
|
|
|
|
// if the user wants some kind of response and motors are armed
|
|
|
|
|
if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) { |
|
|
|
@ -44,10 +43,8 @@ void Sub::fence_check()
@@ -44,10 +43,8 @@ void Sub::fence_check()
|
|
|
|
|
|
|
|
|
|
// log an error in the dataflash
|
|
|
|
|
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// record clearing of breach
|
|
|
|
|
if (orig_breaches != AC_FENCE_TYPE_NONE && fence.get_breaches() == AC_FENCE_TYPE_NONE) { |
|
|
|
|
} else if (orig_breaches) { |
|
|
|
|
// record clearing of breach
|
|
|
|
|
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|