dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: save previous velocity setpoint after constraining, fix smooth takeoff to be still excluded from slewrate

This commit is contained in:
Matthias Grob 2017-11-08 17:58:48 +01:00
parent 26d95ef674
commit 26171df9fc
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -2453,13 +2453,10 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
_vel_sp(2) = math::min(_vel_sp(2), vel_limit);
/* apply slewrate (aka acceleration limit) for smooth flying */
if (!_control_mode.flag_control_auto_enabled) {
if (!_control_mode.flag_control_auto_enabled && !_in_smooth_takeoff) {
vel_sp_slewrate(dt);
}
_vel_sp_prev = _vel_sp;
/* special velocity setpoint limitation for smooth takeoff (after slewrate!) */
if (_in_smooth_takeoff) {
_in_smooth_takeoff = _takeoff_vel_limit < -_vel_sp(2);
@ -2478,6 +2475,8 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
}
_vel_sp(2) = math::constrain(_vel_sp(2), -_params.vel_max_up, _params.vel_max_down);
_vel_sp_prev = _vel_sp;
}
void