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

Geofence - Do not trigger geofence failsafe while in low battery

Browse Source 
failsafe action.
Also move geofence flags from static variables declared into the if
statement to private members of the class.
This commit is contained in:
bresch
2019-05-23 15:14:50 +02:00
committed by Julian Oes
parent a707403eaf
commit 427b2e6636
2 changed files with 34 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+28 -23
View File
@@ -156,7 +156,6 @@ static uint8_t _last_sp_man_arm_switch = 0;
static struct vtol_vehicle_status_s vtol_status = {};
static struct cpuload_s cpuload = {};
static bool warning_action_on = false;
static bool last_overload = false;
static struct vehicle_status_flags_s status_flags = {};
@@ -1738,7 +1737,7 @@ Commander::run()
}
if (!warning_action_on) {
if (!_warning_action_on) {
// store the last good main_state when not in an navigation
// hold state
main_state_before_rtl = internal_state.main_state;
@@ -1747,7 +1746,7 @@ Commander::run()
&& internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER
&& internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
// reset flag again when we switched out of it
warning_action_on = false;
_warning_action_on = false;
}
orb_check(cpuload_sub, &updated);
@@ -1834,19 +1833,18 @@ Commander::run()
}
// Geofence actions
if (armed.armed && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)) {
static bool geofence_loiter_on = false;
static bool geofence_rtl_on = false;
// Geofence failsafe is disabled when in a low battery handling action (critical or emergency)
if (armed.armed
&& (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) {
static hrt_abstime last_geofence_violation = 0;
const hrt_abstime geofence_violation_action_interval = 10_s;
if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) {
if (hrt_elapsed_time(&_last_geofence_violation) > geofence_violation_action_interval) {
last_geofence_violation = hrt_absolute_time();
_last_geofence_violation = hrt_absolute_time();
switch (geofence_result.geofence_action) {
case (geofence_result_s::GF_ACTION_NONE) : {
@@ -1862,7 +1860,7 @@ Commander::run()
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;
_geofence_loiter_on = true;
}
break;
@@ -1871,7 +1869,7 @@ Commander::run()
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;
_geofence_rtl_on = true;
}
break;
@@ -1889,23 +1887,30 @@ Commander::run()
}
// 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)
&& (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF
|| sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_NONE);
_geofence_loiter_on = _geofence_loiter_on
&& (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)
&& (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF
|| sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_NONE);
// reset if no longer in RTL or if manually switched to RTL
geofence_rtl_on = geofence_rtl_on
&& (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL)
&& (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF
|| sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_NONE);
_geofence_rtl_on = _geofence_rtl_on
&& (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL)
&& (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF
|| sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_NONE);
warning_action_on = warning_action_on || (geofence_loiter_on || geofence_rtl_on);
_warning_action_on = _warning_action_on || (_geofence_loiter_on || _geofence_rtl_on);
} else {
// No geofence checks, reset flags
_geofence_loiter_on = false;
_geofence_rtl_on = false;
_warning_action_on = false;
_last_geofence_violation = 0;
}
// revert geofence failsafe transition if sticks are moved and we were previously in a manual mode
// but only if not in a low battery handling action
if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) && (warning_action_on &&
if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) && (_warning_action_on &&
(main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL ||
main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL ||
main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL ||
@@ -2721,7 +2726,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
// if the system now later enters an autonomous state the pilot can move
// the sticks to break out of the autonomous state
if (!warning_action_on
if (!_warning_action_on
&& (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL ||