diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index bfe734ac74..2b2bee7b54 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -50,10 +50,11 @@ bool FlightTaskManualAltitude::updateInitialize() { bool ret = FlightTask::updateInitialize(); - const bool sticks_available = _sticks.evaluateSticks(_time_stamp_current, _gear); + _sticks.evaluateSticks(_time_stamp_current); + _sticks.applyGearSwitch(_gear); if (_sticks_data_required) { - ret = ret && sticks_available; + ret = ret && _sticks.isAvailable(); } // in addition to manual require valid position and velocity in D-direction and valid yaw diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp index df368bf6a1..f4f1931571 100644 --- a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp @@ -43,7 +43,7 @@ Sticks::Sticks(ModuleParams *parent) : ModuleParams(parent) {}; -bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear) +void Sticks::evaluateSticks(hrt_abstime now) { _sub_manual_control_setpoint.update(); @@ -51,7 +51,6 @@ bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear) // Sticks are rescaled linearly and exponentially to [-1,1] if ((now - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) { - // Linear scale _positions(0) = _sub_manual_control_setpoint.get().x; // NED x, pitch [-1,1] _positions(1) = _sub_manual_control_setpoint.get().y; // NED y, roll [-1,1] @@ -64,41 +63,46 @@ bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear) _positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get()); _positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get()); - // Only switch the landing gear up if the user switched from gear down to gear up. - // If the user had the switch in the gear up position and took off ignore it - // until he toggles the switch to avoid retracting the gear immediately on takeoff. - int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch; - - if (_gear_switch_old != gear_switch) { - applyGearSwitch(gear_switch, gear); - } - - _gear_switch_old = gear_switch; - // valid stick inputs are required const bool valid_sticks = PX4_ISFINITE(_positions(0)) && PX4_ISFINITE(_positions(1)) && PX4_ISFINITE(_positions(2)) && PX4_ISFINITE(_positions(3)); - return valid_sticks; + _input_available = valid_sticks; } else { + _input_available = false; + } + + if (!_input_available) { // Timeout: set all sticks to zero _positions.zero(); _positions_expo.zero(); - gear.landing_gear = landing_gear_s::GEAR_KEEP; - return false; } } -void Sticks::applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear) +void Sticks::applyGearSwitch(landing_gear_s &gear) { - if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { - gear.landing_gear = landing_gear_s::GEAR_DOWN; - } + // Only switch the landing gear up if the user switched from gear down to gear up. + // If the user had the switch in the gear up position and took off ignore it + // until he toggles the switch to avoid retracting the gear immediately on takeoff. + if (!isAvailable()) { + gear.landing_gear = landing_gear_s::GEAR_KEEP; - if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - gear.landing_gear = landing_gear_s::GEAR_UP; + } else { + const int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch; + + if (_gear_switch_old != gear_switch) { + if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + gear.landing_gear = landing_gear_s::GEAR_DOWN; + } + + if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + gear.landing_gear = landing_gear_s::GEAR_UP; + } + } + + _gear_switch_old = gear_switch; } } diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp index 28a878e55b..693f66affd 100644 --- a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp @@ -53,11 +53,14 @@ public: Sticks(ModuleParams *parent); ~Sticks() = default; - bool evaluateSticks(hrt_abstime now, landing_gear_s &gear); ///< checks and sets stick inputs - void applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear); ///< Sets gears according to switch + void evaluateSticks(hrt_abstime now); ///< checks and sets stick inputs + void applyGearSwitch(landing_gear_s &gear); ///< Sets gears according to switch + bool isAvailable() { return _input_available; }; const matrix::Vector &getPosition() { return _positions; }; const matrix::Vector &getPositionExpo() { return _positions_expo; }; + private: + bool _input_available = false; matrix::Vector _positions; ///< unmodified manual stick inputs matrix::Vector _positions_expo; ///< modified manual sticks using expo function int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; ///< old switch state