fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 17:17:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Geofence - Change from "re-engage geofence failsafe every 10s when

Browse Source 
outside the fence" to "engage geofence failsafe on transition". This
way, the pilot is not stucked in the non flight zone and can switch to
a manual mode and fly back or trigger RTL.
This commit is contained in:
bresch
2019-05-23 15:39:15 +02:00
committed by Julian Oes
parent 427b2e6636
commit f8f967f073
2 changed files with 34 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+33 -37
View File
@@ -1838,54 +1838,50 @@ Commander::run()
&& (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)
&& (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL)) {
// check for geofence violation
if (geofence_result.geofence_violated) {
const hrt_abstime geofence_violation_action_interval = 10_s;
// check for geofence violation transition
if (geofence_result.geofence_violated && !_geofence_violated_prev) {
if (hrt_elapsed_time(&_last_geofence_violation) > geofence_violation_action_interval) {
switch (geofence_result.geofence_action) {
case (geofence_result_s::GF_ACTION_NONE) : {
// do nothing
break;
}
_last_geofence_violation = hrt_absolute_time();
case (geofence_result_s::GF_ACTION_WARN) : {
// do nothing, mavlink critical messages are sent by navigator
break;
}
switch (geofence_result.geofence_action) {
case (geofence_result_s::GF_ACTION_NONE) : {
// do nothing
break;
case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
&internal_state)) {
_geofence_loiter_on = true;
}
case (geofence_result_s::GF_ACTION_WARN) : {
// do nothing, mavlink critical messages are sent by navigator
break;
break;
}
case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
&internal_state)) {
_geofence_rtl_on = true;
}
case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
&internal_state)) {
_geofence_loiter_on = true;
}
break;
}
break;
}
case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
&internal_state)) {
_geofence_rtl_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_TERMINATE) : {
warnx("Flight termination because of geofence");
mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated");
armed.force_failsafe = true;
status_changed = true;
break;
}
case (geofence_result_s::GF_ACTION_TERMINATE) : {
warnx("Flight termination because of geofence");
mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated");
armed.force_failsafe = true;
status_changed = true;
break;
}
}
}
_geofence_violated_prev = geofence_result.geofence_violated;
// reset if no longer in LOITER or if manually switched to LOITER
_geofence_loiter_on = _geofence_loiter_on
&& (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)
@@ -1905,7 +1901,7 @@ Commander::run()
_geofence_loiter_on = false;
_geofence_rtl_on = false;
_warning_action_on = false;
_last_geofence_violation = 0;
_geofence_violated_prev = false;
}
// revert geofence failsafe transition if sticks are moved and we were previously in a manual mode