Refactored nav_finken and some PIDs