feat(epicardium): Add driver for BHI160

Related to #20.

Signed-off-by: Rahix <rahix@rahix.de>

epicardium/epicardium.h

@@ -3,6 +3,8 @@
 
 #include <stdint.h>
 #include <errno.h>
+#include <stdbool.h>
+#include <stddef.h>
 
 #ifndef __SPHINX_DOC
 /* Some headers are not recognized by hawkmoth for some odd reason */
@@ -111,6 +113,9 @@ typedef _Bool bool;
 #define API_BME680_DEINIT          0xD1
 #define API_BME680_GET_DATA        0xD2
 
+#define API_BHI160_ENABLE          0xe0
+#define API_BHI160_DISABLE         0xe1
+
 /* clang-format on */
 
 typedef uint32_t api_int_id_t;
@@ -859,6 +864,160 @@ API(API_PERSONAL_STATE_IS_PERSISTENT, int epic_personal_state_is_persistent());
  */
 API(API_STREAM_READ, int epic_stream_read(int sd, void *buf, size_t count));
 
+/**
+ * BHI160 Sensor Fusion
+ * ====================
+ * card10 has a BHI160 onboard which is used as an IMU.  BHI160 exposes a few
+ * different sensors which can be accessed using Epicardium API.
+ *
+ * **Example**:
+ *
+ * .. code-block:: cpp
+ *
+ *    #include "epicardium.h"
+ *
+ *    // Configure a sensor & enable it
+ *    struct bhi160_sensor_config cfg = {0};
+ *    cfg.sample_buffer_len = 40;
+ *    cfg.sample_rate = 4;   // Hz
+ *    cfg.dynamic_range = 2; // g
+ *
+ *    int sd = epic_bhi160_enable_sensor(BHI160_ACCELEROMETER, &cfg);
+ *
+ *    // Read sensor data
+ *    while (1) {
+ *            struct bhi160_data_vector buf[10];
+ *
+ *            int n = epic_stream_read(sd, buf, sizeof(buf));
+ *
+ *            for (int i = 0; i < n; i++) {
+ *                    printf("X: %6d Y: %6d Z: %6d\n",
+ *                           buf[i].x,
+ *                           buf[i].y,
+ *                           buf[i].z);
+ *            }
+ *    }
+ *
+ *    // Disable the sensor
+ *    epic_bhi160_disable_sensor(BHI160_ACCELEROMETER);
+ */
+
+/**
+ * BHI160 Sensor Types
+ * -------------------
+ */
+
+/**
+ * BHI160 virtual sensor type.
+ */
+enum bhi160_sensor_type {
+	/**
+	 * Accelerometer
+	 *
+	 * - Data type: :c:type:`bhi160_data_vector`
+	 * - Dynamic range: g's (1x Earth Gravity, ~9.81m*s^-2)
+	 */
+	BHI160_ACCELEROMETER               = 0,
+	/** Magnetometer (**Unimplemented**) */
+	BHI160_MAGNETOMETER                = 1,
+	/** Orientation (**Unimplemented**) */
+	BHI160_ORIENTATION                 = 2,
+	/** Gyroscope (**Unimplemented**) */
+	BHI160_GYROSCOPE                   = 3,
+	/** Gravity (**Unimplemented**) */
+	BHI160_GRAVITY                     = 4,
+	/** Linear acceleration (**Unimplemented**) */
+	BHI160_LINEAR_ACCELERATION         = 5,
+	/** Rotation vector (**Unimplemented**) */
+	BHI160_ROTATION_VECTOR             = 6,
+	/** Uncalibrated magnetometer (**Unimplemented**) */
+	BHI160_UNCALIBRATED_MAGNETOMETER   = 7,
+	/** Game rotation vector (whatever that is supposed to be) */
+	BHI160_GAME_ROTATION_VECTOR        = 8,
+	/** Uncalibrated gyroscrope (**Unimplemented**) */
+	BHI160_UNCALIBRATED_GYROSCOPE      = 9,
+	/** Geomagnetic rotation vector (**Unimplemented**) */
+	BHI160_GEOMAGNETIC_ROTATION_VECTOR = 10,
+};
+
+/**
+ * BHI160 Sensor Data Types
+ * ------------------------
+ */
+
+/**
+ * Vector Data.  The scaling of these values is dependent on the chosen dynamic
+ * range.  See the individual sensor's documentation for details.
+ */
+struct bhi160_data_vector {
+	/** X */
+	int16_t x;
+	/** Y */
+	int16_t y;
+	/** Z */
+	int16_t z;
+};
+
+/**
+ * BHI160 API
+ * ----------
+ */
+
+/**
+ * Configuration for a BHI160 sensor.
+ *
+ * This struct is used when enabling a sensor using
+ * :c:func:`epic_bhi160_enable_sensor`.
+ */
+struct bhi160_sensor_config {
+	/**
+	 * Number of samples Epicardium should keep for this sensor.  Do not set
+	 * this number too high as the sample buffer will eat RAM.
+	 */
+	size_t sample_buffer_len;
+	/**
+	 * Sample rate for the sensor in Hz.  Maximum data rate is limited
+	 * to 200 Hz for all sensors though some might be limited at a lower
+	 * rate.
+	 */
+	uint16_t sample_rate;
+	/**
+	 * Dynamic range.  Interpretation of this value depends on
+	 * the sensor type.  Please refer to the specific sensor in
+	 * :c:type:`bhi160_sensor_type` for details.
+	 */
+	uint16_t dynamic_range;
+	/** Always zero. Reserved for future parameters. */
+	uint8_t _padding[8];
+};
+
+/**
+ * Enable a BHI160 virtual sensor.  Calling this funciton will instruct the
+ * BHI160 to collect data for this specific virtual sensor.  You can then
+ * retrieve the samples using :c:func:`epic_stream_read`.
+ *
+ * :param bhi160_sensor_type sensor_type: Which sensor to enable.
+ * :param bhi160_sensor_config* config: Configuration for this sensor.
+ * :returns: A sensor descriptor which can be used with
+ *    :c:func:`epic_stream_read` or a negative error value:
+ *
+ *    - ``-EBUSY``:  The BHI160 driver is currently busy with other tasks and
+ *      could not be acquired for enabling a sensor.
+ */
+API(API_BHI160_ENABLE, int epic_bhi160_enable_sensor(
+	enum bhi160_sensor_type sensor_type,
+	struct bhi160_sensor_config *config
+));
+
+/**
+ * Disable a BHI160 sensor.
+ *
+ * :param bhi160_sensor_type sensor_type: Which sensor to disable.
+ */
+API(API_BHI160_DISABLE, int epic_bhi160_disable_sensor(
+	enum bhi160_sensor_type sensor_type
+));
+
 /**
  * Vibration Motor
  * ===============

epicardium/main.c

@@ -50,6 +50,18 @@ int main(void)
 		abort();
 	}
 
+	/* BHI160 */
+	if (xTaskCreate(
+		    vBhi160Task,
+		    (const char *)"BHI160 Driver",
+		    configMINIMAL_STACK_SIZE * 2,
+		    NULL,
+		    tskIDLE_PRIORITY + 1,
+		    NULL) != pdPASS) {
+		printf("Failed to create bhi160 dispatcher task!\n");
+		abort();
+	}
+
 	/* API */
 	if (xTaskCreate(
 		    vApiDispatcher,

epicardium/meson.build

@@ -80,7 +80,7 @@ elf = executable(
   l0der_sources,
   ble_sources,
   version_hdr,
-  dependencies: [libcard10, max32665_startup_core0, maxusb, libff13, ble],
+  dependencies: [libcard10, max32665_startup_core0, maxusb, libff13, ble, bhy1],
   link_with: [api_dispatcher_lib, freertos],
   link_whole: [max32665_startup_core0_lib, board_card10_lib, newlib_heap_lib],
   include_directories: [freertos_includes],

epicardium/modules/bhi.c

+#include <stdio.h>
+#include <string.h>
+
+#include "gpio.h"
+#include "bhy_uc_driver.h"
+#include "bhy.h"
+#include "pmic.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "semphr.h"
+#include "queue.h"
+
+#include "epicardium.h"
+#include "modules/log.h"
+#include "modules/modules.h"
+#include "modules/stream.h"
+
+/* Ticks to wait when trying to acquire lock */
+#define LOCK_WAIT pdMS_TO_TICKS(BHI160_MUTEX_WAIT_MS)
+
+/* BHI160 Firmware Blob.  Contents are defined in libcard10. */
+extern uint8_t bhy1_fw[];
+
+/* Interrupt Pin */
+static const gpio_cfg_t bhi160_interrupt_pin = {
+	PORT_0, PIN_13, GPIO_FUNC_IN, GPIO_PAD_PULL_UP
+};
+
+/* Axis remapping matrices */
+static int8_t bhi160_mapping_matrix[3 * 3] = { 0, -1, 0, 1, 0, 0, 0, 0, 1 };
+static int8_t bmm150_mapping_matrix[3 * 3] = { -1, 0, 0, 0, 1, 0, 0, 0, -1 };
+
+/*
+ * From the official docs:
+ *
+ *    The sic matrix should be calculated for customer platform by logging
+ *    uncalibrated magnetometer data.  The sic matrix here is only an example
+ *    array (identity matrix). Customer should generate their own matrix.  This
+ *    affects magnetometer fusion performance.
+ *
+ * TODO: Get data for card10
+ */
+/* clang-format off */
+static float bhi160_sic_array[3 * 3] = { 1.0, 0.0, 0.0,
+                                         0.0, 1.0, 0.0,
+                                         0.0, 0.0, 1.0 };
+/* clang-format on */
+
+/* BHI160 Fifo */
+static uint8_t bhi160_fifo[BHI160_FIFO_SIZE];
+static size_t start_index = 0;
+
+/* BHI160 Task ID */
+static TaskHandle_t bhi160_task_id = NULL;
+
+/* BHI160 Mutex */
+static StaticSemaphore_t bhi160_mutex_data;
+static SemaphoreHandle_t bhi160_mutex = NULL;
+
+/* Streams */
+static struct stream_info bhi160_streams[10];
+
+/* -- Utilities -------------------------------------------------------- {{{ */
+/*
+ * Retrieve the data size for a sensor.  This value is needed for the creation
+ * of the sensor's sample queue.
+ */
+static size_t bhi160_lookup_data_size(enum bhi160_sensor_type type)
+{
+	switch (type) {
+	case BHI160_ACCELEROMETER:
+	case BHI160_MAGNETOMETER:
+	case BHI160_ORIENTATION:
+		return sizeof(struct bhi160_data_vector);
+	default:
+		return 0;
+	}
+}
+
+/*
+ * Map a sensor type to the virtual sensor ID used by BHy1.
+ */
+static bhy_virtual_sensor_t bhi160_lookup_vs_id(enum bhi160_sensor_type type)
+{
+	switch (type) {
+	case BHI160_ACCELEROMETER:
+		return VS_ID_ACCELEROMETER;
+	default:
+		return -1;
+	}
+}
+
+/*
+ * Map a sensor type to its stream descriptor.
+ */
+static int bhi160_lookup_sd(enum bhi160_sensor_type type)
+{
+	switch (type) {
+	case BHI160_ACCELEROMETER:
+		return SD_BHI160_ACCELEROMETER;
+	default:
+		return -1;
+	}
+}
+/* }}} */
+
+/* -- API -------------------------------------------------------------- {{{ */
+int epic_bhi160_enable_sensor(
+	enum bhi160_sensor_type sensor_type,
+	struct bhi160_sensor_config *config
+) {
+	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
+	if (vs_id < 0) {
+		return -ENODEV;
+	}
+
+	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) == pdTRUE) {
+		struct stream_info *stream = &bhi160_streams[sensor_type];
+		stream->item_size = bhi160_lookup_data_size(sensor_type);
+		/* TODO: Sanity check length */
+		stream->queue = xQueueCreate(
+			config->sample_buffer_len, stream->item_size
+		);
+		if (stream->queue == NULL) {
+			xSemaphoreGive(bhi160_mutex);
+			return -ENOMEM;
+		}
+
+		stream_register(bhi160_lookup_sd(sensor_type), stream);
+
+		bhy_enable_virtual_sensor(
+			vs_id,
+			VS_WAKEUP,
+			config->sample_rate,
+			0,
+			VS_FLUSH_NONE,
+			0,
+			config->dynamic_range /* dynamic range is sensor dependent */
+		);
+		xSemaphoreGive(bhi160_mutex);
+	} else {
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
+{
+	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
+	if (vs_id < 0) {
+		return -ENODEV;
+	}
+
+	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) == pdTRUE) {
+		struct stream_info *stream = &bhi160_streams[sensor_type];
+		stream_deregister(bhi160_lookup_sd(sensor_type), stream);
+		vQueueDelete(stream->queue);
+		stream->queue = NULL;
+
+		bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
+		xSemaphoreGive(bhi160_mutex);
+	} else {
+		return -EBUSY;
+	}
+
+	return 0;
+}
+/* }}} */
+
+/* -- Driver ----------------------------------------------------------- {{{ */
+/*
+ * Handle a single packet from the FIFO.  For most sensors this means pushing
+ * the sample into its sample queue.
+ */
+static void
+bhi160_handle_packet(bhy_data_type_t data_type, bhy_data_generic_t *sensor_data)
+{
+	uint8_t sensor_id = sensor_data->data_vector.sensor_id;
+	struct bhi160_data_vector data_vector;
+	/*
+	 * Timestamp of the next samples, counting at 32 kHz.
+	 * Currently unused.
+	 */
+	static uint32_t timestamp = 0;
+
+	switch (sensor_id) {
+	case VS_ID_TIMESTAMP_MSW:
+	case VS_ID_TIMESTAMP_MSW_WAKEUP:
+		MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
+		timestamp = sensor_data->data_scalar_u16.data << 16;
+		break;
+	case VS_ID_TIMESTAMP_LSW:
+	case VS_ID_TIMESTAMP_LSW_WAKEUP:
+		MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
+		timestamp = (timestamp & 0xFFFF0000) |
+			    sensor_data->data_scalar_u16.data;
+		break;
+	case VS_ID_ACCELEROMETER:
+	case VS_ID_ACCELEROMETER_WAKEUP:
+		MXC_ASSERT(data_type == BHY_DATA_TYPE_VECTOR);
+		if (bhi160_streams[BHI160_ACCELEROMETER].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_ACCELEROMETER].queue,
+			&data_vector,
+			BHI160_MUTEX_WAIT_MS
+		);
+		break;
+	default:
+		break;
+	}
+}
+
+/*
+ * Fetch all data available from BHI160's FIFO buffer and handle all packets
+ * contained in it.
+ */
+static int bhi160_fetch_fifo(void)
+{
+	/*
+	 * Warning:  The code from the BHy1 docs has some issues.  This
+	 * implementation looks similar, but has a few important differences.
+	 * You'll probably be best of leaving it as it is ...
+	 */
+
+	int ret = BHY_SUCCESS;
+	/* Number of bytes left in BHI160's FIFO buffer */
+	uint16_t bytes_left_in_fifo = 1;
+
+	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
+		return -EBUSY;
+	}
+
+	while (bytes_left_in_fifo) {
+		/* Fill local FIFO buffer with as many bytes as possible */
+		uint16_t bytes_read;
+		bhy_read_fifo(
+			&bhi160_fifo[start_index],
+			BHI160_FIFO_SIZE - start_index,
+			&bytes_read,
+			&bytes_left_in_fifo
+		);
+
+		/* Add the bytes left from the last transfer on top */
+		bytes_read += start_index;
+
+		/* Handle all full packets received in this transfer */
+		uint8_t *fifo_ptr   = bhi160_fifo;
+		uint16_t bytes_left = bytes_read;
+		while (ret == BHY_SUCCESS &&
+		       bytes_left > sizeof(bhy_data_generic_t)) {
+			/*
+			 * TODO: sizeof(bhy_data_generic_t) is probably
+			 * incorrect and makes some measurements arrive late.
+			 */
+			bhy_data_generic_t sensor_data;
+			bhy_data_type_t data_type;
+			ret = bhy_parse_next_fifo_packet(
+				&fifo_ptr,
+				&bytes_left,
+				&sensor_data,
+				&data_type
+			);
+
+			if (ret == BHY_SUCCESS) {
+				bhi160_handle_packet(data_type, &sensor_data);
+			}
+		}
+
+		/* Shift the remaining bytes to the beginning */
+		for (int i = 0; i < bytes_left; i++) {
+			bhi160_fifo[i] =
+				bhi160_fifo[bytes_read - bytes_left + i];
+		}
+		start_index = bytes_left;
+	}
+
+	xSemaphoreGive(bhi160_mutex);
+	return 0;
+}
+
+/*
+ * Callback for the BHI160 interrupt pin.  This callback is called from the
+ * SDK's GPIO interrupt driver, in interrupt context.
+ */
+static void bhi160_interrupt_callback(void *_)
+{
+	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+
+	if (bhi160_task_id != NULL) {
+		vTaskNotifyGiveFromISR(
+			bhi160_task_id, &xHigherPriorityTaskWoken
+		);
+		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+	}
+}
+/* }}} */
+
+void vBhi160Task(void *pvParameters)
+{
+	int ret;
+
+	bhi160_task_id = xTaskGetCurrentTaskHandle();
+	bhi160_mutex   = xSemaphoreCreateMutexStatic(&bhi160_mutex_data);
+
+	/* Take Mutex during initialization, just in case */
+	if (xSemaphoreTake(bhi160_mutex, 0) != pdTRUE) {
+		LOG_CRIT("bhi160", "Failed to acquire BHI160 mutex!");
+		vTaskDelay(portMAX_DELAY);
+	}
+
+	memset(bhi160_streams, 0x00, sizeof(bhi160_streams));
+
+	/* Install interrupt callback */
+	GPIO_Config(&bhi160_interrupt_pin);
+	GPIO_RegisterCallback(
+		&bhi160_interrupt_pin, bhi160_interrupt_callback, NULL
+	);
+	GPIO_IntConfig(&bhi160_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_RISING);
+	GPIO_IntEnable(&bhi160_interrupt_pin);
+	NVIC_SetPriority(
+		(IRQn_Type)MXC_GPIO_GET_IRQ(bhi160_interrupt_pin.port), 2
+	);
+	NVIC_EnableIRQ((IRQn_Type)MXC_GPIO_GET_IRQ(bhi160_interrupt_pin.port));
+
+	/* Upload firmware */
+	ret = bhy_driver_init(bhy1_fw);
+	if (ret) {
+		LOG_CRIT("bhi160", "BHy1 init failed!");
+		vTaskDelay(portMAX_DELAY);
+	}
+
+	/* Wait for first two interrupts */
+	ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
+	ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
+
+	/* Remap axes to match card10 layout */
+	bhy_mapping_matrix_set(
+		PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
+	);
+	bhy_mapping_matrix_set(
+		PHYSICAL_SENSOR_INDEX_MAG, bmm150_mapping_matrix
+	);
+	bhy_mapping_matrix_set(
+		PHYSICAL_SENSOR_INDEX_GYRO, bhi160_mapping_matrix
+	);
+
+	/* Set "SIC" matrix.  TODO: Find out what this is about */
+	bhy_set_sic_matrix(bhi160_sic_array);
+
+	xSemaphoreGive(bhi160_mutex);
+
+	/* ----------------------------------------- */
+
+	while (1) {
+		int ret = bhi160_fetch_fifo();
+		if (ret == -EBUSY) {
+			LOG_WARN("bhi160", "Could not acquire mutex for FIFO?");
+			continue;
+		} else if (ret < 0) {
+			LOG_ERR("bhi160", "Unknown error: %d", -ret);
+		}
+
+		/*
+		 * Wait for interrupt.  After two seconds, fetch FIFO anyway in
+		 * case there are any diagnostics or errors.
+		 *
+		 * In the future, reads using epic_stream_read() might also
+		 * trigger a FIFO fetch, from outside this task.
+		 */
+		ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(2000));
+	}
+}

epicardium/modules/meson.build

 module_sources = files(
+  'bhi.c',
   'bme680.c',
   'buttons.c',
   'dispatcher.c',

epicardium/modules/modules.h

@@ -84,4 +84,10 @@ int hwlock_release(enum hwlock_periph p);
 /* Forces an unlock of the display. Only to be used in Epicardium */
 void disp_forcelock();
 
+/* ---------- BHI160 ------------------------------------------------------- */
+#define BHI160_FIFO_SIZE             128
+#define BHI160_MUTEX_WAIT_MS          50
+void vBhi160Task(void *pvParameters);
+
+
 #endif /* MODULES_H */

epicardium/modules/stream.h

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