ardupilot/ArduSub/AP_State.cpp