diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index f7c0e9da9541..0fb3698107f1 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -123,14 +123,52 @@ void ManualControl::processInput(hrt_abstime now) processStickArming(_selector.setpoint()); - // User override by stick + // User override by stick: double-flick gesture (roll right then left, or left then right) const float dt_s = (now - _timestamp_last_loop) / 1e6f; - const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); + const float flick_threshold = 0.01f * _param_com_rc_stick_ov.get(); + const float roll = _selector.setpoint().roll; + const float roll_rate = _roll_diff.update(roll, dt_s); + bool gesture_fired = false; + + switch (_flick_state) { + case FlickState::Idle: + + // Require active velocity so a statically held or resting stick cannot arm + if (roll > flick_threshold && roll_rate > flick_threshold) { + _flick_state = FlickState::FirstFlickRight; + _flick_timestamp = now; + + } else if (roll < -flick_threshold && roll_rate < -flick_threshold) { + _flick_state = FlickState::FirstFlickLeft; + _flick_timestamp = now; + } + + break; + + case FlickState::FirstFlickRight: + if ((now - _flick_timestamp) > (hrt_abstime)(FLICK_WINDOW_S * 1e6f)) { + _flick_state = FlickState::Idle; + + } else if (roll < -flick_threshold && roll_rate < -flick_threshold) { + gesture_fired = true; + _flick_state = FlickState::Idle; + } + + break; + + case FlickState::FirstFlickLeft: + if ((now - _flick_timestamp) > (hrt_abstime)(FLICK_WINDOW_S * 1e6f)) { + _flick_state = FlickState::Idle; + + } else if (roll > flick_threshold && roll_rate > flick_threshold) { + gesture_fired = true; + _flick_state = FlickState::Idle; + } + + break; + } - _selector.setpoint().sticks_moving = (fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > minimum_stick_change) - || (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > minimum_stick_change) - || (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > minimum_stick_change) - || (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > minimum_stick_change); + _selector.setpoint().sticks_moving = gesture_fired; _selector.setpoint().timestamp = now; _manual_control_setpoint_pub.publish(_selector.setpoint()); @@ -160,9 +198,8 @@ void ManualControl::processInput(hrt_abstime now) } _roll_diff.reset(); - _pitch_diff.reset(); - _yaw_diff.reset(); - _throttle_diff.reset(); + _flick_state = FlickState::Idle; + _flick_timestamp = 0; _stick_arm_hysteresis.set_state_and_update(false, now); _stick_disarm_hysteresis.set_state_and_update(false, now); _stick_kill_hysteresis.set_state_and_update(false, now); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 345e89ec6c0b..e70b90e5a871 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -128,9 +128,16 @@ class ManualControl : public ModuleBase, public ModuleParams, public px4::Schedu systemlib::Hysteresis _button_arm_hysteresis{false}; MovingDiff _roll_diff{}; - MovingDiff _pitch_diff{}; - MovingDiff _yaw_diff{}; - MovingDiff _throttle_diff{}; + + // Double-flick RC override gesture state + enum class FlickState : uint8_t { + Idle, + FirstFlickRight, + FirstFlickLeft, + }; + FlickState _flick_state{FlickState::Idle}; + hrt_abstime _flick_timestamp{0}; + static constexpr float FLICK_WINDOW_S = 0.5f; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};