Rotate axes in controller and improve compass
Add averaging to pitch and roll calculation