Made accel / gyro self tests aware of offsets and scales, added support ...

...to config command to call these.

This is still WIP and the test fails at locations where it shouldn't, so it needs minor fixes. But all functionality is there.