Add generic IMU fusion and PMOD IRQ verification

soc/software/gps_system/tests/Makefile

@@ -0,0 +1,18 @@
+CC ?= gcc
+CFLAGS ?= -O2 -std=c11 -Wall -Wextra -Werror
+
+ROOT := ..
+TARGET := test_imu_calibration
+
+.PHONY: all run clean
+
+all: $(TARGET)
+
+$(TARGET): test_imu_calibration.c $(ROOT)/src/imu_calibration.c
+	$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
+
+run: $(TARGET)
+	./$(TARGET)
+
+clean:
+	rm -f $(TARGET)

soc/software/gps_system/tests/test_imu_calibration.c

@@ -0,0 +1,78 @@
+#include "imu_calibration.h"
+
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+static bool near(double actual, double expected)
+{
+	return fabs(actual - expected) < 1.0e-12;
+}
+
+static void test_identity_is_noop(void)
+{
+	imu_calibration_t calibration;
+	imu_calibration_default(&calibration);
+	fusion_imu_measurement_t measurement = {
+		.accel_mps2 = {1.0, -2.0, 9.7},
+		.gyro_rps = {0.01, -0.02, 0.03},
+		.temperature_c = 41.0
+	};
+	fusion_imu_measurement_t expected = measurement;
+	assert(imu_calibration_apply(&calibration, &measurement));
+	assert(memcmp(measurement.accel_mps2, expected.accel_mps2,
+	              sizeof(measurement.accel_mps2)) == 0);
+	assert(memcmp(measurement.gyro_rps, expected.gyro_rps,
+	              sizeof(measurement.gyro_rps)) == 0);
+}
+
+static void test_bias_transform_and_temperature(void)
+{
+	imu_calibration_t calibration;
+	imu_calibration_default(&calibration);
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		calibration.accel_bias_mps2[axis] = 1.0;
+		calibration.gyro_bias_rps[axis] = 0.1;
+		calibration.accel_temperature_coefficient_mps2_per_c[axis] = 0.1;
+		calibration.gyro_temperature_coefficient_rps_per_c[axis] = 0.01;
+	}
+	memset(calibration.accel_transform, 0,
+	       sizeof(calibration.accel_transform));
+	calibration.accel_transform[0][1] = 1.0;
+	calibration.accel_transform[1][0] = -1.0;
+	calibration.accel_transform[2][2] = 2.0;
+	calibration.reference_temperature_c = 25.0;
+	calibration.temperature_compensation_enabled = true;
+
+	fusion_imu_measurement_t measurement = {
+		.accel_mps2 = {3.0, 5.0, 7.0},
+		.gyro_rps = {0.4, 0.5, 0.6},
+		.temperature_c = 35.0
+	};
+	assert(imu_calibration_apply(&calibration, &measurement));
+	assert(near(measurement.accel_mps2[0], 3.0));
+	assert(near(measurement.accel_mps2[1], -1.0));
+	assert(near(measurement.accel_mps2[2], 10.0));
+	assert(near(measurement.gyro_rps[0], 0.2));
+	assert(near(measurement.gyro_rps[1], 0.3));
+	assert(near(measurement.gyro_rps[2], 0.4));
+}
+
+static void test_singular_transform_is_rejected(void)
+{
+	imu_calibration_t calibration;
+	imu_calibration_default(&calibration);
+	memset(calibration.gyro_transform, 0, sizeof(calibration.gyro_transform));
+	assert(!imu_calibration_is_valid(&calibration));
+}
+
+int main(void)
+{
+	test_identity_is_noop();
+	test_bias_transform_and_temperature();
+	test_singular_transform_is_rejected();
+	puts("imu_calibration tests passed");
+	return 0;
+}