79 lines
2.3 KiB
C
79 lines
2.3 KiB
C
#include "imu/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);
|
|
imu_sample_t measurement = {
|
|
.accel_mps2 = {1.0, -2.0, 9.7},
|
|
.gyro_rps = {0.01, -0.02, 0.03},
|
|
.temperature_c = 41.0
|
|
};
|
|
imu_sample_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;
|
|
|
|
imu_sample_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;
|
|
}
|