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

#include "epicardium.h"
#include "modules/log.h"
#include "modules/modules.h"
#include "modules/stream.h"

/* 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 */
55
static struct mutex bhi160_mutex = { 0 };
56
57
58
59

/* Streams */
static struct stream_info bhi160_streams[10];

60
61
62
/* Active */
static bool bhi160_sensor_active[10] = { 0 };

63
64
65
66
67
68
69
/*
 * 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;

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

134
	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
135
	if (vs_id == (bhy_virtual_sensor_t)-1) {
136
137
138
		return -ENODEV;
	}

139
140
	mutex_lock(&bhi160_mutex);
	hwlock_acquire(HWLOCK_I2C);
141

142
143
144
145
146
	if (bhi160_driver_b0rked) {
		result = -ENODEV;
		goto out_free;
	}

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

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

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

koalo's avatar
koalo committed
175
	bhi160_sensor_active[sensor_type] = true;
176
177
	/* Return the sensor stream descriptor */
	result = bhi160_lookup_sd(sensor_type);
koalo's avatar
koalo committed
178

179
out_free:
koalo's avatar
koalo committed
180
	hwlock_release(HWLOCK_I2C);
181
	mutex_unlock(&bhi160_mutex);
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
	bhy_virtual_sensor_t vs_id = bhi160_lookup_vs_id(sensor_type);
190
	if (vs_id == (bhy_virtual_sensor_t)-1) {
191
192
193
		return -ENODEV;
	}

194
195
	mutex_lock(&bhi160_mutex);
	hwlock_acquire(HWLOCK_I2C);
196

197
198
199
200
201
	if (bhi160_driver_b0rked) {
		result = -ENODEV;
		goto out_free;
	}

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

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

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

	result = 0;
218
out_free:
koalo's avatar
koalo committed
219
	hwlock_release(HWLOCK_I2C);
220
	mutex_unlock(&bhi160_mutex);
koalo's avatar
koalo committed
221
	return result;
222
}
223
224
225

void epic_bhi160_disable_all_sensors()
{
226
	for (size_t i = 0; i < sizeof(bhi160_sensor_active); i++) {
227
228
229
230
231
232
		if (bhi160_sensor_active[i]) {
			epic_bhi160_disable_sensor(i);
		}
	}
}

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

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

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

303
		assert(data_type == BHY_DATA_TYPE_VECTOR);
304
		if (bhi160_streams[sensor_type].queue == NULL) {
koalo's avatar
koalo committed
305
306
			break;
		}
307
308
309
310
		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
311
		data_vector.status    = sensor_data->data_vector.status;
312
313
314
315
316
317

		/* Discard overflow.  See discussion in !316. */
		if (xQueueSend(
			    bhi160_streams[sensor_type].queue,
			    &data_vector,
			    0) != pdTRUE) {
318
319
320
321
322
323
324
325
326
327
			if (!bhi160_streams[sensor_type].was_full) {
				LOG_WARN(
					"bhi160",
					"queue full for %d",
					sensor_type
				);
			}
			bhi160_streams[sensor_type].was_full = true;
		} else {
			bhi160_streams[sensor_type].was_full = false;
328
329
		}

330
		if (wakeup) {
331
			interrupt_trigger(epic_int);
koalo's avatar
koalo committed
332
333
		}
		break;
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
	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
351
	int result = 0;
352
353
354
	/* Number of bytes left in BHI160's FIFO buffer */
	uint16_t bytes_left_in_fifo = 1;

355
356
	mutex_lock(&bhi160_mutex);
	hwlock_acquire(HWLOCK_I2C);
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373

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

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

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

koalo's avatar
koalo committed
399
	hwlock_release(HWLOCK_I2C);
400
	mutex_unlock(&bhi160_mutex);
koalo's avatar
koalo committed
401
	return result;
402
403
404
405
406
407
408
409
410
411
}

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

412
	if (bhi160_task_id != NULL && !bhi160_driver_b0rked) {
413
414
415
416
417
418
419
420
421
422
423
424
425
		vTaskNotifyGiveFromISR(
			bhi160_task_id, &xHigherPriorityTaskWoken
		);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
}
/* }}} */

void vBhi160Task(void *pvParameters)
{
	int ret;

	bhi160_task_id = xTaskGetCurrentTaskHandle();
426

427
428
429
	mutex_create(&bhi160_mutex);
	mutex_lock(&bhi160_mutex);
	hwlock_acquire(HWLOCK_I2C);
430
431
432

	memset(bhi160_streams, 0x00, sizeof(bhi160_streams));

433
434
435
436
437
438
439
440
441
	/*
	 * 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.
	 */
442
443
444
445
	GPIO_Config(&bhi160_interrupt_pin);
	GPIO_RegisterCallback(
		&bhi160_interrupt_pin, bhi160_interrupt_callback, NULL
	);
446
	GPIO_IntConfig(&bhi160_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_FALLING);
447
448
449
450
451
452
453
454
455
	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) {
456
457
458
459
460
461
462
		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);
463
464
	}

465
	/* Wait for first interrupt, a falling edge */
koalo's avatar
koalo committed
466
	hwlock_release(HWLOCK_I2C);
467
468
469
470
471
472
473
474
475
476
477
	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);
	}
478
	hwlock_acquire(HWLOCK_I2C);
479

480
481
482
483
484
485
	/*
	 * 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);

486
487
488
489
490
491
492
493
494
495
	/*
	 * Remap axes to match card10 layout.
	 *
	 * TODO: We set the matrix for the accelerometer twice because on some
	 * badges, the axis mapping is not applied properly the first time.  We
	 * should fix this properly at some point.
	 */
	bhy_mapping_matrix_set(
		PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
	);
496
497
498
499
500
501
502
503
504
505
506
507
508
	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);

koalo's avatar
koalo committed
509
	hwlock_release(HWLOCK_I2C);
510
	mutex_unlock(&bhi160_mutex);
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532

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

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