Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 13:15:32 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Merge pull request #2143 from PX4/mc_pos_D_reset

Browse Source 
mc_pos_control: always update previous velocity
This commit is contained in:
Lorenz Meier
2015-05-13 13:58:15 +02:00
parent b3ed0cf36b 1d544e028d
commit 164a254214
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 */ /* 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; 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 */ /* 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; 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; reset_yaw_sp = true;
} }
/* update previous velocity for velocity controller D part */
_vel_prev = _vel;
/* publish attitude setpoint /* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled, * 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 * in this case the attitude setpoint is published by the mavlink app