This paper presents a novel algorithm to dock a non-holonomic Autonomous Underwater Vehicle (AUV) into a funnel-shaped Docking Station (DS), in the presence of ocean currents. In a previous work, the authors have compared several docking algorithms through Monte Carlo simulations. In this paper, a new control algorithm is presented with a goal to improve over the previous ones to fulfil the specific needs of the ATLANTIS project. Performance of the new proposed algorithm has been compared with the results of the previous study, using the same environemnt on the Stonefish hardware-in-the-loop simulator.