fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 13:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: AUTO speed limiting bug fixed

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+6 -4
View File
@@ -829,10 +829,12 @@ MulticopterPositionControl::control_auto(float dt)
/* move setpoint not faster than max allowed speed */
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();
if (d_pos_s_len > dt) {
pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt;
/* difference between current and desired position setpoints, 1 = max speed */
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
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 */