diff --git a/Inc/config.h b/Inc/config.h
index edc18b8..331d70b 100644
--- a/Inc/config.h
+++ b/Inc/config.h
@@ -42,6 +42,12 @@
 // #define PRINTF_FLOAT_SUPPORT                  // [-] Uncomment this for printf to support float on Serial Debug. It will increase code size! Better to avoid it!
 /* =============================================================================================== */
 
+// #define MPU6052C   // uncommit for MPU6052C
+
+#ifdef MPU6052C
+#define I2C_SINGLE_BYTE
+#define USE_RCC_DELAY  // Use RCC_Delay instead of HAL_Delay
+#endif
 
 /* ==================================== SETTINGS MPU-6050 ==================================== */
 #define MPU_SENSOR_ENABLE                     // [-] Enable flag for MPU-6050 sensor. Comment-out this flag to Disable the MPU sensor and reduce code size.
diff --git a/Src/mpu6050.c b/Src/mpu6050.c
index d3a5858..8c99fb8 100644
--- a/Src/mpu6050.c
+++ b/Src/mpu6050.c
@@ -2625,13 +2625,11 @@ int mpu_run_self_test(long *gyro, long *accel)
     unsigned char accel_fsr, fifo_sensors, sensors_on;
     unsigned short gyro_fsr, sample_rate, lpf;
     unsigned char dmp_was_on;
-
     if (st.chip_cfg.dmp_on) {
         mpu_set_dmp_state(0);
         dmp_was_on = 1;
     } else
         dmp_was_on = 0;
-
     /* Get initial settings. */
     mpu_get_gyro_fsr(&gyro_fsr);
     mpu_get_accel_fsr(&accel_fsr);
@@ -2639,7 +2637,6 @@ int mpu_run_self_test(long *gyro, long *accel)
     mpu_get_sample_rate(&sample_rate);
     sensors_on = st.chip_cfg.sensors;
     mpu_get_fifo_config(&fifo_sensors);
-
     /* For older chips, the self-test will be different. */
 #if defined MPU6050
     for (ii = 0; ii < tries; ii++)
@@ -2653,7 +2650,7 @@ int mpu_run_self_test(long *gyro, long *accel)
         goto restore;
     }
     for (ii = 0; ii < tries; ii++)
-        if (!get_st_biases(gyro_st, accel_st, 1))
+       if (!get_st_biases(gyro_st, accel_st, 1))
             break;
     if (ii == tries) {
         /* Again, probably an I2C error. */
@@ -3231,7 +3228,6 @@ void mpu_start_self_test(void)
 {
     int result;
     long gyro[3], accel[3];
-
 #if defined (MPU6500) || defined (MPU9250)
     result = mpu_run_6500_self_test(gyro, accel, 0);
 #elif defined (MPU6050) || defined (MPU9150)
@@ -3781,7 +3777,9 @@ void mpu_handle_input(char c)
             consoleLog("f: Set DMP on/off\r\n");
             consoleLog("v: Set Quaternion on/off\r\n");
             consoleLog("w: Test low-power accel mode\r\n");
+#ifndef MPU6052C
             consoleLog("s: Run self-test (device must be facing up or down)\r\n");
+#endif
             #endif // MPU_SENSOR_ENABLE
             consoleLog("============\r\n");
             break;
@@ -3947,7 +3945,7 @@ void mpu_handle_input(char c)
             hal.sensors |= ACCEL_ON;
             hal.lp_accel_mode = 1;
             break;
-
+#ifndef MPU6052C
         /* The hardware self test is completely localized in the gyro driver.
         * Logging is assumed to be enabled; otherwise, a couple LEDs could
         * probably be used here to display the test results.
@@ -3955,6 +3953,7 @@ void mpu_handle_input(char c)
         case 's':
             mpu_start_self_test();
             break;
+#endif
         #endif // MPU_SENSOR_ENABLE
 
         default:
diff --git a/Src/util.c b/Src/util.c
index 335bdb5..0d1bbb1 100644
--- a/Src/util.c
+++ b/Src/util.c
@@ -554,9 +554,24 @@ int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_
     // HAL_I2C_Mem_Write_IT(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
     // while(HAL_I2C_STATE_READY != HAL_I2C_GetState(&hi2c1));                     // Wait until all data bytes are sent/received
     // return 0;
-
+#ifdef I2C_SINGLE_BYTE
+    HAL_StatusTypeDef res = HAL_OK;
+    if(regAddr==0x6b && length ==2){
+        uint8_t i;
+        for(i=0; i < length && res == HAL_OK; i++) {
+            res = HAL_I2C_Mem_Write(&hi2c1, slaveAddr << 1, regAddr+i, 1, data+i, 1, 100);    // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
+        }
+    }
+      else {
+            res = HAL_I2C_Mem_Write(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100);    // Address is shifted one position to the left. LSB is reserved for the Read/Write bit. 
+      }
+      if(res != HAL_OK) {
+         printf("i2c_writeBytes not ok %u %u\r\n", regAddr, length);
+      }
+      return res;
+#else
     return HAL_I2C_Mem_Write(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100);    // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
-
+#endif
 }
 
 /*
@@ -588,8 +603,27 @@ int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t
     // HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
     // while(HAL_I2C_STATE_READY != HAL_I2C_GetState(&hi2c1));                   // Wait until all data bytes are sent/received
     // return 0;
+#ifdef I2C_SINGLE_BYTE
+    HAL_StatusTypeDef res = HAL_OK;
+    if((regAddr==0x72 || regAddr == 0x41) && length ==2) {
+       uint8_t i;
+       for(i=0; i < length && res == HAL_OK; i++) {
+          res= HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr+i, 1, data+i, 1, 100);     // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
+       }
+    }
+    else {
+        res = HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100);     // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
 
+    }
+      if(res != HAL_OK) {
+        printf("i2c_readBytes not ok %u %u\r\n", regAddr, length);
+      }
+      return res;
+#else
     return HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100);     // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
+#endif
+
+//    
 
 }
 
@@ -615,7 +649,17 @@ int8_t i2c_readBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *
 
 
 
-
+#ifdef USE_RCC_DELAY
+void HAL_Delay(uint32_t mdelay)
+{
+  __IO uint32_t Delay = mdelay * (SystemCoreClock / 8U / 1000U);
+  do
+  {
+    __NOP();
+  }
+  while (Delay --);
+}
+#endif