feat(bhi160): Provide Gyroscope data

epicardium/epicardium.h

@@ -157,9 +157,11 @@ API(API_INTERRUPT_DISABLE, int epic_interrupt_disable(api_int_id_t int_id));
 /** TODO: BHI */
 #define EPIC_INT_BHI160_ACCELEROMETER   4
 API_ISR(EPIC_INT_BHI160_ACCELEROMETER, epic_isr_bhi160_accelerometer);
+#define EPIC_INT_BHI160_GYROSCOPE       5
+API_ISR(EPIC_INT_BHI160_GYROSCOPE, epic_isr_bhi160_gyroscope);
 
 /* Number of defined interrupts. */
-#define EPIC_INT_NUM                    5
+#define EPIC_INT_NUM                    6
 /* clang-format on */
 
 /*
@@ -923,7 +925,7 @@ enum bhi160_sensor_type {
 	BHI160_MAGNETOMETER                = 1,
 	/** Orientation (**Unimplemented**) */
 	BHI160_ORIENTATION                 = 2,
-	/** Gyroscope (**Unimplemented**) */
+	/** Gyroscope */
 	BHI160_GYROSCOPE                   = 3,
 	/** Gravity (**Unimplemented**) */
 	BHI160_GRAVITY                     = 4,

epicardium/modules/bhi.c

@@ -73,6 +73,7 @@ static size_t bhi160_lookup_data_size(enum bhi160_sensor_type type)
 	case BHI160_ACCELEROMETER:
 	case BHI160_MAGNETOMETER:
 	case BHI160_ORIENTATION:
+	case BHI160_GYROSCOPE:
 		return sizeof(struct bhi160_data_vector);
 	default:
 		return 0;
@@ -87,6 +88,8 @@ static bhy_virtual_sensor_t bhi160_lookup_vs_id(enum bhi160_sensor_type type)
 	switch (type) {
 	case BHI160_ACCELEROMETER:
 		return VS_ID_ACCELEROMETER;
+	case BHI160_GYROSCOPE:
+		return VS_ID_GYROSCOPE;
 	default:
 		return -1;
 	}
@@ -100,6 +103,8 @@ static int bhi160_lookup_sd(enum bhi160_sensor_type type)
 	switch (type) {
 	case BHI160_ACCELEROMETER:
 		return SD_BHI160_ACCELEROMETER;
+	case BHI160_GYROSCOPE:
+		return SD_BHI160_GYROSCOPE;
 	default:
 		return -1;
 	}
@@ -134,9 +139,14 @@ int epic_bhi160_enable_sensor(
 			return -ENOMEM;
 		}
 
-		stream_register(bhi160_lookup_sd(sensor_type), stream);
+		int streamret = stream_register(bhi160_lookup_sd(sensor_type), stream);
+	        if (streamret < 0) {
+			xSemaphoreGive(bhi160_mutex);
+			hwlock_release(HWLOCK_I2C);
+			return streamret;
+		}
 
-		bhy_enable_virtual_sensor(
+		int bhyret = bhy_enable_virtual_sensor(
 			vs_id,
 			VS_WAKEUP,
 			config->sample_rate,
@@ -145,6 +155,11 @@ int epic_bhi160_enable_sensor(
 			0,
 			config->dynamic_range /* dynamic range is sensor dependent */
 		);
+		if (bhyret != BHY_SUCCESS) {
+			xSemaphoreGive(bhi160_mutex);
+			hwlock_release(HWLOCK_I2C);
+			return bhyret;
+		}
 		xSemaphoreGive(bhi160_mutex);
 	} else {
 		hwlock_release(HWLOCK_I2C);
@@ -152,7 +167,7 @@ int epic_bhi160_enable_sensor(
 	}
 
 	hwlock_release(HWLOCK_I2C);
-	return 0;
+	return bhi160_lookup_sd(sensor_type);
 }
 
 int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
@@ -231,6 +246,24 @@ bhi160_handle_packet(bhy_data_type_t data_type, bhy_data_generic_t *sensor_data)
 			api_interrupt_trigger(EPIC_INT_BHI160_ACCELEROMETER);
 		}
 		break;
