APM-WIP

Plane : APM_Control

ROLL and PITCH controllers

These changes reduce height variation in turns and improve robustness. the specific changes are:

1) Linked roll and pitch integrator protection to the final output value so that if final output is on upper limit, the integrator is prevented from increasing and vice-versa. This improves wind-up protection.
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. This a mathematically correct calculation and will work over a range of bank angles and aircraft with minimal (if any) tuning required.
4) The integrator in the roll and pitch loop is clamped when the estimated speed is below the minimum FBW speed
5) The noise filter in the pitch and roll loop has been removed. Adequate filtering is provided for in the MPU 6000. Due to hardware differences, APM 1.0 users will get higher noise levels and will be limited in the amount of K_D gain they can use.
6) The inverted flight logic looked like a crash waiting to happen and has been removed. It's problematic to test and even if implemented correctly would still crash a plane with poor inverted flight capability. The conclusion was we should either implement it properly and fully tested or delete it.

The following block diagram shows the general arrangement of the pitch controller (the roll controller is similar, but without the compensation input)

![PitchAP](https://f.cloud.github.com/assets/3596952/429479/e4af364e-ae45-11e2-9908-729c3d8feb60.jpg)


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. 
Compare to standard PIDS, the revised pitch and roll controllers allow the use of larger rate feedback (damping) (effectively the same as the old D term) without beating the servos to death or becoming unstable. This provides less angular motion in turbulence which is good for video.
The bank angle compensation in the pitch loop works effectively over a much larger range of bank angles and requires minimal tuning compared to the old calculation.

YAW CONTROLLER

This is based on a 3-loop topology as commonly used in tactical skid to to turn missiles and is easy to tune. The following block diagram shows the general arrangement of the yaw controller:

![latAP](https://f.cloud.github.com/assets/3596952/423661/1bf8f150-ad68-11e2-8044-b8e338ac7660.jpg)


Note that the acceleration measurement has to pass through an integrator before it gets to the actuator. This is a important feature as it eliminates problems of high frequency noise and potential coupling with structural modes associated with direct feedback of measured acceleration to actuator.

The high pass filter has been inserted to compensate for airspeed and bank angle measurement errors which will cause steady state errors in the calculation of the turn yaw rate.

The yaw controller flies SITL well, and has had limited flight testing. It can be configured either as a simple yaw damper, or the acceleration and integral term can be turned on to allow feedback control of lateral acceleration/sideslip.

TO-DO : 

Flight testing by other users to verify tuning guide.

UPDATE:

This branch has been merged into main and is now included in 2.72 beta