Verified Commit 163954af authored by Rahix's avatar Rahix
Browse files

feat(epicardium): Add driver for BHI160



Related to #20.

Signed-off-by: Rahix's avatarRahix <rahix@rahix.de>
parent 83ad85a7
......@@ -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
* ===============
......
......@@ -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,
......
......@@ -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],
......
#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));
}
}
module_sources = files(
'bhi.c',
'bme680.c',
'buttons.c',
'dispatcher.c',
......
......@@ -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 */
......@@ -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,
};
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment