Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-11 07:18:45 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

feat(manual_control): gate RC override with sign-consistency check

Browse Source
This commit is contained in:
gguidone
2026-04-28 13:15:09 +02:00
parent 3d1bb72e10
commit ca9687cb3f
3 changed files with 31 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/manual_control/ManualControl.cpp
+9 -5
View File
@@ -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());
src/modules/manual_control/ManualControl.hpp
+2
View File
@@ -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")};
src/modules/manual_control/MovingDiff.hpp
+20
View File
@@ -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<float> _difference_filter;
float _last_value{NAN};
int8_t _last_sign{0};
uint8_t _consecutive_same_sign{0};
};