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

Copter: support fence actions SmartRTL and Brake

Browse Source
This commit is contained in:
chobitsfan 2019-01-24 19:01:26 +09:00 committed by Randy Mackay
parent e7f87f767a
commit 7f6b7dab2f
1 changed files with 32 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
ArduCopter/fence.cpp
View File

@ -24,7 +24,8 @@ void Copter::fence_check()
if (new_breaches) {
// if the user wants some kind of response and motors are armed
if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
uint8_t fence_act = fence.get_action();
if (fence_act != AC_FENCE_ACTION_REPORT_ONLY ) {
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
@ -32,17 +33,38 @@ void Copter::fence_check()
init_disarm_motors();
} else {
// if always land option mode is specified, land
if (fence.get_action() == AC_FENCE_ACTION_ALWAYS_LAND) {
// if more than 100m outside the fence just force a land
if (fence.get_breach_distance(new_breaches) > AC_FENCE_GIVE_UP_DISTANCE) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
} else if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
// if we are within 100m of the fence, RTL
}
} else {
// if more than 100m outside the fence just force a land
set_mode(LAND, MODE_REASON_FENCE_BREACH);
switch (fence_act) {
case AC_FENCE_ACTION_RTL_AND_LAND:
default:
// switch to RTL, if that fails then Land
if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
break;
case AC_FENCE_ACTION_ALWAYS_LAND:
// if always land option mode is specified, land
set_mode(LAND, MODE_REASON_FENCE_BREACH);
break;
case AC_FENCE_ACTION_SMART_RTL:
// Try SmartRTL, if that fails, RTL, if that fails Land
if (!set_mode(SMART_RTL, MODE_REASON_FENCE_BREACH)) {
if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
}
break;
case AC_FENCE_ACTION_BRAKE:
// Try Brake, if that fails Land
if (!set_mode(BRAKE, MODE_REASON_FENCE_BREACH)) {
set_mode(LAND, MODE_REASON_FENCE_BREACH);
}
break;
}
}
}
}