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

Merge pull request #2143 from PX4/mc_pos_D_reset 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/mc_pos_control/mc_pos_control_main.cpp
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