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.
Also rotated proper rounding when converting from float to integer