diff --git a/epicardium/epicardium.h b/epicardium/epicardium.h
index 4e52d85c51c38d897f38dbee03ed96cbbb7b0de1..00779c9cdc51c50ed8b7b99d9fc36f6569c5ab11 100644
--- a/epicardium/epicardium.h
+++ b/epicardium/epicardium.h
@@ -1,8 +1,9 @@
#ifndef _EPICARDIUM_H
#define _EPICARDIUM_H
-#include <stdint.h>
-#include <stddef.h>
#include <errno.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
#ifndef API
#define API(id, def) def
@@ -15,6 +16,8 @@
#define API_VIBRA_SET 0x4
#define API_VIBRA_VIBRATE 0x5
#define API_STREAM_READ 0x6
+#define API_BHI160_ENABLE 0x7
+#define API_BHI160_DISABLE 0x8
/* clang-format on */
/**
@@ -120,6 +123,160 @@ API(API_LEDS_SET, void epic_leds_set(int led, uint8_t r, uint8_t g, uint8_t b));
*/
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
+));
+
/**
* Misc
* ====
diff --git a/epicardium/main.c b/epicardium/main.c
index 51d365f2c0a754b1432dfe5ae14ddf95de620aee..188d4912d072581701bab92b779113a6d718862a 100644
--- a/epicardium/main.c
+++ b/epicardium/main.c
@@ -82,6 +82,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,
diff --git a/epicardium/meson.build b/epicardium/meson.build
index 26f36cd47e471bf658b35b6447a69489110197ea..ae787bab4f4111f4c1e040392986486bf6543839 100644
--- a/epicardium/meson.build
+++ b/epicardium/meson.build
@@ -70,7 +70,7 @@ elf = executable(
'main.c',
'support.c',
module_sources,
- dependencies: [libcard10, max32665_startup_core0, maxusb, libff13],
+ dependencies: [libcard10, max32665_startup_core0, maxusb, libff13, bhy1],
link_with: [api_dispatcher_lib, freertos],
link_whole: [max32665_startup_core0_lib, board_card10_lib],
include_directories: [freertos_includes],
diff --git a/epicardium/modules/bhi.c b/epicardium/modules/bhi.c
new file mode 100644
index 0000000000000000000000000000000000000000..7b8f0d789fe01f7c91eee0988c2ed84c2cd3f5bf
--- /dev/null
+++ b/epicardium/modules/bhi.c
@@ -0,0 +1,385 @@
+#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"
+
+/*
+ * Seriously, this BHy1 library is horrible. This is a best effort to wrap it
+ * in something usable which also hopefully won't break immediately. Fingers
+ * crossed.
+ *
+ * Don't read the official docs, the code in there is a mess. For example,
+ * there is a `bytes_left_in_fifo` variable which never actually set to a value
+ * besides the initial 0. The implementation in this file also has a variable
+ * named `bytes_left_in_fifo` but here it is actually used for a purpose related
+ * to its name.
+ */
+
+/* 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)
+{
+ 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));
+ }
+}
diff --git a/epicardium/modules/meson.build b/epicardium/modules/meson.build
index a038fb21d7f5ef7c650a10851503150a27c0b1eb..3936f4caa2032fd2095a6abf8aa1beda2365c220 100644
--- a/epicardium/modules/meson.build
+++ b/epicardium/modules/meson.build
@@ -1,4 +1,5 @@
module_sources = files(
+ 'bhi.c',
'fatfs.c',
'leds.c',
'log.c',
diff --git a/epicardium/modules/modules.h b/epicardium/modules/modules.h
index 0db3d11bf84ca07f8a65f499fa1497a55940968c..f8f32b588667d662e7b30b75856fd1e3b0122fcd 100644
--- a/epicardium/modules/modules.h
+++ b/epicardium/modules/modules.h
@@ -5,7 +5,7 @@
void fatfs_init(void);
/* ---------- Serial ------------------------------------------------------- */
-#define SERIAL_READ_BUFFER_SIZE 128
+#define SERIAL_READ_BUFFER_SIZE 128
void vSerialTask(void *pvParameters);
/* ---------- PMIC --------------------------------------------------------- */
@@ -14,4 +14,9 @@ void vSerialTask(void *pvParameters);
#define PMIC_PRESS_POWEROFF 40
void vPmicTask(void *pvParameters);
+/* ---------- BHI160 ------------------------------------------------------- */
+#define BHI160_FIFO_SIZE 128
+#define BHI160_MUTEX_WAIT_MS 50
+void vBhi160Task(void *pvParameters);
+
#endif /* MODULES_H */
diff --git a/epicardium/modules/stream.h b/epicardium/modules/stream.h
index 32c9e6e219bc7b7998babcd25236d2417d61b8c5..84bad37ab8b4e42029af129d7db392f5e73c9b0b 100644
--- a/epicardium/modules/stream.h
+++ b/epicardium/modules/stream.h
@@ -19,6 +19,8 @@
* Please keep IDs in sequential order.
*/
enum stream_descriptor {
+ /** BHI160 Accelerometer */
+ SD_BHI160_ACCELEROMETER,
/** Highest descriptor must always be ``SD_MAX``. */
SD_MAX,
};