diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 69c5ad1e9d3..c3d021864fd 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -123,15 +123,19 @@ void ManualControl::processInput(hrt_abstime now) processStickArming(_selector.setpoint()); - // User override by stick: any axis velocity exceeds COM_RC_STICK_OV + // User override by stick const float dt_s = (now - _timestamp_last_loop) / 1e6f; const float velocity_threshold = _param_com_rc_stick_ov.get(); _selector.setpoint().sticks_moving = - (fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > velocity_threshold) - || (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > velocity_threshold) - || (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > velocity_threshold) - || (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > velocity_threshold); + (fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > velocity_threshold + && _roll_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE) + || (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > velocity_threshold + && _pitch_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE) + || (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > velocity_threshold + && _yaw_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE) + || (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > velocity_threshold + && _throttle_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE); _selector.setpoint().timestamp = now; _manual_control_setpoint_pub.publish(_selector.setpoint()); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 345e89ec6c0..e4d1a4c4a98 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -132,6 +132,8 @@ private: MovingDiff _yaw_diff{}; MovingDiff _throttle_diff{}; + static constexpr uint8_t MIN_SIGN_CONSECUTIVE = 3; + 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")}; diff --git a/src/modules/manual_control/MovingDiff.hpp b/src/modules/manual_control/MovingDiff.hpp index 7fa2b8c8bee..e776be9a802 100644 --- a/src/modules/manual_control/MovingDiff.hpp +++ b/src/modules/manual_control/MovingDiff.hpp @@ -51,19 +51,39 @@ public: if (PX4_ISFINITE(_last_value)) { const float new_diff = (value - _last_value) / dt_s; _difference_filter.update(new_diff); + + // Count consecutive same-sign raw-velocity samples (rejects random noise). + const int8_t s = (new_diff > 0.f) ? 1 : (new_diff < 0.f) ? -1 : 0; + + if (s != 0) { + if (s == _last_sign && _consecutive_same_sign < UINT8_MAX) { + _consecutive_same_sign++; + + } else { + _consecutive_same_sign = 1; + } + + _last_sign = s; + } } _last_value = value; return _difference_filter.getState(); } + uint8_t consecutiveSameSign() const { return _consecutive_same_sign; } + void reset() { _difference_filter.reset(0.f); _last_value = NAN; + _last_sign = 0; + _consecutive_same_sign = 0; } private: AlphaFilter _difference_filter; float _last_value{NAN}; + int8_t _last_sign{0}; + uint8_t _consecutive_same_sign{0}; };