I've been away on vacation for the last week and a half, but I was able to port Arducopter to Flash and do some simulations. This has helped me rewrite the navigation control laws, simple mode, and Loiter to all use a streamlined set of control loops. This not only has saved code space, but the end result is a quad that flies like it's on rails.
Loiter and WP navigation are now the same code and use a rate based solution from the GPS. The heading and ground speed values are parsed into X(long) and Y(lat) components. This gives me a far more accurate estimation of position than just using the Mediatek's lat and long outputs. The controller takes the position error, desired speed, and minimum speed and outputs pitch and roll commands. I've noticed a marked improvement in Loiter hold. Basically it's glued to one spot. I'll need to do more testing in windy conditions, so stay tuned.
Altitude hold uses the same controller design and manages the speed of altitude change. Landing and altitude changes are much more controlled and I've not seen the quad loose altitude on RTL like it did in the past.
The Throttle, Roll/Pitch, and Yaw control loops have all be separated to give you the option of redefining any of the modes. In the zip file below, Alt hold has been redefined (in APM_Config,h) to use SIMPLE mode for Roll/Pitch. Loiter has been redefined to use Manual throttle.
The Flash based simulator uses nearly identical code to Arducopter. Here's the work in progress of the sim:
The code itself is available for download here and is considered alpha to me. WP's are being tested tomorrow. Only RTL, SIMPLE, ALT HOLD have been tested so far. Crosstrack correction is currently disabled. Everything should work, but be careful. Anything could happen. I've only crashed 4 times today ;)