diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp index f5a9b64060..eb65d95598 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp @@ -33,8 +33,10 @@ bool FlightTask::updateInitialize() _sub_vehicle_local_position.update(); _sub_attitude.update(); + _sub_home_position.update(); _evaluateVehicleLocalPosition(); + _evaluateDistanceToGround(); _checkEkfResetCounters(); return true; } @@ -160,6 +162,19 @@ void FlightTask::_evaluateVehicleLocalPosition() } } +void FlightTask::_evaluateDistanceToGround() +{ + _dist_to_ground = NAN; + + // if there is a valid distance to bottom or vertical distance to home + if (PX4_ISFINITE(_dist_to_bottom)) { + _dist_to_ground = _dist_to_bottom; + + } else if (_sub_home_position.get().valid_alt) { + _dist_to_ground = -(_position(2) - _sub_home_position.get().z); + } +} + void FlightTask::_setDefaultConstraints() { _constraints.speed_xy = _param_mpc_xy_vel_max.get(); diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp index 65a02c0228..9ff10180ab 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include class FlightTask : public ModuleParams @@ -170,6 +171,7 @@ protected: uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; uORB::SubscriptionData _sub_attitude{ORB_ID(vehicle_attitude)}; + uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; /** Reset all setpoints to NAN */ void _resetSetpoints(); @@ -177,6 +179,8 @@ protected: /** Check and update local position */ void _evaluateVehicleLocalPosition(); + void _evaluateDistanceToGround(); + /** Set constraints to default values */ virtual void _setDefaultConstraints(); @@ -208,6 +212,7 @@ protected: matrix::Vector3f _velocity; /**< current vehicle velocity */ float _yaw = 0.f; /**< current vehicle yaw heading */ float _dist_to_bottom = 0.0f; /**< current height above ground level */ + float _dist_to_ground = 0.f; /**< equals _dist_to_bottom if valid, height above home otherwise */ /** * Setpoints which the position controller has to execute. diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index ef29b2f37b..cd30f459d5 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -45,8 +45,6 @@ bool FlightTaskManualAltitude::updateInitialize() { bool ret = FlightTaskManual::updateInitialize(); - _sub_home_position.update(); - // in addition to manual require valid position and velocity in D-direction and valid yaw return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw); } @@ -267,22 +265,12 @@ void FlightTaskManualAltitude::_respectMaxAltitude() void FlightTaskManualAltitude::_respectGroundSlowdown() { - float dist_to_ground = NAN; - - // if there is a valid distance to bottom or vertical distance to home - if (PX4_ISFINITE(_dist_to_bottom)) { - dist_to_ground = _dist_to_bottom; - - } else if (_sub_home_position.get().valid_alt) { - dist_to_ground = -(_position(2) - _sub_home_position.get().z); - } - // limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2 - if (PX4_ISFINITE(dist_to_ground)) { - const float limit_down = math::gradual(dist_to_ground, + if (PX4_ISFINITE(_dist_to_ground)) { + const float limit_down = math::gradual(_dist_to_ground, _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), _param_mpc_land_speed.get(), _constraints.speed_down); - const float limit_up = math::gradual(dist_to_ground, + const float limit_up = math::gradual(_dist_to_ground, _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up); _velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down); diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index da2f41a085..512c181117 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -40,7 +40,6 @@ #pragma once #include "FlightTaskManual.hpp" -#include class FlightTaskManualAltitude : public FlightTaskManual { @@ -123,8 +122,6 @@ private: */ void _respectGroundSlowdown(); - uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; - float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */ uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ float _max_speed_up = 10.0f; diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 8a51fe29e9..550a4d465b 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -113,6 +113,8 @@ void FlightTaskManualPosition::_scaleSticks() } } + _velocity_scale = fminf(_computeVelXYGroundDist(), _velocity_scale); + // scale velocity to its maximum limits Vector2f vel_sp_xy = stick_xy * _velocity_scale; @@ -129,6 +131,20 @@ void FlightTaskManualPosition::_scaleSticks() _velocity_setpoint(1) = vel_sp_xy(1); } +float FlightTaskManualPosition::_computeVelXYGroundDist() +{ + float max_vel_xy = _constraints.speed_xy; + + // limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2 + if (PX4_ISFINITE(_dist_to_ground)) { + max_vel_xy = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_vel_xy.get(), _constraints.speed_xy); + } + + return max_vel_xy; +} + void FlightTaskManualPosition::_updateXYlock() { /* If position lock is not active, position setpoint is set to NAN.*/ diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 9e9e3dde22..9ac3164687 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -65,11 +65,13 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude, (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_land_vel_xy, (ParamFloat) _param_mpc_acc_hor_max, (ParamFloat) _param_mpc_hold_max_xy, (ParamFloat) _param_mpc_acc_hor_estm ) private: + float _computeVelXYGroundDist(); float _velocity_scale{0.0f}; //scales the stick input to velocity uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ 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 92ba63de3f..6b237ca115 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -350,6 +350,16 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); +/** + * Maximum horizontal velocity during landing + * + * @unit m/s + * @min 0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_VEL_XY, 2.f); + /** * Enable user assisted descent speed for autonomous land routine. * When enabled, descent speed will be equal to MPC_LAND_SPEED at half throttle, @@ -684,7 +694,9 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f); * * Below this altitude descending velocity gets limited * to a value between "MPC_Z_VEL_MAX" and "MPC_LAND_SPEED" - * to enable a smooth descent experience + * to enable a smooth descent experience. + * The horizontal velocity also gets limited to a value + * between "MPC_VEL_MANUAL" and "MPC_LAND_VEL_XY" * Value needs to be higher than "MPC_LAND_ALT2" * * @unit m @@ -698,7 +710,8 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f); /** * Altitude for 2. step of slow landing (landing) * - * Below this altitude descending velocity gets limited to "MPC_LAND_SPEED" + * Below this altitude descending and horizontal velocities get + * limited to "MPC_LAND_SPEED" and "MPC_LAND_VEL_XY", respectively. * Value needs to be lower than "MPC_LAND_ALT1" * * @unit m