Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -4
View File
@@ -2453,13 +2453,10 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
_vel_sp(2) = math::min(_vel_sp(2), vel_limit); _vel_sp(2) = math::min(_vel_sp(2), vel_limit);
/* apply slewrate (aka acceleration limit) for smooth flying */ /* apply slewrate (aka acceleration limit) for smooth flying */
if (!_control_mode.flag_control_auto_enabled && !_in_smooth_takeoff) {
if (!_control_mode.flag_control_auto_enabled) {
vel_sp_slewrate(dt); vel_sp_slewrate(dt);
} }
_vel_sp_prev = _vel_sp;
/* special velocity setpoint limitation for smooth takeoff (after slewrate!) */ /* special velocity setpoint limitation for smooth takeoff (after slewrate!) */
if (_in_smooth_takeoff) { if (_in_smooth_takeoff) {
_in_smooth_takeoff = _takeoff_vel_limit < -_vel_sp(2); _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(2) = math::constrain(_vel_sp(2), -_params.vel_max_up, _params.vel_max_down);
_vel_sp_prev = _vel_sp;
} }
void void