diff --git a/Documentation/pycardium/bhi160.rst b/Documentation/pycardium/bhi160.rst
index 350f43c72b9bffbfcfea8b398f572d270ac3e491..1aaa362e0f238177fe59e2310f0659fd8b51ac89 100644
--- a/Documentation/pycardium/bhi160.rst
+++ b/Documentation/pycardium/bhi160.rst
@@ -12,8 +12,16 @@ The coordinate system of the BHI160 sensor data looks like this:
.. image:: ../static/bhi160-coordinates.png
-All angles and angular velocities (like gyroscope, orientation) rotate counter
-clockwise around their respective axis.
+* The **accelerometer** axes are just the ones shown in the picture.
+* The **gyroscope**'s angular velocities rotate counter clockwise around
+ their respective axis.
+* The **orientation** sensor uses the following mapping:
+
+ +---------------------+----------------------+-------------------+
+ | X | Y | Z |
+ +=====================+======================+===================+
+ | Azimuth (0° - 360°) | Pitch (-180° - 180°) | Roll (-90° - 90°) |
+ +---------------------+----------------------+-------------------+
**Example**:
diff --git a/epicardium/FreeRTOSConfig.h b/epicardium/FreeRTOSConfig.h
index 22c6dfb0f050c38c2d33b7f1d64b1e7940d8ea97..8ff4a4b468f9ea938e92491aa41eafb9f5dad1e3 100644
--- a/epicardium/FreeRTOSConfig.h
+++ b/epicardium/FreeRTOSConfig.h
@@ -50,6 +50,7 @@
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelay 1
+#define INCLUDE_vTaskDelete 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
#define INCLUDE_xTimerPendFunctionCall 1
#define INCLUDE_xSemaphoreGetMutexHolder 1
diff --git a/epicardium/modules/bhi.c b/epicardium/modules/bhi.c
index 1332c12ea0c3164b56087d45c4099fa21e894ec4..c767a8cc24979bb661e6421d90161e7c95151151 100644
--- a/epicardium/modules/bhi.c
+++ b/epicardium/modules/bhi.c
@@ -9,7 +9,6 @@
#include "FreeRTOS.h"
#include "task.h"
-#include "semphr.h"
#include "queue.h"
#include "api/interrupt-sender.h"
@@ -18,9 +17,6 @@
#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[];
@@ -57,8 +53,7 @@ static size_t start_index = 0;
static TaskHandle_t bhi160_task_id = NULL;
/* BHI160 Mutex */
-static StaticSemaphore_t bhi160_mutex_data;
-static SemaphoreHandle_t bhi160_mutex = NULL;
+static struct mutex bhi160_mutex = { 0 };
/* Streams */
static struct stream_info bhi160_streams[10];
@@ -66,6 +61,13 @@ static struct stream_info bhi160_streams[10];
/* Active */
static bool bhi160_sensor_active[10] = { 0 };
+/*
+ * Driver State: A flag that is set when an unrecoverable error occurred.
+ * Effectively, this means the sensor will be disabled until next reboot and any
+ * API calls will fail immediately.
+ */
+static bool bhi160_driver_b0rked = false;
+
/* -- Utilities -------------------------------------------------------- {{{ */
/*
* Retrieve the data size for a sensor. This value is needed for the creation
@@ -135,14 +137,12 @@ int epic_bhi160_enable_sensor(
return -ENODEV;
}
- result = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
- if (result < 0) {
- return result;
- }
+ mutex_lock(&bhi160_mutex);
+ hwlock_acquire(HWLOCK_I2C);
- if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
- result = -EBUSY;
- goto out_free_i2c;
+ if (bhi160_driver_b0rked) {
+ result = -ENODEV;
+ goto out_free;
}
struct stream_info *stream = &bhi160_streams[sensor_type];
@@ -152,12 +152,12 @@ int epic_bhi160_enable_sensor(
xQueueCreate(config->sample_buffer_len, stream->item_size);
if (stream->queue == NULL) {
result = -ENOMEM;
- goto out_free_both;
+ goto out_free;
}
result = stream_register(bhi160_lookup_sd(sensor_type), stream);
if (result < 0) {
- goto out_free_both;
+ goto out_free;
}
result = bhy_enable_virtual_sensor(
@@ -170,16 +170,16 @@ int epic_bhi160_enable_sensor(
config->dynamic_range /* dynamic range is sensor dependent */
);
if (result != BHY_SUCCESS) {
- goto out_free_both;
+ goto out_free;
}
bhi160_sensor_active[sensor_type] = true;
- result = bhi160_lookup_sd(sensor_type);
+ /* Return the sensor stream descriptor */
+ result = bhi160_lookup_sd(sensor_type);
-out_free_both:
- xSemaphoreGive(bhi160_mutex);
-out_free_i2c:
+out_free:
hwlock_release(HWLOCK_I2C);
+ mutex_unlock(&bhi160_mutex);
return result;
}
@@ -192,36 +192,33 @@ int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
return -ENODEV;
}
- result = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
- if (result < 0) {
- return result;
- }
+ mutex_lock(&bhi160_mutex);
+ hwlock_acquire(HWLOCK_I2C);
- if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
- result = -EBUSY;
- goto out_free_i2c;
+ if (bhi160_driver_b0rked) {
+ result = -ENODEV;
+ goto out_free;
}
struct stream_info *stream = &bhi160_streams[sensor_type];
result = stream_deregister(bhi160_lookup_sd(sensor_type), stream);
if (result < 0) {
- goto out_free_both;
+ goto out_free;
}
vQueueDelete(stream->queue);
stream->queue = NULL;
result = bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
if (result < 0) {
- goto out_free_both;
+ goto out_free;
}
bhi160_sensor_active[sensor_type] = false;
result = 0;
-out_free_both:
- xSemaphoreGive(bhi160_mutex);
-out_free_i2c:
+out_free:
hwlock_release(HWLOCK_I2C);
+ mutex_unlock(&bhi160_mutex);
return result;
}
@@ -347,15 +344,8 @@ static int bhi160_fetch_fifo(void)
/* Number of bytes left in BHI160's FIFO buffer */
uint16_t bytes_left_in_fifo = 1;
- result = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
- if (result < 0) {
- return result;
- }
-
- if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
- result = -EBUSY;
- goto out_free_i2c;
- }
+ mutex_lock(&bhi160_mutex);
+ hwlock_acquire(HWLOCK_I2C);
while (bytes_left_in_fifo) {
/* Fill local FIFO buffer with as many bytes as possible */
@@ -398,9 +388,8 @@ static int bhi160_fetch_fifo(void)
start_index = bytes_left;
}
- xSemaphoreGive(bhi160_mutex);
-out_free_i2c:
hwlock_release(HWLOCK_I2C);
+ mutex_unlock(&bhi160_mutex);
return result;
}
@@ -412,7 +401,7 @@ static void bhi160_interrupt_callback(void *_)
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
- if (bhi160_task_id != NULL) {
+ if (bhi160_task_id != NULL && !bhi160_driver_b0rked) {
vTaskNotifyGiveFromISR(
bhi160_task_id, &xHigherPriorityTaskWoken
);
@@ -426,33 +415,27 @@ void vBhi160Task(void *pvParameters)
int ret;
bhi160_task_id = xTaskGetCurrentTaskHandle();
- bhi160_mutex = xSemaphoreCreateMutexStatic(&bhi160_mutex_data);
- /*
- * Wait a little before initializing BHI160.
- */
- vTaskDelay(pdMS_TO_TICKS(3));
-
- int lockret = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
- if (lockret < 0) {
- LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
- vTaskDelay(portMAX_DELAY);
- }
-
- /* Take Mutex during initialization, just in case */
- if (xSemaphoreTake(bhi160_mutex, 0) != pdTRUE) {
- LOG_CRIT("bhi160", "Failed to acquire BHI160 mutex!");
- vTaskDelay(portMAX_DELAY);
- }
+ mutex_create(&bhi160_mutex);
+ mutex_lock(&bhi160_mutex);
+ hwlock_acquire(HWLOCK_I2C);
memset(bhi160_streams, 0x00, sizeof(bhi160_streams));
- /* Install interrupt callback */
+ /*
+ * The BHI160, coming out of power-on-reset will hold its interrupt line
+ * high until a firmware image is loaded. Once that firmware is loaded
+ * and running, the interrupt line is deasserted and from then on,
+ * interrupts are signaled using a rising edge.
+ *
+ * So, initially we need to configure the IRQ for a falling edge, load
+ * the firmware and then reconfigure for a rising edge.
+ */
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_IntConfig(&bhi160_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_FALLING);
GPIO_IntEnable(&bhi160_interrupt_pin);
NVIC_SetPriority(
(IRQn_Type)MXC_GPIO_GET_IRQ(bhi160_interrupt_pin.port), 2
@@ -462,25 +445,37 @@ void vBhi160Task(void *pvParameters)
/* Upload firmware */
ret = bhy_driver_init(bhy1_fw);
if (ret) {
- LOG_CRIT("bhi160", "BHy1 init failed!");
- vTaskDelay(portMAX_DELAY);
+ LOG_CRIT("bhi160", "BHy1 init failed! Disabling.");
+
+ /* Disable BHI160 until next reboot */
+ bhi160_driver_b0rked = true;
+ hwlock_release(HWLOCK_I2C);
+ mutex_unlock(&bhi160_mutex);
+ vTaskDelete(NULL);
}
- /* Wait for first interrupt */
+ /* Wait for first interrupt, a falling edge */
hwlock_release(HWLOCK_I2C);
- ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
- lockret = hwlock_acquire_timeout(HWLOCK_I2C, portMAX_DELAY);
- if (lockret < 0) {
- LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
- vTaskDelay(portMAX_DELAY);
+ if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(2000)) == 0) {
+ LOG_CRIT(
+ "bhi160",
+ "Sensor firmware was not loaded correctly. Disabling."
+ );
+
+ /* Disable BHI160 until next reboot */
+ bhi160_driver_b0rked = true;
+ mutex_unlock(&bhi160_mutex);
+ vTaskDelete(NULL);
}
+ hwlock_acquire(HWLOCK_I2C);
+
+ /*
+ * The firmware is now loaded; as stated above, we now need to
+ * reconfigure the IRQ for a rising edge.
+ */
+ GPIO_IntConfig(&bhi160_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_RISING);
/* Remap axes to match card10 layout */
- /* Due to a known issue (#133) the first call to
- * bhy_mapping_matrix_set might fail. */
- bhy_mapping_matrix_set(
- PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
- );
bhy_mapping_matrix_set(
PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
);
@@ -494,8 +489,8 @@ void vBhi160Task(void *pvParameters)
/* Set "SIC" matrix. TODO: Find out what this is about */
bhy_set_sic_matrix(bhi160_sic_array);
- xSemaphoreGive(bhi160_mutex);
hwlock_release(HWLOCK_I2C);
+ mutex_unlock(&bhi160_mutex);
/* ----------------------------------------- */
diff --git a/preload/apps/bhi160/__init__.py b/preload/apps/bhi160/__init__.py
index c1f4325092a83b4b23975f9057bec7b2caee3e67..a6de8fc23ade4b2b34d05f5f32031cd5dd12fb6b 100644
--- a/preload/apps/bhi160/__init__.py
+++ b/preload/apps/bhi160/__init__.py
@@ -14,19 +14,23 @@ STATUS_COLORS = [
color.GREEN,
]
-with contextlib.ExitStack() as cx:
- disp = cx.enter_context(display.open())
+def sensors(**args):
+ while True:
+ with bhi160.BHI160Orientation(**args) as s:
+ yield s, "Orientation"
+ with bhi160.BHI160Accelerometer(**args) as s:
+ yield s, "Accel"
+ with bhi160.BHI160Gyroscope(**args) as s:
+ yield s, "Gyro"
+ with bhi160.BHI160Magnetometer(**args) as s:
+ yield s, "Magnetic"
+
+
+with display.open() as disp:
args = {"sample_rate": 10, "sample_buffer_len": 20}
- sensor_iter = itertools.cycle(
- [
- (cx.enter_context(bhi160.BHI160Orientation(**args)), "Orientation"),
- (cx.enter_context(bhi160.BHI160Accelerometer(**args)), "Accel"),
- (cx.enter_context(bhi160.BHI160Gyroscope(**args)), "Gyro"),
- (cx.enter_context(bhi160.BHI160Magnetometer(**args)), "Magnetic"),
- ]
- )
+ sensor_iter = sensors(**args)
sensor, sensor_name = next(sensor_iter)
for ev in simple_menu.button_events(timeout=0.1):