Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-18 00:08:22 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

mc_pos_control: always update previous velocity to avoid spikes due to differentiation

Browse Source
This commit is contained in:
Roman Bapst
2015-05-07 15:04:59 +02:00
parent 5003875911
commit 1d544e028d
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -1
View File
@@ -1102,7 +1102,6 @@ MulticopterPositionControl::task_main()
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
_vel_prev = _vel;
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
@@ -1406,6 +1405,9 @@ MulticopterPositionControl::task_main()
reset_yaw_sp = true;
}
/* update previous velocity for velocity controller D part */
_vel_prev = _vel;
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled,
* in this case the attitude setpoint is published by the mavlink app