diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 839274716d..3acc4d03a3 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -227,9 +227,15 @@ void FlightTaskAuto::_prepareLandSetpoints() _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint // Slow down automatic descend close to ground - float land_speed = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_speed.get(), _param_mpc_z_v_auto_dn.get()); + float speed = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()); + + bool range_dist_available = PX4_ISFINITE(_dist_to_bottom); + + if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) { + speed = _param_mpc_land_crawl_speed.get(); + } if (_type_previous != WaypointType::land) { // initialize xy-position and yaw to waypoint such that home is reached exactly without user input @@ -241,7 +247,7 @@ void FlightTaskAuto::_prepareLandSetpoints() // User input assisted landing if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) { // Stick full up -1 -> stop, stick full down 1 -> double the speed - land_speed *= (1 + _sticks.getPositionExpo()(2)); + speed *= (1 + _sticks.getPositionExpo()(2)); _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime); @@ -263,7 +269,7 @@ void FlightTaskAuto::_prepareLandSetpoints() _position_setpoint = _land_position; // The last element of the land position has to stay NAN _yaw_setpoint = _land_heading; - _velocity_setpoint(2) = land_speed; + _velocity_setpoint(2) = speed; _gear.landing_gear = landing_gear_s::GEAR_DOWN; } diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 50042232e3..2e87a701f0 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -171,11 +171,14 @@ protected: (ParamFloat) _param_mpc_xy_traj_p, (ParamFloat) _param_mpc_xy_err_max, (ParamFloat) _param_mpc_land_speed, + (ParamFloat) _param_mpc_land_crawl_speed, (ParamInt) _param_mpc_land_rc_help, (ParamFloat) - _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed + _param_mpc_land_alt1, // altitude at which we start ramping down speed (ParamFloat) - _param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed + _param_mpc_land_alt2, // altitude at which we descend at land speed + (ParamFloat) + _param_mpc_land_alt3, // altitude where we switch to crawl speed, if LIDAR available (ParamFloat) _param_mpc_z_v_auto_up, (ParamFloat) _param_mpc_z_v_auto_dn, (ParamFloat) _param_mpc_tko_speed, 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 fe361d0f5c..7d36e45673 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -414,6 +414,16 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); +/** + * Land crawl descend rate. Used below + * + * @unit m/s + * @min 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.15f); + /** * Enable user assisted descent speed for autonomous land routine. * @@ -714,7 +724,7 @@ 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". + * limited to "MPC_LAND_SPEED" * Value needs to be lower than "MPC_LAND_ALT1" * * @unit m @@ -725,6 +735,21 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); +/** + * Altitude for 3. step of slow landing + * + * Below this altitude descending velocity gets + * limited to "MPC_CRWL_SPEED", if LIDAR available. + * No effect if LIDAR not available + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.0f); + /** * Position control smooth takeoff ramp time constant *