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 90e761badd02..b8c46d791c93 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -926,7 +926,8 @@ MulticopterPositionControl::control_manual(float dt) if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { if (!_pos_hold_engaged) { - float vel_xy_mag = sqrtf(_vel(0)*_vel(0) + _vel(1)*_vel(1)); + float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)); + if (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy) { /* reset position setpoint to have smooth transition from velocity control to position control */ _pos_hold_engaged = true;