Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Fence breaches can go to RTL mode with new FENCE_ACTION setting.

This commit is contained in:
Michael Day 2016-06-01 09:05:16 -04:00 committed by Andrew Tridgell
parent 7d824247af
commit cdeef9d692
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/Parameters.cpp
View File

@ -410,8 +410,8 @@ const AP_Param::Info Plane::var_info[] = {
#if GEOFENCE_ENABLED == ENABLED #if GEOFENCE_ENABLED == ENABLED
// @Param: FENCE_ACTION // @Param: FENCE_ACTION
// @DisplayName: Action on geofence breach // @DisplayName: Action on geofence breach
// @Description: What to do on fence breach. If this is set to 0 then no action is taken, and geofencing is disabled. If this is set to 1 then the plane will enter GUIDED mode, with the target waypoint as the fence return point. If this is set to 2 then the fence breach is reported to the ground station, but no other action is taken. If set to 3 then the plane enters guided mode but the pilot retains manual throttle control. // @Description: What to do on fence breach. If this is set to 0 then no action is taken, and geofencing is disabled. If this is set to 1 then the plane will enter GUIDED mode, with the target waypoint as the fence return point. If this is set to 2 then the fence breach is reported to the ground station, but no other action is taken. If set to 3 then the plane enters guided mode but the pilot retains manual throttle control. If set to 4 the plane enters RTL mode, with the target waypoint as the closest rally point (or home point if there are no rally points).
// @Values: 0:None,1:GuidedMode,2:ReportOnly,3:GuidedModeThrPass // @Values: 0:None,1:GuidedMode,2:ReportOnly,3:GuidedModeThrPass,4:RTL_Mode
// @User: Standard // @User: Standard
GSCALAR(fence_action, "FENCE_ACTION", 0), GSCALAR(fence_action, "FENCE_ACTION", 0),

20
ArduPlane/geofence.cpp
View File

@ -284,7 +284,7 @@ void Plane::geofence_check(bool altitude_check_only)
// switch back to the chosen control mode if still in // switch back to the chosen control mode if still in
// GUIDED to the return point // GUIDED to the return point
if (geofence_state != NULL && if (geofence_state != NULL &&
(g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) && (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) &&
control_mode == GUIDED && control_mode == GUIDED &&
geofence_present() && geofence_present() &&
geofence_state->boundary_uptodate && geofence_state->boundary_uptodate &&
@ -348,7 +348,7 @@ void Plane::geofence_check(bool altitude_check_only)
// we are outside the fence // we are outside the fence
if (geofence_state->fence_triggered && if (geofence_state->fence_triggered &&
(control_mode == GUIDED || g.fence_action == FENCE_ACTION_REPORT)) { (control_mode == GUIDED || control_mode == RTL || g.fence_action == FENCE_ACTION_REPORT)) {
// we have already triggered, don't trigger again until the // we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch // user disables/re-enables using the fence channel switch
return; return;
@ -375,13 +375,18 @@ void Plane::geofence_check(bool altitude_check_only)
case FENCE_ACTION_GUIDED: case FENCE_ACTION_GUIDED:
case FENCE_ACTION_GUIDED_THR_PASS: case FENCE_ACTION_GUIDED_THR_PASS:
case FENCE_ACTION_RTL:
// make sure we don't auto trim the surfaces on this mode change // make sure we don't auto trim the surfaces on this mode change
int8_t saved_auto_trim = g.auto_trim; int8_t saved_auto_trim = g.auto_trim;
g.auto_trim.set(0); g.auto_trim.set(0);
set_mode(GUIDED); if (g.fence_action == FENCE_ACTION_RTL) {
set_mode(RTL);
} else {
set_mode(GUIDED);
}
g.auto_trim.set(saved_auto_trim); g.auto_trim.set(saved_auto_trim);
if (g.fence_ret_rally != 0) { //return to a rally point if (g.fence_ret_rally != 0 || g.fence_action == FENCE_ACTION_RTL) { //return to a rally point
guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
} else { //return to fence return point, not a rally point } else { //return to fence return point, not a rally point
@ -404,9 +409,10 @@ void Plane::geofence_check(bool altitude_check_only)
geofence_state->guided_lng = guided_WP_loc.lng; geofence_state->guided_lng = guided_WP_loc.lng;
geofence_state->old_switch_position = oldSwitchPosition; geofence_state->old_switch_position = oldSwitchPosition;
setup_terrain_target_alt(guided_WP_loc); if (g.fence_action != FENCE_ACTION_RTL) { //not needed for RTL mode
setup_terrain_target_alt(guided_WP_loc);
set_guided_WP(); set_guided_WP();
}
if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) { if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) {
guided_throttle_passthru = true; guided_throttle_passthru = true;