Adds maths for calculating gps speed & heading. Code in a test phase, unusable for now