+	case VS_ID_GYROSCOPE:
+	case VS_ID_GYROSCOPE_WAKEUP:
+		MXC_ASSERT(data_type == BHY_DATA_TYPE_VECTOR);
+		if (bhi160_streams[BHI160_GYROSCOPE].queue == NULL) {
+			break;
+		}
+		data_vector.x = sensor_data->data_vector.x;
+		data_vector.y = sensor_data->data_vector.y;
+		data_vector.z = sensor_data->data_vector.z;
+		xQueueSend(
+			bhi160_streams[BHI160_GYROSCOPE].queue,
+			&data_vector,
+			BHI160_MUTEX_WAIT_MS
+		);
+		if (sensor_id == VS_ID_GYROSCOPE_WAKEUP) {
+			api_interrupt_trigger(EPIC_INT_BHI160_GYROSCOPE);
+		}
+		break;
 	default:
 		break;
 	}

epicardium/modules/stream.h

@@ -25,8 +25,9 @@ typedef unsigned int size_t;
  *    Please keep IDs in sequential order.
  */
 enum stream_descriptor {
-	/** BHI160 Accelerometer */
+	/** BHI160 */
 	SD_BHI160_ACCELEROMETER,
+	SD_BHI160_GYROSCOPE,
 	/** Highest descriptor must always be ``SD_MAX``. */
 	SD_MAX,
 };

pycardium/modules/bhi160-sys.c

@@ -23,17 +23,17 @@ STATIC mp_obj_t mp_bhi160_enable_sensor(size_t n_args, const mp_obj_t *args)
 	cfg.sample_rate                 = mp_obj_get_int(args[2]);
 	cfg.dynamic_range               = mp_obj_get_int(args[3]);
 
-	int sd = epic_bhi160_enable_sensor(sensor_type, &cfg);
+	int stream_id = epic_bhi160_enable_sensor(sensor_type, &cfg);
 
