mc_pos_control: smooth takeoff, fixed gradient of the velocity ramp by takeoff speed parameter

because we need a workaround for the broken auto takeoff logic which can start the vehicle with a loiter setpoint
in this case the very first moment is still a jump but then it gets taken over by the fix gradient ramp
this can basically be reverted after the auto takeoff logic is fixed
This commit is contained in:
Matthias Grob
2017-04-27 14:05:20 +02:00
parent b5820afa14
commit 0c4719de3e
@@ -1736,7 +1736,7 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
if (_in_takeoff) {
_in_takeoff = _takeoff_vel_limit < -_vel_sp(2);
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_limit += -_vel_sp(2) * dt / _takeoff_ramp_time.get();
_takeoff_vel_limit += _params.tko_speed * dt / _takeoff_ramp_time.get();
/* limit vertical velocity to the current ramp value */
_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
}