aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: add Hold and SmartRTL to FENCE_ACTION

Browse Source
This commit is contained in:
Tatsuya Yamaguchi 2020-02-01 19:58:07 +09:00 committed by Randy Mackay
parent 0b8ad28272
commit 83f285bbfc
1 changed files with 28 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
APMrover2/fence.cpp
View File

@ -16,13 +16,36 @@ void Rover::fence_check()
// if there is a new breach take action
if (new_breaches) {
gcs().send_text(MAV_SEVERITY_NOTICE,"Geofence triggered");
// if the user wants some kind of response and motors are armed
if (g2.fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
// if we are within 100m of the fence, RTL
if (g2.fence.get_action() != Failsafe_Action_None) {
// if we are within 100m of the fence, HOLD
if (g2.fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) {
set_mode(mode_hold, ModeReason::FENCE_BREACHED);
}
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);