Tested vision kinematics. All is woring. Will now add switching to D2P option. Best switching point seems to be between 1 and 2 m