- GPS location is used to figure out heading to waypoint.
- Waypoint heading is compared against GPS heading to give heading error.
- Heading error is translated to target yaw rate.
- Yaw rate is compared against measured yaw rate to give yaw rate error.
- Yaw rate error is translated to roll angle.
- Roll angle is compared against measured roll angle to give roll angle error.
- Roll angle error is translated to aileron and rudder control.
Although the test was mostly successful and the autopilot could keep the plane under control (and even land the plane) the controller was only borderline stable. Horrible oscillations rise especially when the controller tries to track a heading. After looking at the recorded telemetry I've re-tuned the controllers. Hopefully next time will go smoother. Anyway, I'm happy that for the first time it actually works well enough to be useful.
See this video.