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 bfd9ae3fa895927f201cc3ad767dcbe77ad49316..01c12c30d12ce1bfc03f16f08996a6d790a07f0a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1045,7 +1045,7 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl 0.2f; // takeoff was initiated through velocity setpoint - _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < math::min(-_tko_speed.get(), -0.6f); + _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) { // There is a position setpoint above current position or velocity setpoint larger than @@ -1076,19 +1076,24 @@ MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float } // Ramp up takeoff speed. - _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get(); - _takeoff_speed = math::min(_takeoff_speed, desired_tko_speed); + if (_takeoff_ramp_time.get() > _dt) { + _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get(); + + } else { + // No ramp, directly go to desired takeoff speed + _takeoff_speed = desired_tko_speed; + } + + _takeoff_speed = math::min(_takeoff_speed, _tko_speed.get()); // Smooth takeoff is achieved once desired altitude/velocity setpoint is reached. if (!_smooth_velocity_takeoff) { _in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - MPC_LAND_ALT2.get()); - } else { - _in_smooth_takeoff = _takeoff_speed < -vz_sp; + } else { + // Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector + _in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed; } - - } else { - _in_smooth_takeoff = false; } } 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 f87debef19a23dee984c6c11656a82bb4b1d6baf..2d5b80a410c2a54c7db4df5ca73e839d24948cef 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -650,8 +650,9 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); * * Increasing this value will make automatic and manual takeoff slower. * If it's too slow the drone might scratch the ground and tip over. + * A time constant of 0 disables the ramp * - * @min 0.1 + * @min 0 * @max 1 * @group Multicopter Position Control */