mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-28 19:32:36 +08:00
feat(manual_control): gate RC override with sign-consistency check
This commit is contained in:
@@ -123,15 +123,19 @@ void ManualControl::processInput(hrt_abstime now)
|
|||||||
|
|
||||||
processStickArming(_selector.setpoint());
|
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 dt_s = (now - _timestamp_last_loop) / 1e6f;
|
||||||
const float velocity_threshold = _param_com_rc_stick_ov.get();
|
const float velocity_threshold = _param_com_rc_stick_ov.get();
|
||||||
|
|
||||||
_selector.setpoint().sticks_moving =
|
_selector.setpoint().sticks_moving =
|
||||||
(fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > velocity_threshold)
|
(fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > velocity_threshold
|
||||||
|| (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > velocity_threshold)
|
&& _roll_diff.consecutiveSameSign() >= MIN_SIGN_CONSECUTIVE)
|
||||||
|| (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > velocity_threshold)
|
|| (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > velocity_threshold
|
||||||
|| (fabsf(_throttle_diff.update(_selector.setpoint().throttle, 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;
|
_selector.setpoint().timestamp = now;
|
||||||
_manual_control_setpoint_pub.publish(_selector.setpoint());
|
_manual_control_setpoint_pub.publish(_selector.setpoint());
|
||||||
|
|||||||
@@ -132,6 +132,8 @@ private:
|
|||||||
MovingDiff _yaw_diff{};
|
MovingDiff _yaw_diff{};
|
||||||
MovingDiff _throttle_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_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||||
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
|
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
|
||||||
|
|
||||||
|
|||||||
@@ -51,19 +51,39 @@ public:
|
|||||||
if (PX4_ISFINITE(_last_value)) {
|
if (PX4_ISFINITE(_last_value)) {
|
||||||
const float new_diff = (value - _last_value) / dt_s;
|
const float new_diff = (value - _last_value) / dt_s;
|
||||||
_difference_filter.update(new_diff);
|
_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;
|
_last_value = value;
|
||||||
return _difference_filter.getState();
|
return _difference_filter.getState();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint8_t consecutiveSameSign() const { return _consecutive_same_sign; }
|
||||||
|
|
||||||
void reset()
|
void reset()
|
||||||
{
|
{
|
||||||
_difference_filter.reset(0.f);
|
_difference_filter.reset(0.f);
|
||||||
_last_value = NAN;
|
_last_value = NAN;
|
||||||
|
_last_sign = 0;
|
||||||
|
_consecutive_same_sign = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
AlphaFilter<float> _difference_filter;
|
AlphaFilter<float> _difference_filter;
|
||||||
float _last_value{NAN};
|
float _last_value{NAN};
|
||||||
|
int8_t _last_sign{0};
|
||||||
|
uint8_t _consecutive_same_sign{0};
|
||||||
};
|
};
|
||||||
|
|||||||
Reference in New Issue
Block a user