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

commander: remove manual mode before geofence handling

Browse Source
This commit is contained in:
Matthias Grob
2019-12-06 15:23:21 +01:00
committed by Daniel Agar
parent 34121e655b
commit de95e3d274
1 changed files with 6 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+6 -21
View File
@@ -141,8 +141,6 @@ static struct safety_s safety = {};
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM];
static struct commander_state_s internal_state = {};
static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX;
static manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint
static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch
static uint8_t _last_sp_man_arm_switch = 0;
@@ -1773,15 +1771,10 @@ Commander::run()
_auto_disarm_killed.set_state_and_update(false, hrt_absolute_time());
}
if (!_geofence_warning_action_on) {
// store the last good main_state when not in an navigation
// hold state
main_state_before_rtl = internal_state.main_state;
} else if (internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
&& internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER
&& internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
if (_geofence_warning_action_on
&& internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
&& 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
_geofence_warning_action_on = false;
}
@@ -1938,23 +1931,15 @@ Commander::run()
// abort auto mode or geofence reaction if sticks are moved significantly
// but only if not in a low battery handling action
const bool not_in_low_battery_reaction = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL;
const bool low_battery_reaction = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL;
const bool is_rotary_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
const bool manual_mode_before_geofence =
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 ||
main_state_before_rtl == commander_state_s::MAIN_STATE_ACRO ||
main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE ||
main_state_before_rtl == commander_state_s::MAIN_STATE_STAB;
const bool in_auto_mode =
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
if (rc_override != 0 && is_rotary_wing && not_in_low_battery_reaction
&& (in_auto_mode || (_geofence_warning_action_on && manual_mode_before_geofence))) {
if (rc_override != 0 && is_rotary_wing && !low_battery_reaction && !_geofence_warning_action_on && in_auto_mode) {
// transition to previous state if sticks are touched
if ((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) ||