bhi.c 12.7 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;
94
95
	case BHI160_MAGNETOMETER:
		return VS_ID_MAGNETOMETER;
koalo's avatar
koalo committed
96
97
	case BHI160_ORIENTATION:
		return VS_ID_ORIENTATION;
koalo's avatar
koalo committed
98
99
	case BHI160_GYROSCOPE:
		return VS_ID_GYROSCOPE;
100
101
102
103
104
105
106
107
108
109
110
111
112
	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;
113
114
	case BHI160_MAGNETOMETER:
		return SD_BHI160_MAGNETOMETER;
koalo's avatar
koalo committed
115
116
	case BHI160_ORIENTATION:
		return SD_BHI160_ORIENTATION;
koalo's avatar
koalo committed
117
118
	case BHI160_GYROSCOPE:
		return SD_BHI160_GYROSCOPE;
119
120
121
122
123
124
125
126
127
128
129
	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
130
131
	int result = 0;

132
133
134
135
136
	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
	if (vs_id < 0) {
		return -ENODEV;
	}

koalo's avatar
koalo committed
137
138
139
	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (result < 0) {
		return result;
koalo's avatar
koalo committed
140
141
	}

koalo's avatar
koalo committed
142
143
144
145
	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
		result = -EBUSY;
		goto out_free_i2c;
	}
146

koalo's avatar
koalo committed
147
148
149
150
151
152
153
154
155
	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;
	}
156

koalo's avatar
koalo committed
157
158
159
160
	result = stream_register(bhi160_lookup_sd(sensor_type), stream);
	if (result < 0) {
		goto out_free_both;
	}
161

koalo's avatar
koalo committed
162
163
164
165
166
167
168
169
170
171
172
	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;
173
174
	}

koalo's avatar
koalo committed
175
176
177
178
179
180
	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
181
	hwlock_release(HWLOCK_I2C);
koalo's avatar
koalo committed
182
	return result;
183
184
185
186
}

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

189
190
191
192
193
	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
	if (vs_id < 0) {
		return -ENODEV;
	}

koalo's avatar
koalo committed
194
195
196
	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (result < 0) {
		return result;
koalo's avatar
koalo committed
197
198
	}

koalo's avatar
koalo committed
199
200
201
202
	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
		result = -EBUSY;
		goto out_free_i2c;
	}
203

koalo's avatar
koalo committed
204
205
206
207
208
	struct stream_info *stream = &bhi160_streams[sensor_type];
	result = stream_deregister(bhi160_lookup_sd(sensor_type), stream);
	if (result < 0) {
		goto out_free_both;
	}
209

koalo's avatar
koalo committed
210
211
212
213
214
	vQueueDelete(stream->queue);
	stream->queue = NULL;
	result        = bhy_disable_virtual_sensor(vs_id, VS_WAKEUP);
	if (result < 0) {
		goto out_free_both;
215
216
	}

koalo's avatar
koalo committed
217
218
219
220
221
222
	bhi160_sensor_active[sensor_type] = false;

	result = 0;
out_free_both:
	xSemaphoreGive(bhi160_mutex);
out_free_i2c:
koalo's avatar
koalo committed
223
	hwlock_release(HWLOCK_I2C);
koalo's avatar
koalo committed
224
	return result;
225
}
226
227
228
229
230
231
232
233
234
235

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

236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
/* }}} */

/* -- 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.
	 */
252
253
254
255
	static uint32_t timestamp           = 0;
	enum bhi160_sensor_type sensor_type = 0;
	int epic_int                        = 0;
	bool wakeup                         = false;
