bhi.c 12.1 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
#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"

14
#include "api/interrupt-sender.h"
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
#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];

65
66
67
/* Active */
static bool bhi160_sensor_active[10] = { 0 };

68
69
70
71
72
73
74
75
76
77
78
/* -- 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:
koalo's avatar
koalo committed
79
	case BHI160_GYROSCOPE:
80
81
82
83
84
85
86
87
88
89
90
91
92
93
		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;
koalo's avatar
koalo committed
94
95
	case BHI160_ORIENTATION:
		return VS_ID_ORIENTATION;
koalo's avatar
koalo committed
96
97
	case BHI160_GYROSCOPE:
		return VS_ID_GYROSCOPE;
98
99
100
101
102
103
104
105
106
107
108
109
110
	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;
koalo's avatar
koalo committed
111
112
	case BHI160_ORIENTATION:
		return SD_BHI160_ORIENTATION;
koalo's avatar
koalo committed
113
114
	case BHI160_GYROSCOPE:
		return SD_BHI160_GYROSCOPE;
115
116
117
118
119
120
121
122
123
124
125
	default:
		return -1;
	}
}
/* }}} */

/* -- API -------------------------------------------------------------- {{{ */
int epic_bhi160_enable_sensor(
	enum bhi160_sensor_type sensor_type,
	struct bhi160_sensor_config *config
) {
koalo's avatar
koalo committed
126
127
	int result = 0;

128
129
130
131
132
	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
	if (vs_id < 0) {
		return -ENODEV;
	}

koalo's avatar
koalo committed
133
134
135
	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (result < 0) {
		return result;
koalo's avatar
koalo committed
136
137
	}

koalo's avatar
koalo committed
138
139
140
141
	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
		result = -EBUSY;
		goto out_free_i2c;
	}
142

koalo's avatar
koalo committed
143
144
145
146
147
148
149
150
151
	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) {
		result = -ENOMEM;
		goto out_free_both;
	}
152

koalo's avatar
koalo committed
153
154
155
156
	result = stream_register(bhi160_lookup_sd(sensor_type), stream);
	if (result < 0) {
		goto out_free_both;
	}
157

koalo's avatar
koalo committed
158
159
160
161
162
163
164
165
166
167
168
	result = bhy_enable_virtual_sensor(
		vs_id,
		VS_WAKEUP,
		config->sample_rate,
		0,
		VS_FLUSH_NONE,
		0,
		config->dynamic_range /* dynamic range is sensor dependent */
	);
	if (result != BHY_SUCCESS) {
		goto out_free_both;
169
170
	}

koalo's avatar
koalo committed
171
172
173
174
175
176
	bhi160_sensor_active[sensor_type] = true;
	result                            = bhi160_lookup_sd(sensor_type);

out_free_both:
	xSemaphoreGive(bhi160_mutex);
out_free_i2c:
koalo's avatar
koalo committed
177
	hwlock_release(HWLOCK_I2C);
koalo's avatar
koalo committed
178
	return result;
179
180
181
182
}

int epic_bhi160_disable_sensor(enum bhi160_sensor_type sensor_type)
{
koalo's avatar
koalo committed
183
184
	int result = 0;

185
186
187
188
189
	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
	if (vs_id < 0) {
		return -ENODEV;
	}

koalo's avatar
koalo committed
190
191
192
	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (result < 0) {
		return result;
koalo's avatar
koalo committed
193
194
	}

koalo's avatar
koalo committed
195
196
197
198
	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
		result = -EBUSY;
		goto out_free_i2c;
	}
199

koalo's avatar
koalo committed
200
201
202
203
204
	struct stream_info *stream = &bhi160_streams[sensor_type];
	result = stream_deregister(bhi160_lookup_sd(sensor_type), stream);
	if (result < 0) {
		goto out_free_both;
	}
205

koalo's avatar
koalo committed
206
207
208
209
210
	vQueueDelete(stream->queue);
	stream->queue = NULL;
	result        = bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
	if (result < 0) {
		goto out_free_both;
211
212
	}

koalo's avatar
koalo committed
213
214
215
216
217
218
	bhi160_sensor_active[sensor_type] = false;

	result = 0;
out_free_both:
	xSemaphoreGive(bhi160_mutex);
out_free_i2c:
koalo's avatar
koalo committed
219
	hwlock_release(HWLOCK_I2C);
koalo's avatar
koalo committed
220
	return result;
221
}
222
223
224
225
226
227
228
229
230
231

void epic_bhi160_disable_all_sensors()
{
	for (int i = 0; i < sizeof(bhi160_sensor_active); i++) {
		if (bhi160_sensor_active[i]) {
			epic_bhi160_disable_sensor(i);
		}
	}
}

232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
/* }}} */

/* -- 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.
	 */
248
249
250
251
	static uint32_t timestamp           = 0;
	enum bhi160_sensor_type sensor_type = 0;
	int epic_int                        = 0;
	bool wakeup                         = false;
