Return correct autopilot custom mode

- Take into account custom modes
  based on MAV_AUTOPILOT and MAV_TYPE
  since fixed wing and quad custom
  modes differ. Will need to expand
  to all MAV supported types and
  autopilots down the road.