corrected heading

seems like it makes a difference if rotating back roll or pitch first.
Rotate Pitch then Roll doesn't work properly, but rotate Roll then Pitch
works.
It's also using proper rounding now when converting from float to integer