Made accel / gyro self tests aware of offsets and scales, added support to config command to call these
Merge branch 'sensor_selftests'