diff --git a/epicardium/modules/bhi.c b/epicardium/modules/bhi.c
index bacb88ae0fb55f889f2f4cc168fd8d71a886f2bc..271825d721860cb18161625a6c690e1d1c1b7674 100644
--- a/epicardium/modules/bhi.c
+++ b/epicardium/modules/bhi.c
@@ -182,18 +182,27 @@ int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
 		return -ENODEV;
 	}
 
-	int ret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
-	if (ret < 0) {
-		return ret;
+	int lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
+	if (lockret < 0) {
+		return lockret;
 	}
 
 	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) == pdTRUE) {
 		struct stream_info *stream = &bhi160_streams[sensor_type];
-		stream_deregister(bhi160_lookup_sd(sensor_type), stream);
+		int streamret = stream_deregister(bhi160_lookup_sd(sensor_type), stream);
+		if (streamret < 0) {
+			xSemaphoreGive(bhi160_mutex);
+			hwlock_release(HWLOCK_I2C);
+			return streamret;
+		}
 		vQueueDelete(stream->queue);
 		stream->queue = NULL;
-
-		bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
+		int bhyret = bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
+		if (bhyret < 0) {
+			xSemaphoreGive(bhi160_mutex);
+			hwlock_release(HWLOCK_I2C);
+			return bhyret;
+		}
 		xSemaphoreGive(bhi160_mutex);
 	} else {
 		hwlock_release(HWLOCK_I2C);
diff --git a/preload/apps/bhi160/__init__.py b/preload/apps/bhi160/__init__.py
index e732581541517ef69b2d18c1e6c5713b9b4317cf..ff94f0b9b72df2531576e13f35e71ffe8b897fa0 100644
--- a/preload/apps/bhi160/__init__.py
+++ b/preload/apps/bhi160/__init__.py
@@ -4,19 +4,18 @@ import utime
 
 disp = display.open()
 
-bhi = bhi160.BHI160Accelerometer()
-
 while True:
-    samples = bhi.read()
-    if len(samples) > 0:
-        disp.clear()
-        sample = samples[0]
+    with bhi160.BHI160Accelerometer(sample_rate=20) as bhi:
+        utime.sleep(0.25)
+        samples = bhi.read()
+        if len(samples) > 0:
+            disp.clear()
+            sample = samples[0]
 
-        disp.print("Accelerometer", posy=0)
-        disp.print("X: %f" % sample["x"], posy=20)
-        disp.print("Y: %f" % sample["y"], posy=40)
-        disp.print("Z: %f" % sample["z"], posy=60)
+            disp.print("Accelerometer", posy=0)
+            disp.print("X: %f" % sample["x"], posy=20)
+            disp.print("Y: %f" % sample["y"], posy=40)
+            disp.print("Z: %f" % sample["z"], posy=60)
 
-        disp.update()
+            disp.update()
 
-    utime.sleep(0.1)
diff --git a/pycardium/modules/bhi160-sys.c b/pycardium/modules/bhi160-sys.c
index af1cb8b86702ef820648bd6f4b45954998f4a98e..88b0766da2138472cb957c08cbe81e5a13a72751 100644
--- a/pycardium/modules/bhi160-sys.c
+++ b/pycardium/modules/bhi160-sys.c
@@ -21,6 +21,10 @@ STATIC mp_obj_t mp_bhi160_enable_sensor(size_t n_args, const mp_obj_t *args)
 	return MP_OBJ_NEW_SMALL_INT(stream_id);
 }
 
+STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(
+	mp_bhi160_enable_sensor_obj, 4, 4, mp_bhi160_enable_sensor
+);
+
 STATIC mp_obj_t mp_bhi160_read_sensor(mp_obj_t stream_id_in)
 {
 	struct bhi160_data_vector buf[100];
@@ -44,20 +48,32 @@ STATIC mp_obj_t mp_bhi160_read_sensor(mp_obj_t stream_id_in)
 	return MP_OBJ_FROM_PTR(list);
 }
 
-STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(
-	mp_bhi160_enable_sensor_obj, 4, 4, mp_bhi160_enable_sensor
+STATIC MP_DEFINE_CONST_FUN_OBJ_1(
+	mp_bhi160_read_sensor_obj, mp_bhi160_read_sensor
 );
 
+STATIC mp_obj_t mp_bhi160_disable_sensor(mp_obj_t sensor_type_in)
+{
+	int sensor_type = mp_obj_get_int(sensor_type_in);
+
+	int ret = epic_bhi160_disable_sensor(sensor_type);
+
+	return MP_OBJ_NEW_SMALL_INT(ret);
+}
+
 STATIC MP_DEFINE_CONST_FUN_OBJ_1(
-	mp_bhi160_read_sensor_obj, mp_bhi160_read_sensor
+	mp_bhi160_disable_sensor_obj, mp_bhi160_disable_sensor
 );
 
+
 STATIC const mp_rom_map_elem_t bhi160_module_globals_table[] = {
 	{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_sys_bhi160) },
 	{ MP_ROM_QSTR(MP_QSTR_enable_sensor),
 	  MP_ROM_PTR(&mp_bhi160_enable_sensor_obj) },
 	{ MP_ROM_QSTR(MP_QSTR_read_sensor),
 	  MP_ROM_PTR(&mp_bhi160_read_sensor_obj) },
+	{ MP_ROM_QSTR(MP_QSTR_disable_sensor),
+	  MP_ROM_PTR(&mp_bhi160_disable_sensor_obj) },
 };
 STATIC MP_DEFINE_CONST_DICT(bhi160_module_globals, bhi160_module_globals_table);
 
diff --git a/pycardium/modules/py/bhi160.py b/pycardium/modules/py/bhi160.py
index afd0fdba5f81457a89cae71d14e13e74cf8c2e9a..b5839194165c5244a297493d6bb3a7f0fa133548 100644
--- a/pycardium/modules/py/bhi160.py
+++ b/pycardium/modules/py/bhi160.py
@@ -27,7 +27,11 @@ class BHI160:
     def close(self):
         if self.active:
             self.active = False
-            sys_bhi160.disable_sensor(self.sensor_id)
+            ret = sys_bhi160.disable_sensor(self.sensor_id)
+
+            if ret < 0:
+                raise ValueError("Disable sensor returned %i", ret)
+
             interrupt.disable_callback(self.interrupt_id)
             interrupt.set_callback(self.interrupt_id, None)
 
diff --git a/pycardium/modules/qstrdefs.h b/pycardium/modules/qstrdefs.h
index d6e4efa12172392d188b3a9e3c44cbb57b1a336b..82c1d12577ce06917fa8a94408f2961b2fbb5905 100644
--- a/pycardium/modules/qstrdefs.h
+++ b/pycardium/modules/qstrdefs.h
@@ -65,6 +65,7 @@ Q(RTC_ALARM)
 /* bhi160 */
 Q(sys_bhi160)
 Q(enable_sensor)
+Q(disable_sensor)
 Q(read_sensor)
 Q(x)
 Q(y)