Stitched in code

I added the all the code to calculate and set the roll and pitch angles
in FBWA. Hopefully this works.