diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 64fb8d2546..2d49b21956 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -180,22 +180,17 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear() float FlightTaskAutoMapper::_getLandSpeed() { - float speed = 0; + float land_speed = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _constraints.speed_down); - if (_dist_to_ground > _param_mpc_land_alt1.get()) { - speed = _constraints.speed_down; - - } else { - const float land_speed = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_speed.get(), _constraints.speed_down); - - // user input assisted land speed - if (_param_mpc_land_rc_help.get()) { - _sticks.evaluateSticks(_time_stamp_current); - speed = land_speed + _sticks.getPositionExpo()(2) * land_speed; - } + // user input assisted land speed + if (_param_mpc_land_rc_help.get() + && (_dist_to_ground < _param_mpc_land_alt1.get()) + && _sticks.evaluateSticks(_time_stamp_current)) { + // stick full up -1 -> stop, stick full down 1 -> double the speed + land_speed *= (1 + _sticks.getPositionExpo()(2)); } - return speed; + return land_speed; } diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp index f4f1931571..d45363789b 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) {}; -void Sticks::evaluateSticks(hrt_abstime now) +bool Sticks::evaluateSticks(hrt_abstime now) { _sub_manual_control_setpoint.update(); @@ -80,6 +80,8 @@ void Sticks::evaluateSticks(hrt_abstime now) _positions.zero(); _positions_expo.zero(); } + + return _input_available; } void Sticks::applyGearSwitch(landing_gear_s &gear) diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp index 693f66affd..6a7dad32c1 100644 --- a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp @@ -53,7 +53,7 @@ public: Sticks(ModuleParams *parent); ~Sticks() = default; - void evaluateSticks(hrt_abstime now); ///< checks and sets stick inputs + bool 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; }; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 101e531268..f46cda4053 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -359,8 +359,11 @@ PARAM_DEFINE_FLOAT(MPC_LAND_VEL_XY, 10.0f); /** * Enable user assisted descent speed for autonomous land routine. - * When enabled, descent speed will be equal to MPC_LAND_SPEED at half throttle, - * MPC_Z_VEL_MAX_DN at zero throttle, and 0.5 * MPC_LAND_SPEED at full throttle. + * + * When enabled, descent speed will be: + * stick full up - 0 + * stick centered - MPC_LAND_SPEED + * stick full down - 2 * MPC_LAND_SPEED * * @min 0 * @max 1