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

commander: negate main_state_rc entering condition

Browse Source 
pure refactor using De Morgan's law to make the condition more intuitive
since you think about when should I enter and not when should I skip
This commit is contained in:
Matthias Grob
2019-04-25 20:45:56 +01:00
committed by Lorenz Meier
parent 1f3ebd98b1
commit f177a68da9
1 changed files with 18 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+18 -15
View File
@@ -2731,22 +2731,25 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
// feature, just in case offboard control goes crazy.
const bool position_got_valid = !_last_condition_global_position_valid && status_flags.condition_global_position_valid;
const bool had_rc_before = _last_sp_man.timestamp != 0;
const bool rc_values_not_updated = _last_sp_man.timestamp == sp_man.timestamp;
const bool all_switches_stayed =
(_last_sp_man.offboard_switch == sp_man.offboard_switch)
&& (_last_sp_man.return_switch == sp_man.return_switch)
&& (_last_sp_man.mode_switch == sp_man.mode_switch)
&& (_last_sp_man.acro_switch == sp_man.acro_switch)
&& (_last_sp_man.rattitude_switch == sp_man.rattitude_switch)
&& (_last_sp_man.posctl_switch == sp_man.posctl_switch)
&& (_last_sp_man.loiter_switch == sp_man.loiter_switch)
&& (_last_sp_man.mode_slot == sp_man.mode_slot)
&& (_last_sp_man.stab_switch == sp_man.stab_switch)
&& (_last_sp_man.man_switch == sp_man.man_switch);
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)
|| (_last_sp_man.mode_switch != sp_man.mode_switch)
|| (_last_sp_man.acro_switch != sp_man.acro_switch)
|| (_last_sp_man.rattitude_switch != sp_man.rattitude_switch)
|| (_last_sp_man.posctl_switch != sp_man.posctl_switch)
|| (_last_sp_man.loiter_switch != sp_man.loiter_switch)
|| (_last_sp_man.mode_slot != sp_man.mode_slot)
|| (_last_sp_man.stab_switch != sp_man.stab_switch)
|| (_last_sp_man.man_switch != sp_man.man_switch);
/* manual setpoint has not updated, do not re-evaluate it */
if (!position_got_valid && ((had_rc_before && rc_values_not_updated) || all_switches_stayed)) {
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
const bool should_evaluate_rc_mode_switch = position_got_valid || ((first_time_rc || rc_values_updated)
&& some_switch_changed);
if (!should_evaluate_rc_mode_switch) {
// store the last manual control setpoint set by the pilot in a manual state
// if the system now later enters an autonomous state the pilot can move