There were some issues with the compass and/or gyroscope spinning in ArduSub 4.0 (which tended to be resolved at least temporarily by re-calibrating the sensors) but as far as we’re aware that was resolved with ArduSub 4.1 - which version are you using?
I brought this up with @williangalvani, and he mentioned it may be due to the DVL attitude not being passed along to ArduSub, which I’ve raised an issue for here. It will hopefully get worked on early next week.