zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

sbg
Daniel Agar 6 years ago committed by Matthias Grob
parent
093d3b76d9
commit
97efbde6f4
1 changed files with 9 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 14
      src/modules/commander/Commander.cpp

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

@ -159,6 +159,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was @@ -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() @@ -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 @@ -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 @@ -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) {

Write Preview
Loading…
Cancel
Save