APM-WIP

Plane : APM_Control

These changes reduce height variation in turns and improve robustness. the specific changes are:

1) Linked roll and pitch integrator protection tot he final output value so that if final output is on upper limit, the integrator is prevented from increasing and vice-versa
2) Modified rate feedback in roll and pitch controllers to use body rates rather than Euler or earth rates.
3) Changed the roll to pitch compensation to use measured roll angle and estimated airspeed to calculate the component of turn rate (assuming a level coordinated turn) around the pitch axis.
4) The integrator in the roll and pitch loop is clamped when the estimated speed is below the minimum FBW speed

This has been flown on the rascal in the SITL and on a X-8 with limited flight testing. Initial results have been encouraging with reduced height variation in turns.