252
253
254

	switch (sensor_id) {
	case VS_ID_TIMESTAMP_MSW_WAKEUP:
255
256
257
		wakeup = true;
		/* fall through */
	case VS_ID_TIMESTAMP_MSW:
258
259
260
261
		MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
		timestamp = sensor_data->data_scalar_u16.data << 16;
		break;
	case VS_ID_TIMESTAMP_LSW_WAKEUP:
262
263
264
		wakeup = true;
		/* fall through */
	case VS_ID_TIMESTAMP_LSW:
265
266
267
268
269
		MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
		timestamp = (timestamp & 0xFFFF0000) |
			    sensor_data->data_scalar_u16.data;
		break;
	case VS_ID_ACCELEROMETER_WAKEUP:
koalo's avatar
koalo committed
270
	case VS_ID_ORIENTATION_WAKEUP:
271
272
273
274
	case VS_ID_GYROSCOPE_WAKEUP:
		wakeup = true;
		/* fall through */
	case VS_ID_ACCELEROMETER:
koalo's avatar
koalo committed
275
	case VS_ID_ORIENTATION:
276
277
278
279
280
281
282
	case VS_ID_GYROSCOPE:
		switch (sensor_id) {
		case VS_ID_ACCELEROMETER_WAKEUP:
		case VS_ID_ACCELEROMETER:
			sensor_type = BHI160_ACCELEROMETER;
			epic_int    = EPIC_INT_BHI160_ACCELEROMETER;
			break;
koalo's avatar
koalo committed
283
284
285
286
287
		case VS_ID_ORIENTATION_WAKEUP:
		case VS_ID_ORIENTATION:
			sensor_type = BHI160_ORIENTATION;
			epic_int    = EPIC_INT_BHI160_ORIENTATION;
			break;
288
289
290
291
		case VS_ID_GYROSCOPE_WAKEUP:
		case VS_ID_GYROSCOPE:
			sensor_type = BHI160_GYROSCOPE;
			epic_int    = EPIC_INT_BHI160_GYROSCOPE;
292
293
			break;
		}
294

koalo's avatar
koalo committed
295
		MXC_ASSERT(data_type == BHY_DATA_TYPE_VECTOR);
296
		if (bhi160_streams[sensor_type].queue == NULL) {
koalo's avatar
koalo committed
297
298
			break;
		}
299
300
301
302
		data_vector.data_type = BHI160_DATA_TYPE_VECTOR;
		data_vector.x         = sensor_data->data_vector.x;
		data_vector.y         = sensor_data->data_vector.y;
		data_vector.z         = sensor_data->data_vector.z;
koalo's avatar
koalo committed
303
		data_vector.status    = sensor_data->data_vector.status;
koalo's avatar
koalo committed
304
		xQueueSend(
305
			bhi160_streams[sensor_type].queue,
koalo's avatar
koalo committed
306
307
308
			&data_vector,
			BHI160_MUTEX_WAIT_MS
		);
309
310
		if (wakeup) {
			api_interrupt_trigger(epic_int);
koalo's avatar
koalo committed
311
312
		}
		break;
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
	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 ...
	 */

koalo's avatar
koalo committed
330
	int result = 0;
331
332
333
	/* Number of bytes left in BHI160's FIFO buffer */
	uint16_t bytes_left_in_fifo = 1;

koalo's avatar
koalo committed
334
335
336
	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (result < 0) {
		return result;
koalo's avatar
koalo committed
337
338
	}

339
	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
koalo's avatar
koalo committed
340
341
		result = -EBUSY;
		goto out_free_i2c;
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
	}

	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;
360
		while (bytes_left > 0) {
361
362
			bhy_data_generic_t sensor_data;
			bhy_data_type_t data_type;
koalo's avatar
koalo committed
363
			result = bhy_parse_next_fifo_packet(
364
365
366
367
368
369
				&fifo_ptr,
				&bytes_left,
				&sensor_data,
				&data_type
			);

koalo's avatar
koalo committed
370
			if (result == BHY_SUCCESS) {
371
				bhi160_handle_packet(data_type, &sensor_data);
372
373
			} else {
				break;
374
375
376
377
378
379
380
381
382
383
384
385
			}
		}

		/* 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);
koalo's avatar
koalo committed
386
out_free_i2c:
koalo's avatar
koalo committed
387
	hwlock_release(HWLOCK_I2C);
koalo's avatar
koalo committed
388
	return result;
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
}

/*
 * 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);

koalo's avatar
koalo committed
415
416
	int lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (lockret < 0) {
koalo's avatar
koalo committed
417
418
		LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
		vTaskDelay(portMAX_DELAY);
koalo's avatar
koalo committed
419
420
	}

421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
	/* 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);
	}

koalo's avatar
koalo committed
448
449
	/* Wait for first interrupt */
	hwlock_release(HWLOCK_I2C);
450
	ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
koalo's avatar
koalo committed
451
452
453
454
455
	lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (lockret < 0) {
		LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
		vTaskDelay(portMAX_DELAY);
	}
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471

	/* 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);
koalo's avatar
koalo committed
472
	hwlock_release(HWLOCK_I2C);
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494

	/* ----------------------------------------- */

	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));
	}
}