diff options
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 | 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(); |