Working accelerometer algorithm, minor refactoring