diff --git a/lib/bosch/BHy1_driver_and_MCU_solution/driver/src/bhy_support.c b/lib/bosch/BHy1_driver_and_MCU_solution/driver/src/bhy_support.c
index c7c0e000d270a8483e9153337071c6abd042ce21..2c83fe6d3f3b604153b01943461b77f22154e00d 100644
--- a/lib/bosch/BHy1_driver_and_MCU_solution/driver/src/bhy_support.c
+++ b/lib/bosch/BHy1_driver_and_MCU_solution/driver/src/bhy_support.c
@@ -79,17 +79,18 @@ static int8_t sensor_i2c_write(uint8_t addr, uint8_t reg, uint8_t *p_buf, uint16
     buf[0] = reg;
     memcpy(buf + 1, p_buf, size);
 
-    return I2C_MasterWrite(I2C_DEVICE, addr << 1, buf, size + 1, 0);
+    int l = I2C_MasterWrite(I2C_DEVICE, addr << 1, buf, size + 1, 0);
+    return l == size + 1 ? BHY_SUCCESS : BHY_ERROR;
 }
 
 static int8_t sensor_i2c_read(uint8_t addr, uint8_t reg, uint8_t *p_buf, uint16_t size)
 {
     //printf("sensor_i2c_read 0x%02x %d\n", reg, size);
     if(I2C_MasterWrite(I2C_DEVICE, addr << 1, &reg, 1, 1) == 1) {
-        return I2C_MasterRead(I2C_DEVICE, addr << 1, p_buf, size, 0);
+        int l = I2C_MasterRead(I2C_DEVICE, addr << 1, p_buf, size, 0);
+        return l == size ? BHY_SUCCESS : BHY_ERROR;
     }
-    // TODO: return error code
-    return 0;
+    return BHY_ERROR;
 }
 
 /********************************************************************************/