void PositionControl::_positionController()
  215 {	  // PART 1	
  216     // P-position controller
  217     Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(MPC_XY_P.get(), MPC_XY_P.get(), MPC_Z_P.get()));
  218     _vel_sp = vel_sp_position + _vel_sp;
  219     // PART 2
  220     // Constrain horizontal velocity by prioritizing the velocity component along the
  221     // the desired position setpoint over the feed-forward term.
  222     Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)),
  223                  Vector2f(&(_vel_sp - vel_sp_position)(0)), _constraints.speed_xy);
  224     _vel_sp(0) = vel_sp_xy(0);
  225     _vel_sp(1) = vel_sp_xy(1);
  226     // Constrain velocity in z-direction.
  227     _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
  228 }
  229