diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index f395c48a4d..b7654bb569 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -294,6 +294,8 @@ private: float _acceleration_state_dependent_z; /**< acceleration limit applied in manual mode in z */ float _manual_jerk_limit_xy; /**< jerk limit in manual mode dependent on stick input */ float _manual_jerk_limit_z; /**< jerk limit in manual mode in z */ + float _z_derivative; /**< velocity in z that agrees with position rate */ + float _takeoff_vel_limit; /**< velocity limit value which gets ramped up */ // counters for reset events on position and velocity states @@ -470,6 +472,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _acceleration_state_dependent_z(0.0f), _manual_jerk_limit_xy(1.0f), _manual_jerk_limit_z(1.0f), + _z_derivative(0.0f), _takeoff_vel_limit(0.0f), _z_reset_counter(0), _xy_reset_counter(0), @@ -2347,8 +2350,21 @@ MulticopterPositionControl::update_velocity_derivative() } else { _vel(2) = _local_pos.vz; } + + if (!_run_alt_control) { + /* set velocity to the derivative of position + * because it has less bias but blend it in across the landing speed range*/ + float weighting = fminf(fabsf(_vel_sp(2)) / _params.land_speed, 1.0f); + _vel(2) = _z_derivative * weighting + _vel(2) * (1.0f - weighting); + + } + } + if(PX4_ISFINITE(_local_pos.z_deriv)){ + _z_derivative = _local_pos.z_deriv; + }; + _vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); _vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); _vel_err_d(2) = _vel_z_deriv.update(-_vel(2));