fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

WIP: commander re-evaluate RC mode switch when local position becomes valid

Browse Source
This commit is contained in:
Daniel Agar 2019-05-29 18:02:23 -04:00 committed by Matthias Grob
parent 093d3b76d9
commit 97efbde6f4
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/commander/Commander.cpp
View File

@ -159,6 +159,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was
static uint8_t arm_requirements = ARM_REQ_NONE;
static bool _last_condition_local_altitude_valid = false;
static bool _last_condition_local_position_valid = false;
static bool _last_condition_global_position_valid = false;
static struct vehicle_land_detected_s land_detector = {};
@ -2066,6 +2067,7 @@ Commander::run()
/* 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 */
@ -2650,10 +2652,11 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
// 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 position_got_valid = !_last_condition_global_position_valid && status_flags.condition_global_position_valid;
const bool first_time_rc = _last_sp_man.timestamp == 0;
const bool rc_values_updated = _last_sp_man.timestamp != sp_man.timestamp;
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_sp_man.timestamp == 0);
const bool rc_values_updated = (_last_sp_man.timestamp != sp_man.timestamp);
const bool some_switch_changed =
(_last_sp_man.offboard_switch != sp_man.offboard_switch)
|| (_last_sp_man.return_switch != sp_man.return_switch)
@ -2669,7 +2672,8 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
// 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
|| position_got_valid
|| lpos_got_valid
|| gpos_got_valid
|| (rc_values_updated && some_switch_changed);
if (!should_evaluate_rc_mode_switch) {