bhi.c 13 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;
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;
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:
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:
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:
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:
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;
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 318 319 320

		/* Discard overflow.  See discussion in !316. */
		if (xQueueSend(
			    bhi160_streams[sensor_type].queue,
			    &data_vector,
			    0) != pdTRUE) {
			LOG_WARN("bhi160", "queue full for %d", sensor_type);
		}

321
		if (wakeup) {
322
			interrupt_trigger(epic_int);
koalo's avatar
koalo committed
323 324
		}
		break;
325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341
	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
342
	int result = 0;
343 344 345
	/* Number of bytes left in BHI160's FIFO buffer */
	uint16_t bytes_left_in_fifo = 1;

346 347
	mutex_lock(&bhi160_mutex);
	hwlock_acquire(HWLOCK_I2C);
348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364

	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;
365
		while (bytes_left > 0) {
366 367
			bhy_data_generic_t sensor_data;
			bhy_data_type_t data_type;
koalo's avatar
koalo committed
368
			result = bhy_parse_next_fifo_packet(
369 370 371 372 373 374
				&fifo_ptr,
				&bytes_left,
				&sensor_data,
				&data_type
			);

koalo's avatar
koalo committed
375
			if (result == BHY_SUCCESS) {
376
				bhi160_handle_packet(data_type, &sensor_data);
377 378
			} else {
				break;
379 380 381 382 383 384 385 386 387 388 389
			}
		}

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

390
	hwlock_release(HWLOCK_I2C);
391
	mutex_unlock(&bhi160_mutex);
koalo's avatar
koalo committed
392
	return result;
393 394 395 396 397 398 399 400 401 402
}

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

403
	if (bhi160_task_id != NULL && !bhi160_driver_b0rked) {
404 405 406 407 408 409 410 411 412 413 414 415 416
		vTaskNotifyGiveFromISR(
			bhi160_task_id, &xHigherPriorityTaskWoken
		);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
}
/* }}} */

void vBhi160Task(void *pvParameters)
{
	int ret;

	bhi160_task_id = xTaskGetCurrentTaskHandle();
417

418 419 420
	mutex_create(&bhi160_mutex);
	mutex_lock(&bhi160_mutex);
	hwlock_acquire(HWLOCK_I2C);
421 422 423

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

424 425 426 427 428 429 430 431 432
	/*
	 * 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.
	 */
433 434 435 436
	GPIO_Config(&bhi160_interrupt_pin);
	GPIO_RegisterCallback(
		&bhi160_interrupt_pin, bhi160_interrupt_callback, NULL
	);
437
	GPIO_IntConfig(&bhi160_interrupt_pin, GPIO_INT_EDGE, GPIO_INT_FALLING);
438 439 440 441 442 443 444 445 446
	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) {
447 448 449 450 451 452 453
		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);
454 455
	}

456
	/* Wait for first interrupt, a falling edge */
koalo's avatar
koalo committed
457
	hwlock_release(HWLOCK_I2C);
458 459 460 461 462 463 464 465 466 467 468
	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);
	}
469
	hwlock_acquire(HWLOCK_I2C);
470

471 472 473 474 475 476
	/*
	 * 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);

477 478 479 480 481 482 483 484 485 486
	/*
	 * 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
	);
487 488 489 490 491 492 493 494 495 496 497 498 499
	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);

500
	hwlock_release(HWLOCK_I2C);
501
	mutex_unlock(&bhi160_mutex);
502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523

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

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