diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-09-01 18:47:56 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-09-01 18:47:56 +0200 |
commit | 132c9180ead0c64c2edcc36dec2bf8896679a040 (patch) | |
tree | d98ea2ebc63354f47504901806beb1b218b569af /src | |
parent | 8ad1aa789b6f10769dd9977151c9a39971b1ebaf (diff) | |
download | px4-firmware-132c9180ead0c64c2edcc36dec2bf8896679a040.tar.gz px4-firmware-132c9180ead0c64c2edcc36dec2bf8896679a040.tar.bz2 px4-firmware-132c9180ead0c64c2edcc36dec2bf8896679a040.zip |
mc_pos_control: move position offset limiting to separate method
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 28 |
1 files changed, 28 insertions, 0 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 01fdb4c65..ce26f7dc2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -222,6 +222,11 @@ private: void reset_alt_sp(); /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + + /** * Set position setpoint using manual control */ void control_manual(float dt); @@ -545,6 +550,29 @@ MulticopterPositionControl::reset_alt_sp() } void +MulticopterPositionControl::limit_pos_sp_offset() +{ + 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.sp_offs_max); + } +} + +void MulticopterPositionControl::control_manual(float dt) { _sp_move_rate.zero(); |