fixed double data in velocity, added height again
Merge branch 'master' of https://github.com/quinten991/PenO3OnTrack