mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-24 09:38:29 -04:00
APM: implement FENCE_ACTION_REPORT
when FENCE_ACTION is set to FENCE_ACTION_REPORT a fence breach is reported, but the flight mode is not changed
This commit is contained in:
parent
299711f4a9
commit
817017658d
@ -129,8 +129,8 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||||||
#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: Defines the distance from the waypoint center, the plane will maintain during a loiter
|
// @Description: What to do on fence breach
|
||||||
// @Values: 0:None,1:GuidedMode
|
// @Values: 0:None,1:GuidedMode,2:ReportOnly
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
GSCALAR(fence_action, "FENCE_ACTION", 0),
|
GSCALAR(fence_action, "FENCE_ACTION", 0),
|
||||||
|
|
||||||
|
@ -235,7 +235,8 @@ static void geofence_check(bool altitude_check_only)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// we are outside the fence
|
// we are outside the fence
|
||||||
if (geofence_state->fence_triggered && control_mode == GUIDED) {
|
if (geofence_state->fence_triggered &&
|
||||||
|
(control_mode == GUIDED || 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;
|
||||||
@ -256,6 +257,9 @@ static void geofence_check(bool altitude_check_only)
|
|||||||
|
|
||||||
// see what action the user wants
|
// see what action the user wants
|
||||||
switch (g.fence_action) {
|
switch (g.fence_action) {
|
||||||
|
case FENCE_ACTION_REPORT:
|
||||||
|
break;
|
||||||
|
|
||||||
case FENCE_ACTION_GUIDED:
|
case FENCE_ACTION_GUIDED:
|
||||||
// fly to the return point, with an altitude half way between
|
// fly to the return point, with an altitude half way between
|
||||||
// min and max
|
// min and max
|
||||||
|
Loading…
Reference in New Issue
Block a user