AP_Compass: more efficient calculate_heading() implementation

Thanks to piersh for this improvement. See

https://github.com/diydrones/ardupilot/commit/b5f0635455957d6df65c7653e3077a6fa0737ea4#commitcomment-3171806
AP_Compass: use const on more functions and remove old calculate_heading()

the calculate_heading() based on roll/pitch is not needed anywhere