Add Compass heading to graphical output
Unsuccessfull BETA tuning
Code cleaning