mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 12:43:33 +08:00
feat(manual_control): replace stick override threshold with double-flick gesture
RC override during autonomous missions now requires a deliberate roll double-flick (right→left or left→right) within 0.3s instead of a single instantaneous stick velocity threshold. A static deflection or an accidental single bump can no longer trigger the override. COM_RC_STICK_OV is reused as the deflection threshold for each flick; no new parameters are introduced.
This commit is contained in:
@@ -123,14 +123,52 @@ void ManualControl::processInput(hrt_abstime now)
|
|||||||
|
|
||||||
processStickArming(_selector.setpoint());
|
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 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;
|
||||||
|
|
||||||
_selector.setpoint().sticks_moving = (fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > minimum_stick_change)
|
switch (_flick_state) {
|
||||||
|| (fabsf(_pitch_diff.update(_selector.setpoint().pitch, dt_s)) > minimum_stick_change)
|
case FlickState::Idle:
|
||||||
|| (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > minimum_stick_change)
|
|
||||||
|| (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > minimum_stick_change);
|
// 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 = gesture_fired;
|
||||||
|
|
||||||
_selector.setpoint().timestamp = now;
|
_selector.setpoint().timestamp = now;
|
||||||
_manual_control_setpoint_pub.publish(_selector.setpoint());
|
_manual_control_setpoint_pub.publish(_selector.setpoint());
|
||||||
@@ -160,9 +198,8 @@ void ManualControl::processInput(hrt_abstime now)
|
|||||||
}
|
}
|
||||||
|
|
||||||
_roll_diff.reset();
|
_roll_diff.reset();
|
||||||
_pitch_diff.reset();
|
_flick_state = FlickState::Idle;
|
||||||
_yaw_diff.reset();
|
_flick_timestamp = 0;
|
||||||
_throttle_diff.reset();
|
|
||||||
_stick_arm_hysteresis.set_state_and_update(false, now);
|
_stick_arm_hysteresis.set_state_and_update(false, now);
|
||||||
_stick_disarm_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);
|
_stick_kill_hysteresis.set_state_and_update(false, now);
|
||||||
|
|||||||
@@ -128,9 +128,16 @@ private:
|
|||||||
systemlib::Hysteresis _button_arm_hysteresis{false};
|
systemlib::Hysteresis _button_arm_hysteresis{false};
|
||||||
|
|
||||||
MovingDiff _roll_diff{};
|
MovingDiff _roll_diff{};
|
||||||
MovingDiff _pitch_diff{};
|
|
||||||
MovingDiff _yaw_diff{};
|
// Double-flick RC override gesture state
|
||||||
MovingDiff _throttle_diff{};
|
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_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")};
|
||||||
|
|||||||
Reference in New Issue
Block a user