Merge branch 'rahix/bhi160-cleanup' into 'master'

BHI160 cleanup

See merge request card10/firmware!423

epicardium/modules/bhi.c

@@ -24,9 +24,18 @@ static const gpio_cfg_t bhi160_interrupt_pin = {
 	PORT_0, PIN_13, GPIO_FUNC_IN, GPIO_PAD_PULL_UP
 };
 
+/* clang-format off */
 /* 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 };
+static const int8_t bhi160_mapping_matrix[3 * 3] = {
+	 0, -1,  0,
+	 1,  0,  0,
+	 0,  0,  1,
+};
+static const int8_t bmm150_mapping_matrix[3 * 3] = {
+	-1,  0,  0,
+	 0,  1,  0,
+	 0,  0, -1,
+};
 
 /*
  * From the official docs:
@@ -38,16 +47,13 @@ static int8_t bmm150_mapping_matrix[3 * 3] = { -1, 0, 0, 0, 1, 0, 0, 0, -1 };
  *
  * 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 };
+static const float bhi160_sic_array[3 * 3] = {
+	1.0f, 0.0f, 0.0f,
+	0.0f, 1.0f, 0.0f,
+	0.0f, 0.0f, 1.0f,
+};
 /* 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;
 
@@ -340,7 +346,7 @@ bhi160_handle_packet(bhy_data_type_t data_type, bhy_data_generic_t *sensor_data)
  * Fetch all data available from BHI160's FIFO buffer and handle all packets
  * contained in it.
  */
-static int bhi160_fetch_fifo(void)
+static void bhi160_fetch_fifo(void)
 {
 	/*
 	 * Warning:  The code from the BHy1 docs has some issues.  This
@@ -348,7 +354,14 @@ static int bhi160_fetch_fifo(void)
 	 * You'll probably be best of leaving it as it is ...
 	 */
 
-	int result = 0;
+	/*
+	 * FIFO buffer.  Access to this static variable is safe because this
+	 * function is guarded by the bhi160_mutex.
+	 */
+	static uint8_t bhi160_fifo[BHI160_FIFO_SIZE];
+	static size_t start_index = 0;
+
+	BHY_RETURN_FUNCTION_TYPE result = 0;
 	/* Number of bytes left in BHI160's FIFO buffer */
 	uint16_t bytes_left_in_fifo = 1;
 
@@ -358,13 +371,34 @@ static int bhi160_fetch_fifo(void)
 	while (bytes_left_in_fifo) {
 		/* Fill local FIFO buffer with as many bytes as possible */
 		uint16_t bytes_read;
-		bhy_read_fifo(
+		result = bhy_read_fifo(
 			&bhi160_fifo[start_index],
 			BHI160_FIFO_SIZE - start_index,
 			&bytes_read,
 			&bytes_left_in_fifo
 		);
 
+		if (result != BHY_SUCCESS) {
+			/*
+			 * Honestly not sure how we should handle these errors.
+			 *
+			 * Needs more digging to find out how bhy_read_fifo()
+			 * behaves in error situations (a quick glance at the
+			 * code shows that the function might have written
+			 * _somethings_ into our buffer so we can't just retry
+			 * blindly ...)
+			 *
+			 * For now, just abort everything and disable the
+			 * sensor.  Won't cause havoc at least ...
+			 */
+			LOG_ERR("bhi160",
+				"Error while reading fifo: %d.  Disabling.",
+				result);
+
+			bhi160_driver_b0rked = true;
+			break;
+		}
+
 		/* Add the bytes left from the last transfer on top */
 		bytes_read += start_index;
 
@@ -384,6 +418,11 @@ static int bhi160_fetch_fifo(void)
 			if (result == BHY_SUCCESS) {
 				bhi160_handle_packet(data_type, &sensor_data);
 			} else {
+				LOG_WARN(
+					"bhi160",
+					"Error in fifo packet: %d.  Ignoring.",
+					result
+				);
 				break;
 			}
 		}
@@ -398,7 +437,6 @@ static int bhi160_fetch_fifo(void)
 
 	hwlock_release(HWLOCK_I2C);
 	mutex_unlock(&bhi160_mutex);
-	return result;
 }
 
 /*
@@ -491,20 +529,20 @@ void vBhi160Task(void *pvParameters)
 	 * should fix this properly at some point.
 	 */
 	bhy_mapping_matrix_set(
-		PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
+		PHYSICAL_SENSOR_INDEX_ACC, (int8_t *)bhi160_mapping_matrix
 	);
 	bhy_mapping_matrix_set(
-		PHYSICAL_SENSOR_INDEX_ACC, bhi160_mapping_matrix
+		PHYSICAL_SENSOR_INDEX_ACC, (int8_t *)bhi160_mapping_matrix
 	);
 	bhy_mapping_matrix_set(
-		PHYSICAL_SENSOR_INDEX_MAG, bmm150_mapping_matrix
+		PHYSICAL_SENSOR_INDEX_MAG, (int8_t *)bmm150_mapping_matrix
 	);
 	bhy_mapping_matrix_set(
-		PHYSICAL_SENSOR_INDEX_GYRO, bhi160_mapping_matrix
+		PHYSICAL_SENSOR_INDEX_GYRO, (int8_t *)bhi160_mapping_matrix
 	);
 
 	/* Set "SIC" matrix.  TODO: Find out what this is about */
-	bhy_set_sic_matrix(bhi160_sic_array);
+	bhy_set_sic_matrix((float *)bhi160_sic_array);
 
 	hwlock_release(HWLOCK_I2C);
 	mutex_unlock(&bhi160_mutex);
@@ -512,13 +550,7 @@ void vBhi160Task(void *pvParameters)
 	/* ----------------------------------------- */
 
 	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);
-		}
+		bhi160_fetch_fifo();
 
 		/*
 		 * Wait for interrupt.  After two seconds, fetch FIFO anyway in