I made a solution for this very same problem about a year ago, its somewhere on the forum though. If I had my laptop I would give you a working sketch but sadly I am on my tablet and won't be back home until tomorrow night. If you want, look up LCD compass and my username, you should be able to find the post.