diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-10-02 15:45:02 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-10-02 15:45:02 +0400 |
| commit | 84908f8f3db5179ffff0d96d15756ab112535482 (patch) | |
| tree | 4f5bb483a0d2229dac21f14b01433bfb1af46337 /src/modules/mc_pos_control | |
| parent | 1107f5903621f8e41faec6bd31900826b8f99524 (diff) | |
| download | px4-firmware-84908f8f3db5179ffff0d96d15756ab112535482.tar.gz px4-firmware-84908f8f3db5179ffff0d96d15756ab112535482.tar.bz2 px4-firmware-84908f8f3db5179ffff0d96d15756ab112535482.zip | |
mc_pos_control: AUTO speed limiting bug fixed
Diffstat (limited to 'src/modules/mc_pos_control')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 |
1 files changed, 6 insertions, 4 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 99deb0d29..ec7b2a78f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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 */ |