ManualSmoothingXY: velocity as criteria for direction change to prenvent fast acceleration at low spped

ManualSmoothingXY: detect alignement based on body frame; do direction change only if not yawspeed is demanded