|
|
|
@ -20,30 +20,30 @@ void Rover::fence_check()
@@ -20,30 +20,30 @@ void Rover::fence_check()
|
|
|
|
|
if (g2.fence.get_action() != Failsafe_Action_None) { |
|
|
|
|
// if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter
|
|
|
|
|
if (g2.fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { |
|
|
|
|
switch (g2.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; |
|
|
|
|
} |
|
|
|
|
switch (g2.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); |
|
|
|
|