COLA2: A control architecture for AUVs