Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 1 commit into from
Oct 14, 2019
+9 −5
Merged

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

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 9 additions & 5 deletions src/modules/commander/Commander.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,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 = {};
Expand Down Expand Up @@ -2024,6 +2025,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 */
Expand Down Expand Up @@ -2595,10 +2597,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)
Expand All @@ -2614,7 +2617,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) {
Expand Down