diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-08 15:43:58 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-08 15:43:58 +0100 |
| commit | 255db83e9346469ca7da02675a54b753cfd1a074 (patch) | |
| tree | 309e5ccee8641fedcec33ffc93bb974d90e032d7 /src/modules/mc_pos_control/mc_pos_control_main.cpp | |
| parent | 63815909979d8b01738a18bd694afcf2bc11c3a3 (diff) | |
| download | px4-firmware-255db83e9346469ca7da02675a54b753cfd1a074.tar.gz px4-firmware-255db83e9346469ca7da02675a54b753cfd1a074.tar.bz2 px4-firmware-255db83e9346469ca7da02675a54b753cfd1a074.zip | |
mc_pos_control: bug fixed
Diffstat (limited to 'src/modules/mc_pos_control/mc_pos_control_main.cpp')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 15 |
1 files changed, 13 insertions, 2 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 a50a9e50e..28c8861b6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -597,12 +597,23 @@ MulticopterPositionControl::task_main() _pos_sp += sp_move_rate * dt; /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs = (_pos_sp - _pos).edivide(_params.vel_max); + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (!_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } } else { |