Waypoint ArduSub

I have a BlueROV2. The vehicle runs fine in stabilize, manual, and depth hold mode. I have the frame setup called Simple ROV-4. Motors 3 and 4 Control depth.

When I try to do surface waypoint testing the vehicle automatically actuates the vertical thrusters (Motors 3 and 4) and the immediately dives. Is there a way to fix that using code? Is it a sensor depth issue where the vehicle doesn’t understand its altitude?