256
257
258

	switch (sensor_id) {
	case VS_ID_TIMESTAMP_MSW_WAKEUP:
259
260
261
		wakeup = true;
		/* fall through */
	case VS_ID_TIMESTAMP_MSW:
262
263
264
265
		MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
		timestamp = sensor_data->data_scalar_u16.data << 16;
		break;
	case VS_ID_TIMESTAMP_LSW_WAKEUP:
266
267
268
		wakeup = true;
		/* fall through */
	case VS_ID_TIMESTAMP_LSW:
269
270
271
272
273
		MXC_ASSERT(data_type == BHY_DATA_TYPE_SCALAR_U16);
		timestamp = (timestamp & 0xFFFF0000) |
			    sensor_data->data_scalar_u16.data;
		break;
	case VS_ID_ACCELEROMETER_WAKEUP:
274
	case VS_ID_MAGNETOMETER_WAKEUP:
koalo's avatar
koalo committed
275
	case VS_ID_ORIENTATION_WAKEUP:
276
277
278
279
	case VS_ID_GYROSCOPE_WAKEUP:
		wakeup = true;
		/* fall through */
	case VS_ID_ACCELEROMETER:
280
	case VS_ID_MAGNETOMETER:
koalo's avatar
koalo committed
281
	case VS_ID_ORIENTATION:
282
283
284
285
286
287
288
	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;
289
290
291
292
293
		case VS_ID_MAGNETOMETER_WAKEUP:
		case VS_ID_MAGNETOMETER:
			sensor_type = BHI160_MAGNETOMETER;
			epic_int    = EPIC_INT_BHI160_MAGNETOMETER;
			break;
koalo's avatar
koalo committed
294
295
296
297
298
		case VS_ID_ORIENTATION_WAKEUP:
		case VS_ID_ORIENTATION:
			sensor_type = BHI160_ORIENTATION;
			epic_int    = EPIC_INT_BHI160_ORIENTATION;
			break;
299
300
301
302
		case VS_ID_GYROSCOPE_WAKEUP:
		case VS_ID_GYROSCOPE:
			sensor_type = BHI160_GYROSCOPE;
			epic_int    = EPIC_INT_BHI160_GYROSCOPE;
303
304
			break;
		}
305

koalo's avatar
koalo committed
306
		MXC_ASSERT(data_type == BHY_DATA_TYPE_VECTOR);
307
		if (bhi160_streams[sensor_type].queue == NULL) {
koalo's avatar
koalo committed
308
309
			break;
		}
310
311
312
313
		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
314
		data_vector.status    = sensor_data->data_vector.status;
koalo's avatar
koalo committed
315
		xQueueSend(
316
			bhi160_streams[sensor_type].queue,
koalo's avatar
koalo committed
317
318
319
			&data_vector,
			BHI160_MUTEX_WAIT_MS
		);
320
321
		if (wakeup) {
			api_interrupt_trigger(epic_int);
koalo's avatar
koalo committed
322
323
		}
		break;
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
	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
341
	int result = 0;
342
343
344
	/* Number of bytes left in BHI160's FIFO buffer */
	uint16_t bytes_left_in_fifo = 1;

koalo's avatar
koalo committed
345
346
347
	result = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (result < 0) {
		return result;
koalo's avatar
koalo committed
348
349
	}

350
	if (xSemaphoreTake(bhi160_mutex, LOCK_WAIT) != pdTRUE) {
koalo's avatar
koalo committed
351
352
		result = -EBUSY;
		goto out_free_i2c;
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
	}

	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;
371
		while (bytes_left > 0) {
372
373
			bhy_data_generic_t sensor_data;
			bhy_data_type_t data_type;
koalo's avatar
koalo committed
374
			result = bhy_parse_next_fifo_packet(
375
376
377
378
379
380
				&fifo_ptr,
				&bytes_left,
				&sensor_data,
				&data_type
			);

koalo's avatar
koalo committed
381
			if (result == BHY_SUCCESS) {
382
				bhi160_handle_packet(data_type, &sensor_data);
383
384
			} else {
				break;
385
386
387
388
389
390
391
392
393
394
395
396
			}
		}

		/* 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
397
out_free_i2c:
koalo's avatar
koalo committed
398
	hwlock_release(HWLOCK_I2C);
koalo's avatar
koalo committed
399
	return result;
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
}

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

426
427
428
	/*
	 * Wait a little before initializing BHI160.
	 */
Rahix's avatar
Rahix committed
429
	vTaskDelay(pdMS_TO_TICKS(3));
430

koalo's avatar
koalo committed
431
432
	int lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (lockret < 0) {
koalo's avatar
koalo committed
433
434
		LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
		vTaskDelay(portMAX_DELAY);
koalo's avatar
koalo committed
435
436
	}

437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
	/* 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
464
465
	/* Wait for first interrupt */
	hwlock_release(HWLOCK_I2C);
466
	ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
koalo's avatar
koalo committed
467
468
469
470
471
	lockret = hwlock_acquire(HWLOCK_I2C, pdMS_TO_TICKS(100));
	if (lockret < 0) {
		LOG_CRIT("bhi160", "Failed to acquire I2C lock!");
		vTaskDelay(portMAX_DELAY);
	}
472
473

	/* Remap axes to match card10 layout */
474
475
476
477
478
	/* 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
	);
479
480
481
482
483
484
485
486
487
488
489
490
491
492
	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
493
	hwlock_release(HWLOCK_I2C);
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515

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

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