From f177a68da98b9c2a107b03a144bcc335a249bff5 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Thu, 25 Apr 2019 20:45:56 +0100
Subject: [PATCH] commander: negate main_state_rc entering condition
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
---
src/modules/commander/Commander.cpp | 35 ++++++++++++++++-------------
1 file changed, 19 insertions(+), 16 deletions(-)
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index f75b8dd407..e39641feb7 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -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);
-
- /* manual setpoint has not updated, do not re-evaluate it */
- if (!position_got_valid && ((had_rc_before && rc_values_not_updated) || all_switches_stayed)) {
+ 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);
+
+ // 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
--
GitLab