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.

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -413,6 +413,22 @@ void AP_InertialSensor_MPU6000::_register_write(uint8_t reg, uint8_t val)
     _spi->transaction(tx, rx, 2);
 }
 
+/*
+  useful when debugging SPI bus errors
+ */
+void AP_InertialSensor_MPU6000::_register_write_check(uint8_t reg, uint8_t val)
+{
+    uint8_t readed;
+    _register_write(reg, val);
+    readed = _register_read(reg);
+    if (readed != val){
+	hal.console->printf_P(PSTR("Values doesn't match; written: %02x; read: %02x "), val, readed);
+    }
+#if MPU6000_DEBUG
+    hal.console->printf_P(PSTR("Values written: %02x; readed: %02x "), val, readed);
+#endif
+}
+
 /*
   set the DLPF filter frequency. Assumes caller has taken semaphore
  */
@@ -440,7 +456,6 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t
 
     if (filter != 0) {
         _last_filter_hz = filter_hz;
-
         _register_write(MPUREG_CONFIG, filter);
     }
 }

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -45,6 +45,7 @@ private:
     void                 _poll_data(void);
     uint8_t              _register_read( uint8_t reg );
     void                 _register_write( uint8_t reg, uint8_t val );
+    void                 _register_write_check(uint8_t reg, uint8_t val);
     bool                 _hardware_init(Sample_rate sample_rate);
 
     AP_HAL::SPIDeviceDriver *_spi;