38 #include <mathlib/mathlib.h> 49 _sub_manual_control_setpoint.update();
51 const bool sticks_available = _evaluateSticks();
53 if (_sticks_data_required) {
54 ret = ret && sticks_available;
62 hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
65 if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
68 _sticks(0) = _sub_manual_control_setpoint.get().x;
69 _sticks(1) = _sub_manual_control_setpoint.get().y;
70 _sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.
f;
71 _sticks(3) = _sub_manual_control_setpoint.get().r;
74 _sticks_expo(0) =
math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
75 _sticks_expo(1) =
math::expo_deadzone(_sticks(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
76 _sticks_expo(2) =
math::expo_deadzone(_sticks(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
77 _sticks_expo(3) =
math::expo_deadzone(_sticks(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
82 int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
84 if (_gear_switch_old != gear_switch) {
85 _applyGearSwitch(gear_switch);
88 _gear_switch_old = gear_switch;
91 const bool valid_sticks = PX4_ISFINITE(_sticks(0))
92 && PX4_ISFINITE(_sticks(1))
93 && PX4_ISFINITE(_sticks(2))
94 && PX4_ISFINITE(_sticks(3));
102 _gear.landing_gear = landing_gear_s::GEAR_KEEP;
109 if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
110 _gear.landing_gear = landing_gear_s::GEAR_DOWN;
113 if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
114 _gear.landing_gear = landing_gear_s::GEAR_UP;
bool _evaluateSticks()
checks and sets stick inputs
High-resolution timer with callouts and timekeeping.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
virtual bool updateInitialize()
Call before activate() or update() to initialize time and input data.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
void _applyGearSwitch(uint8_t gswitch)
Sets gears according to switch.
const T expo_deadzone(const T &value, const T &e, const T &dz)
bool updateInitialize() override
Call before activate() or update() to initialize time and input data.
Linear and exponential map from stick inputs to range -1 and 1.