Odd Heading-Hold / Depth-Hold behavior with Ardusub 4.0.1