-	return MP_OBJ_NEW_SMALL_INT(sd);
+	return MP_OBJ_NEW_SMALL_INT(stream_id);
 }
 
 STATIC mp_obj_t mp_bhi160_read_sensor(mp_obj_t stream_id_in)
 {
 	struct bhi160_data_vector buf[100];
-	int sd = mp_obj_get_int(stream_id_in);
+	int stream_id = mp_obj_get_int(stream_id_in);
 
-	int n = epic_stream_read(sd, buf, sizeof(buf));
+	int n = epic_stream_read(stream_id, buf, sizeof(buf));
 
 	mp_obj_list_t *list = mp_obj_new_list(0, NULL);
 	for (int i = 0; i < n; i++) {

pycardium/modules/interrupt.c

@@ -87,6 +87,8 @@ static const mp_rom_map_elem_t interrupt_module_globals_table[] = {
 	  MP_OBJ_NEW_SMALL_INT(EPIC_INT_RTC_ALARM) },
 	{ MP_ROM_QSTR(MP_QSTR_BHI160_ACCELEROMETER),
 	  MP_OBJ_NEW_SMALL_INT(EPIC_INT_BHI160_ACCELEROMETER) },
+	{ MP_ROM_QSTR(MP_QSTR_BHI160_GYROSCOPE),
+	  MP_OBJ_NEW_SMALL_INT(EPIC_INT_BHI160_GYROSCOPE) },
 };
 static MP_DEFINE_CONST_DICT(
 	interrupt_module_globals, interrupt_module_globals_table

pycardium/modules/py/bhi160.py

@@ -2,20 +2,21 @@ import sys_bhi160
 import interrupt
 
 
-class BHI160Accelerometer:
-    def __init__(
-        self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
-    ):
-        interrupt.disable_callback(interrupt.BHI160_ACCELEROMETER)
-        interrupt.set_callback(
-            interrupt.BHI160_ACCELEROMETER, self._accelerometer_interrupt
+class BHI160:
+    def enable_sensor(self):
+        interrupt.disable_callback(self.interrupt_id)
+        interrupt.set_callback(self.interrupt_id, self._interrupt)
+        self.stream_id = sys_bhi160.enable_sensor(
+            self.sensor_id, self.sample_buffer_len, self.sample_rate, self.dynamic_range
         )
-        self.acc_sd = sys_bhi160.enable_sensor(
-            0, sample_buffer_len, sample_rate, dynamic_range
-        )
-        self._callback = callback
-        if callback:
-            interrupt.enable_callback(interrupt.BHI160_ACCELEROMETER)
+
+        if self.stream_id < 0:
+            raise ValueError("Enable sensor returned %i", self.stream_id)
+
+        self.active = True
+
+        if self._callback:
+            interrupt.enable_callback(self.interrupt_id)
 
     def __enter__(self):
         return self
@@ -24,33 +25,74 @@ class BHI160Accelerometer:
         self.close()
 
     def close(self):
-        if self.acc_sd is not None:
-            self.acc_sd = None
-            self.acc_sd = sys_bhi160.disable_sensor(0)
-            interrupt.disable_callback(interrupt.BHI160_ACCELEROMETER)
-            interrupt.set_callback(interrupt.BHI160_ACCELEROMETER, None)
-
-    def convert(self, value):
-        return 2 * value / 32768.0
+        if self.active:
+            self.active = False
+            sys_bhi160.disable_sensor(self.sensor_id)
+            interrupt.disable_callback(self.interrupt_id)
+            interrupt.set_callback(self.interrupt_id, None)
 
     def read(self):
         result = []
-        if self.acc_sd is not None:
-            for sample in sys_bhi160.read_sensor(self.acc_sd):
-                result.append(
-                    dict(
-                        {
-                            "x": self.convert(sample.x()),
-                            "y": self.convert(sample.y()),
-                            "z": self.convert(sample.z()),
-                        }
-                    )
-                )
+        if self.active:
+            for sample in sys_bhi160.read_sensor(self.stream_id):
+                result.append(self.convert(sample))
         return result
 
-    def _accelerometer_interrupt(self, _):
-        if self.acc_sd is not None:
-            data = sys_bhi160.read_sensor(self.acc_sd)
+    def _interrupt(self, _):
+        if self.active:
+            data = self.read()
             print(data)
             if self._callback:
                 self._callback(data)
+
+
+class BHI160Accelerometer(BHI160):
+    def __init__(
+        self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
+    ):
+        self.sample_rate = sample_rate
+        self.dynamic_range = dynamic_range
+        self.callback = callback
+        self.sample_buffer_len = sample_buffer_len
+        self.sensor_id = 0
+        self.interrupt_id = interrupt.BHI160_ACCELEROMETER
+        self._callback = callback
+        self.enable_sensor()
+
+    def convert_single(self, value):
+        return 2 * value / 32768.0
+
+    def convert(self, sample):
+        return dict(
+            {
+                "x": self.convert_single(sample.x()),
+                "y": self.convert_single(sample.y()),
+                "z": self.convert_single(sample.z()),
+            }
+        )
+
+
+class BHI160Gyroscope(BHI160):
+    def __init__(
+        self, sample_rate=4, dynamic_range=2, callback=None, sample_buffer_len=200
+    ):
+        self.sample_rate = sample_rate
+        self.dynamic_range = dynamic_range
+        self.callback = callback
+        self.sample_buffer_len = sample_buffer_len
+        self.sensor_id = 3
+        self.interrupt_id = interrupt.BHI160_GYROSCOPE
+        self._callback = callback
+        self.enable_sensor()
+
+    def convert_single(self, value):
+        return 360 * value / 32768.0
+
+    def convert(self, sample):
+        return dict(
+            {
+                "x": self.convert_single(sample.x()),
+                "y": self.convert_single(sample.y()),
+                "z": self.convert_single(sample.z()),
+            }
+        )

pycardium/modules/qstrdefs.h

@@ -58,6 +58,7 @@ Q(set_callback)
 Q(enable_callback)
 Q(disable_callback)
 Q(BHI160_ACCELEROMETER)
+Q(BHI160_GYROSCOPE)
 Q(RTC_ALARM)
 
 /* bhi160 */