Hello,
I got interested in arduino recently when working on a different project that might interest you, If i was you i would head over to diydrones.com, where they have arduino boards running autopilots on radio control airplanes. Sorry i don't have an answer, but read through their forums. check their source codes, and you may find an answer.