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

commander: limit mode switch re-evaluation logic on estimate validity changes

Browse Source 
- re-evaluating mode switches on altitude or position validity change can create confusing when also relying on mode changes via mavlink
 - limiting this logic to only apply when disarmed solves the original problem (respecting initial switch position) without introducing potential problems mid-flight
 - fixes https://github.com/PX4/PX4-Autopilot/issues/16235
This commit is contained in:
Daniel Agar
2020-11-25 21:14:55 -05:00
committed by GitHub
parent 3e4c38f6e8
commit ec9f8998a7
1 changed files with 49 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+49 -34
View File
@@ -1567,7 +1567,9 @@ Commander::run()
}
/* update safety topic */
if (_safety_sub.updated()) {
const bool safety_updated = _safety_sub.updated();
if (safety_updated) {
const bool previous_safety_off = _safety.safety_off;
if (_safety_sub.copy(&_safety)) {
@@ -1780,7 +1782,7 @@ Commander::run()
}
// update manual_control_setpoint before geofence (which might check sticks or switches)
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
const bool manual_control_setpoint_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint);
/* start geofence result check */
@@ -2075,23 +2077,13 @@ Commander::run()
tune_negative(true);
}
/* evaluate the main state machine according to mode switches */
bool first_rc_eval = (_last_manual_control_setpoint.timestamp == 0) && (_manual_control_setpoint.timestamp > 0);
transition_result_t main_res = set_main_state(status, &_status_changed);
/* store last position lock state */
_last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid;
_last_condition_local_position_valid = status_flags.condition_local_position_valid;
_last_condition_global_position_valid = status_flags.condition_global_position_valid;
/* play tune on mode change only if armed, blink LED always */
if (main_res == TRANSITION_CHANGED || first_rc_eval) {
tune_positive(armed.armed);
_status_changed = true;
} else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(&mavlink_log_pub, "Switching to this mode is currently not possible");
if (manual_control_setpoint_updated || safety_updated) {
// evaluate the main state machine according to mode switches
if (set_main_state(status, &_status_changed) == TRANSITION_CHANGED) {
// play tune on mode change only if armed, blink LED always
tune_positive(armed.armed);
_status_changed = true;
}
}
/* check throttle kill switch */
@@ -2493,6 +2485,11 @@ Commander::run()
_status_changed = false;
/* store last position lock state */
_last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid;
_last_condition_local_position_valid = status_flags.condition_local_position_valid;
_last_condition_global_position_valid = status_flags.condition_global_position_valid;
arm_auth_update(now, params_updated || param_init_forced);
px4_indicate_external_reset_lockout(LockoutComponent::Commander, armed.armed);
@@ -2705,19 +2702,19 @@ Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool
transition_result_t
Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed)
{
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
if ((_manual_control_setpoint.timestamp == 0)
|| (_manual_control_setpoint.timestamp == _last_manual_control_setpoint.timestamp)) {
// no manual control or no update -> nothing changed
return TRANSITION_NOT_CHANGED;
}
// Note: even if status_flags.offboard_control_set_by_command is set
// we want to allow rc mode change to take precidence. This is a safety
// feature, just in case offboard control goes crazy.
const bool altitude_got_valid = (!_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid);
const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid);
const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid);
const bool first_time_rc = (_last_manual_control_setpoint.timestamp == 0);
const bool rc_values_updated = (_last_manual_control_setpoint.timestamp != _manual_control_setpoint.timestamp);
const bool some_switch_changed =
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
bool should_evaluate_rc_mode_switch =
(_last_manual_control_setpoint.offboard_switch != _manual_control_setpoint.offboard_switch)
|| (_last_manual_control_setpoint.return_switch != _manual_control_setpoint.return_switch)
|| (_last_manual_control_setpoint.mode_switch != _manual_control_setpoint.mode_switch)
@@ -2729,12 +2726,27 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|| (_last_manual_control_setpoint.stab_switch != _manual_control_setpoint.stab_switch)
|| (_last_manual_control_setpoint.man_switch != _manual_control_setpoint.man_switch);
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
const bool should_evaluate_rc_mode_switch = first_time_rc
|| altitude_got_valid
|| lpos_got_valid
|| gpos_got_valid
|| (rc_values_updated && some_switch_changed);
if (status_local.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// if already armed don't evaluate first time RC
if (_last_manual_control_setpoint.timestamp == 0) {
should_evaluate_rc_mode_switch = false;
_last_manual_control_setpoint = _manual_control_setpoint;
}
} else {
// not armed
if (!should_evaluate_rc_mode_switch) {
// to respect initial switch position (eg POSCTL) force RC switch re-evaluation if estimates become valid
const bool altitude_got_valid = (!_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid);
const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid);
const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid);
if (altitude_got_valid || lpos_got_valid || gpos_got_valid) {
should_evaluate_rc_mode_switch = true;
}
}
}
if (!should_evaluate_rc_mode_switch) {
@@ -2742,7 +2754,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 (!_geofence_warning_action_on
if (!status.rc_signal_lost && !_geofence_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 ||
@@ -2767,6 +2779,9 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
// the desired mode
reset_posvel_validity(changed);
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
/* offboard switch overrides main switch */
if (_manual_control_setpoint.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state);