fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: pure refactor of RC override conditions

Browse Source
This commit is contained in:
Matthias Grob
2019-07-22 09:57:37 +02:00
parent 84a0d16386
commit 5c1ab06343
1 changed files with 15 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+15 -30
View File
@@ -1822,17 +1822,23 @@ Commander::run()
_geofence_violated_prev = false;
}
// revert geofence failsafe transition if sticks are moved and we were previously in a manual mode
// abort auto mode or geofence reaction if sticks are moved significantly
// but only if not in a low battery handling action
if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL)
&& (_geofence_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 ||
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 not_in_low_battery_reaction = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL;
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_MISSION ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
if (rc_override != 0 && not_in_low_battery_reaction
&& (in_auto_mode || (_geofence_warning_action_on && manual_mode_before_geofence))) {
// 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) ||
@@ -1846,27 +1852,6 @@ Commander::run()
}
}
// abort landing or auto or loiter if sticks are moved significantly
// but only if not in a low battery handling action
if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) &&
(internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)) {
// 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) ||
(fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) ||
(fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) ||
(fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) {
// revert to position control in any case
main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state);
mavlink_log_critical(&mavlink_log_pub, "Autopilot off! Returning control to pilot");
}
}
/* Check for mission flight termination */
if (armed.armed && _mission_result_sub.get().flight_termination &&
!status_flags.circuit_breaker_flight_termination_disabled) {