Rebased to ardupilot