Hi @gcelec ,
We are aware of this issues and it is one of the main issues in our list for a future release of ArduSub.
Right now we need to make sure it is still present in Ardupilot master, as many changes were merged since Ardusub 4.0. The issue is being tracked here .