You might want to check out
http://autopilot.sf.net
Apparently, a project like this is quite a bit more complicated than you may expect. The people on that site spent quite a while getting to a hovering point. Software wise, they used an insane multiple input Kalman filter. They may have some good code examples to use though. (I adapted their Kalman filter for a simple 2 input gyro+DAA setup for use with the 2004 FIRST RC this summer)