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

mc_pos_control: AUTO speed limiting bug fixed

This commit is contained in:
Anton Babushkin 2014-10-02 15:45:02 +04:00
parent 1107f59036
commit 84908f8f3d
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -829,10 +829,12 @@ MulticopterPositionControl::control_auto(float dt)
/* move setpoint not faster than max allowed speed */ /* move setpoint not faster than max allowed speed */
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s;
float d_pos_s_len = d_pos_s.length(); /* difference between current and desired position setpoints, 1 = max speed */
if (d_pos_s_len > dt) { math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; float d_pos_m_len = d_pos_m.length();
if (d_pos_m_len > dt) {
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
} }
/* scale result back to normal space */ /* scale result back to normal space */