AP_InertialSensor_MPU6000: Add _register_write_check method.

This private method allows to check whether the value written and
the posterior value readed are the same.
Should be used only for debuging purposes, for release versions use
_register_write instead.
AP_InertialSensor_MPU6000: Enable DEBUG