test(fusion): guard the deadlock and calibration regressions, add noise tool
The existing suite passed both before and after the fusion fixes, so it never covered either failure. Two tests close that gap; both were checked against the pre-fix sources and fail there. calibration_offset_test reproduces the bench part's +28 mg zero-g offset and requires startup to complete, while still refusing a sustained 3 m/s2 acceleration. It also asserts the blind spot rather than leaving it implicit: horizontal acceleration adds to gravity in quadrature, so 1.5 m/s2 moves the magnitude by only 0.11 m/s2 and is not separable from offset by any accelerometer gate. Separating it needs GNSS velocity. long_outage_trajectory_test drives pedestrian, gentle-car (0-100 km/h in 30 s), slow-city and isolated-mount profiles across 1 and 5 minute outages, using the production rest supervisor rather than a hand-set flag. Only the isolated-mount slow-city case reproduces the field bug: GNSS motion evidence compares displacement against covariance, so 4 m/s under 6 m sigma scores a displacement NIS of 0.22 against a 9.21 gate and reads as stationary, whereas highway speed scores 42.9 and is correctly called moving. Pre-fix that case produces 608 zero-velocity updates while driving, matching the 609 in the board logs. Drift is judged against the filter's own covariance rather than a fixed distance. Five minutes unaided legitimately reaches kilometres on this hardware; what must not happen is the error outrunning the covariance, which is divergence rather than honest coasting. tdk_noise_test characterises the sensor at rest through the production driver, so ODR, full scale, UI filter and LSB scaling match the flight build, and reports the norms fusion_motion_update() actually gates on. It also carries an INT1-to-PLIC continuity test that drives the pin to a static level and reads the PLIC pending bit directly, separating a broken line from a source that never fires. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
onur cayan
@@ -10,7 +10,7 @@ IMU_PROFILE_SOURCE := ../../gps_system/src/adapters/imu_fusion_profile.c
|
||||
|
||||
.PHONY: all test gps-system-test scenario-logs sanitize profile benchmark clean
|
||||
|
||||
all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test
|
||||
all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test calibration_offset_test long_outage_trajectory_test
|
||||
|
||||
standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE) $(GPS_EPOCH_SOURCE)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
@@ -42,7 +42,13 @@ platform_scenario_matrix_test: platform_scenario_matrix_test.c $(ESKF_SOURCES)
|
||||
gps_failure_scenario_test: gps_failure_scenario_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test
|
||||
calibration_offset_test: calibration_offset_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
long_outage_trajectory_test: long_outage_trajectory_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test calibration_offset_test long_outage_trajectory_test
|
||||
./standalone_gps_integrity_test
|
||||
./eskf_motion_test
|
||||
./fusion_rest_detector_test
|
||||
@@ -53,6 +59,8 @@ test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test f
|
||||
./gps_reacquisition_test
|
||||
./platform_scenario_matrix_test
|
||||
./gps_failure_scenario_test
|
||||
./calibration_offset_test
|
||||
./long_outage_trajectory_test
|
||||
|
||||
gps-system-test: standalone_gps_integrity_test
|
||||
./standalone_gps_integrity_test
|
||||
@@ -146,4 +154,4 @@ benchmark: standalone_gps_integrity_test
|
||||
./standalone_gps_integrity_test --bench-double 100000
|
||||
|
||||
clean:
|
||||
rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_rest_detector_test fusion_rest_detector_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gps_failure_scenario_test gps_failure_scenario_test_san gmon.out
|
||||
rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_rest_detector_test fusion_rest_detector_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gps_failure_scenario_test gps_failure_scenario_test_san calibration_offset_test calibration_offset_test_san long_outage_trajectory_test long_outage_trajectory_test_san gmon.out
|
||||
|
||||
@@ -0,0 +1,180 @@
|
||||
/*
|
||||
* Startup calibration must complete on a sensor whose zero-g offset is within
|
||||
* datasheet spec, and must still refuse to calibrate while the platform is
|
||||
* accelerating.
|
||||
*
|
||||
* The bench ICM-42688-P on this project measures a stationary accelerometer
|
||||
* magnitude of 10.0847 m/s^2 against a 9.80665 m/s^2 reference: a +28 mg
|
||||
* zero-g offset, comfortably in spec. When one tolerance served both as the
|
||||
* sensor-offset allowance and as the motion gate, that offset consumed 93% of
|
||||
* it and left 0.022 m/s^2 of motion headroom, so ordinary sensor noise reset
|
||||
* the accumulator and the engine never left FUSION_ENGINE_CALIBRATING. Field
|
||||
* logs show exactly that: calibration_samples oscillating 0,3,0,4,2,1,0,7...
|
||||
* and never reaching calibration_min_samples.
|
||||
*
|
||||
* Widening a single combined tolerance would have restored startup at the cost
|
||||
* of the motion gate, so the offset case and the acceleration case are both
|
||||
* asserted here.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "adapters/imu_fusion_profile.h"
|
||||
#include "fusion/fusion_engine.h"
|
||||
#include "imu/imu_sensor.h"
|
||||
|
||||
#define TIMEBASE_HZ 1000000u
|
||||
#define IMU_HZ 50u
|
||||
#define DURATION_S 8u
|
||||
#define GRAVITY_MPS2 9.80665
|
||||
|
||||
/* Measured on the bench unit: see soc/software/tdk_noise_test. */
|
||||
#define MEASURED_Z_OFFSET_MPS2 0.278
|
||||
#define MEASURED_ACCEL_NOISE_MPS2 0.0028
|
||||
#define MEASURED_GYRO_NOISE_RPS 0.00016
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
static uint32_t random_state = 12345u;
|
||||
|
||||
static double uniform_random(void)
|
||||
{
|
||||
random_state = 1664525u * random_state + 1013904223u;
|
||||
return ((double)((random_state >> 8) & 0x00ffffffu) + 0.5) / 16777216.0;
|
||||
}
|
||||
|
||||
static double gaussian_random(void)
|
||||
{
|
||||
double first = fmax(uniform_random(), 1.0e-12);
|
||||
double second = uniform_random();
|
||||
return sqrt(-2.0 * log(first)) * cos(2.0 * 3.14159265358979323846 * second);
|
||||
}
|
||||
|
||||
/*
|
||||
* Runs a stationary (or uniformly accelerating) calibration window and reports
|
||||
* whether the engine reached the running state.
|
||||
*/
|
||||
static bool run_calibration(double z_offset_mps2,
|
||||
double horizontal_acceleration_mps2,
|
||||
imu_sensor_backend_t backend,
|
||||
unsigned int* samples_out)
|
||||
{
|
||||
fusion_imu_quality_profile_t profile;
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_t engine;
|
||||
|
||||
fusion_engine_default_config(&config, TIMEBASE_HZ);
|
||||
if(!imu_fusion_quality_profile_for_backend(backend, &profile))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(!fusion_engine_apply_imu_quality(&config, &profile))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(!fusion_engine_init(&engine, &config))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
random_state = 12345u;
|
||||
unsigned int total = DURATION_S * IMU_HZ;
|
||||
for(unsigned int i = 0u ; i < total ; i++)
|
||||
{
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks =
|
||||
(uint64_t)(i + 1u) * (uint64_t)(TIMEBASE_HZ / IMU_HZ);
|
||||
imu.accel_mps2[0] = horizontal_acceleration_mps2 +
|
||||
MEASURED_ACCEL_NOISE_MPS2 * gaussian_random();
|
||||
imu.accel_mps2[1] = MEASURED_ACCEL_NOISE_MPS2 * gaussian_random();
|
||||
imu.accel_mps2[2] = GRAVITY_MPS2 + z_offset_mps2 +
|
||||
MEASURED_ACCEL_NOISE_MPS2 * gaussian_random();
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
imu.gyro_rps[axis] = MEASURED_GYRO_NOISE_RPS * gaussian_random();
|
||||
}
|
||||
imu.temperature_c = 25.0;
|
||||
imu.sequence = i;
|
||||
imu.vehicle_at_rest = true;
|
||||
(void)fusion_engine_push_imu(&engine, &imu);
|
||||
}
|
||||
|
||||
fusion_runtime_t runtime;
|
||||
bool running = fusion_engine_get_runtime(&engine, &runtime);
|
||||
if(samples_out != NULL)
|
||||
{
|
||||
*samples_out = engine.calibration.sample_count;
|
||||
}
|
||||
return running;
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
unsigned int samples = 0u;
|
||||
|
||||
/* The regression: an in-spec zero-g offset must not block startup. */
|
||||
CHECK(run_calibration(MEASURED_Z_OFFSET_MPS2, 0.0,
|
||||
IMU_SENSOR_TDK_ICM42688, &samples),
|
||||
"TDK calibration must complete with the measured +28 mg zero-g offset");
|
||||
printf("bench offset z=%.3f m/s^2 accel_norm=%.4f -> %s\n",
|
||||
MEASURED_Z_OFFSET_MPS2, GRAVITY_MPS2 + MEASURED_Z_OFFSET_MPS2,
|
||||
failures == 0 ? "calibrated" : "STUCK");
|
||||
|
||||
/* A part right at the datasheet limit must still start. */
|
||||
CHECK(run_calibration(0.392, 0.0, IMU_SENSOR_TDK_ICM42688, NULL),
|
||||
"TDK calibration must complete at the +-40 mg datasheet offset limit");
|
||||
|
||||
/* The same must hold for the ST backend. */
|
||||
CHECK(run_calibration(MEASURED_Z_OFFSET_MPS2, 0.0,
|
||||
IMU_SENSOR_ST_LSM6DSOX, NULL),
|
||||
"ST calibration must complete with an in-spec zero-g offset");
|
||||
|
||||
/*
|
||||
* The motion gate must survive the offset allowance. Calibrating under
|
||||
* power would align attitude to a tilted specific-force vector, so a
|
||||
* sustained acceleration large enough to be visible in the magnitude has
|
||||
* to be rejected on both an ideal and an offset part.
|
||||
*/
|
||||
CHECK(!run_calibration(0.0, 3.0, IMU_SENSOR_TDK_ICM42688, NULL),
|
||||
"calibration must refuse a 3.0 m/s^2 sustained acceleration");
|
||||
CHECK(!run_calibration(MEASURED_Z_OFFSET_MPS2, 3.0,
|
||||
IMU_SENSOR_TDK_ICM42688, NULL),
|
||||
"calibration must refuse 3.0 m/s^2 on an offset part too");
|
||||
|
||||
/*
|
||||
* Documented blind spot, asserted so it stays a known quantity rather than
|
||||
* an assumption. Horizontal acceleration adds to gravity in quadrature,
|
||||
* so 1.5 m/s^2 moves the magnitude by only 0.11 m/s^2 -- below any usable
|
||||
* offset allowance, and below the original 0.30 tolerance as well. This
|
||||
* is not a regression; separating it needs GNSS velocity, not a tighter
|
||||
* accelerometer gate.
|
||||
*/
|
||||
CHECK(run_calibration(0.0, 1.5, IMU_SENSOR_TDK_ICM42688, NULL),
|
||||
"1.5 m/s^2 is expected to remain indistinguishable from offset");
|
||||
|
||||
/* A grossly wrong magnitude still indicates a misconfigured range. */
|
||||
CHECK(!run_calibration(5.0, 0.0, IMU_SENSOR_TDK_ICM42688, NULL),
|
||||
"calibration must refuse an implausible accelerometer magnitude");
|
||||
|
||||
if(failures != 0)
|
||||
{
|
||||
fprintf(stderr, "FAIL: %d calibration offset check(s) failed\n",
|
||||
failures);
|
||||
return 1;
|
||||
}
|
||||
printf("PASS: in-spec zero-g offset calibrates; sustained acceleration and "
|
||||
"implausible magnitude are still refused.\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,816 @@
|
||||
/*
|
||||
* Pedestrian and gentle-vehicle trajectories across 1 and 5 minute GNSS
|
||||
* outages.
|
||||
*
|
||||
* What this is actually checking
|
||||
* -----------------------------
|
||||
* Five minutes of inertial-only navigation on a consumer MEMS part drifts by
|
||||
* hundreds of metres to kilometres. That is physics, not a defect: a residual
|
||||
* gyro bias of only 1.6e-5 rad/s -- what a 2 s static calibration leaves
|
||||
* behind at this sensor's measured noise -- tilts the gravity projection by
|
||||
* 0.27 deg over 300 s and integrates to roughly 2 km. Asserting a small
|
||||
* outage error would therefore only be asserting that the synthetic IMU is
|
||||
* unrealistically good.
|
||||
*
|
||||
* The properties worth defending are the ones that separate honest dead
|
||||
* reckoning from a filter that has come apart:
|
||||
*
|
||||
* 1. every published state stays finite, with no step discontinuities
|
||||
* 2. drift accumulates smoothly rather than diverging super-quadratically
|
||||
* 3. GNSS re-acquisition pulls the solution back within seconds
|
||||
* 4. constant-velocity cruise is never mistaken for standing still
|
||||
*
|
||||
* Property 4 is the field failure this suite exists for. A cruising vehicle
|
||||
* produces almost no specific force and almost no angular rate, so a rest
|
||||
* detector fed only inertial data declares rest. These cases therefore assert
|
||||
* vehicle_at_rest during constant-velocity cruise on purpose, reproducing that
|
||||
* mistake, and require the filter to keep tracking anyway on the strength of
|
||||
* GNSS Doppler velocity.
|
||||
*
|
||||
* Property 3 is the regression guard for the innovation-gate lockout: once the
|
||||
* filter had drifted far enough for every fix to fail the NIS gate, nothing
|
||||
* could re-anchor it and the reported position stayed on the dead-reckoned
|
||||
* track indefinitely.
|
||||
*
|
||||
* IMU noise and bias below are the values measured on this project's bench
|
||||
* ICM-42688-P; see soc/software/tdk_noise_test.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "adapters/imu_fusion_profile.h"
|
||||
#include "fusion/fusion_engine.h"
|
||||
#include "fusion/fusion_rest_detector.h"
|
||||
#include "imu/imu_sensor.h"
|
||||
|
||||
#define TIMEBASE_HZ 1000000u
|
||||
#define IMU_HZ 50u
|
||||
#define GPS_PERIOD_MS 200u
|
||||
#define PI 3.14159265358979323846
|
||||
|
||||
/* Bench-measured stationary noise (soc/software/tdk_noise_test). */
|
||||
#define ACCEL_NOISE_MPS2 0.0028
|
||||
#define GYRO_NOISE_RPS 0.00016
|
||||
/* Residual bias left after a short static calibration. */
|
||||
#define ACCEL_BIAS_MPS2 0.010
|
||||
#define GYRO_BIAS_RPS 0.00004
|
||||
|
||||
/*
|
||||
* Broadband platform vibration. A purely kinematic trajectory has none, which
|
||||
* would flatter the rest detector. These are set at the per-sample sigma the
|
||||
* TDK profile's process noise already assumes at 50 Hz (0.125 m/s^2 and
|
||||
* 0.011 rad/s), so the synthetic stream stays consistent with what the filter
|
||||
* expects; injecting more would make the covariance check fail for modelling
|
||||
* reasons rather than for filter behaviour.
|
||||
*/
|
||||
#define ROAD_VIBRATION_MPS2 0.125
|
||||
#define ROAD_VIBRATION_RPS 0.011
|
||||
|
||||
#define GPS_HORIZONTAL_STD_M 3.0
|
||||
#define GPS_VERTICAL_STD_M 5.0
|
||||
#define GPS_VELOCITY_STD_MPS 0.3
|
||||
|
||||
/* Timeline shared by every case. */
|
||||
#define STATIONARY_UNTIL_S 10.0
|
||||
#define ACCEL_START_S 10.0
|
||||
#define OUTAGE_START_S 90.0
|
||||
#define RECOVERY_OBSERVE_S 60.0
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
typedef enum {
|
||||
PROFILE_PEDESTRIAN = 0,
|
||||
PROFILE_GENTLE_CAR,
|
||||
/*
|
||||
* The field failure's operating point. GNSS motion evidence compares the
|
||||
* displacement between fixes against their combined covariance, so slow
|
||||
* travel under mediocre accuracy reads as stationary: 4 m/s over the 1 s
|
||||
* window is 4 m of displacement against 6 m per-fix sigma, a displacement
|
||||
* NIS of 0.22 versus a 9.21 gate. Highway speed with good accuracy scores
|
||||
* 42.9 and is correctly called moving, which is why fast cruise alone does
|
||||
* not exercise the zero-velocity lockout.
|
||||
*/
|
||||
PROFILE_SLOW_CITY,
|
||||
/*
|
||||
* The field configuration. Board logs show 609 zero-velocity updates while
|
||||
* the vehicle was driving at 3-5 m/s, which only happens if the mounting
|
||||
* isolates the sensor from road vibration well enough for the rest
|
||||
* supervisor's vibration metric to stay under its limit. Suppressing
|
||||
* vibration here reproduces that: the supervisor genuinely declares rest
|
||||
* while moving, and the engine must survive being told so.
|
||||
*/
|
||||
PROFILE_ISOLATED_CITY,
|
||||
/*
|
||||
* Repeated hard course changes while unaided. Answers whether the path
|
||||
* shape survives dead reckoning, separately from how far the whole path
|
||||
* drifts: heading comes from integrating a directly measured angular rate,
|
||||
* whereas position comes from double-integrating a specific force that
|
||||
* carries the attitude error times gravity. The two degrade at very
|
||||
* different rates, so the turns are placed inside the outage window.
|
||||
*/
|
||||
PROFILE_URBAN_TURNS
|
||||
} profile_kind_t;
|
||||
|
||||
#define PROFILE_COUNT 5u
|
||||
|
||||
typedef struct {
|
||||
double position_enu_m[3];
|
||||
double velocity_enu_mps[3];
|
||||
double acceleration_enu_mps2[3];
|
||||
double yaw_rad;
|
||||
double yaw_rate_rps;
|
||||
double speed_mps;
|
||||
bool constant_velocity;
|
||||
} truth_t;
|
||||
|
||||
static uint32_t random_state;
|
||||
|
||||
static double uniform_random(void)
|
||||
{
|
||||
random_state = 1664525u * random_state + 1013904223u;
|
||||
return ((double)((random_state >> 8) & 0x00ffffffu) + 0.5) / 16777216.0;
|
||||
}
|
||||
|
||||
static double gaussian_random(void)
|
||||
{
|
||||
double first = fmax(uniform_random(), 1.0e-12);
|
||||
double second = uniform_random();
|
||||
return sqrt(-2.0 * log(first)) * cos(2.0 * PI * second);
|
||||
}
|
||||
|
||||
static const char* profile_name(profile_kind_t profile)
|
||||
{
|
||||
switch(profile)
|
||||
{
|
||||
case PROFILE_PEDESTRIAN: return "pedestrian";
|
||||
case PROFILE_GENTLE_CAR: return "gentle-car";
|
||||
case PROFILE_SLOW_CITY: return "slow-city";
|
||||
case PROFILE_ISOLATED_CITY: return "isolated-city";
|
||||
default: return "urban-turns";
|
||||
}
|
||||
}
|
||||
|
||||
/* Slow urban travel is where the receiver reports its weakest geometry. */
|
||||
static double profile_gps_horizontal_std_m(profile_kind_t profile)
|
||||
{
|
||||
return ((profile == PROFILE_SLOW_CITY) ||
|
||||
(profile == PROFILE_ISOLATED_CITY) ||
|
||||
(profile == PROFILE_URBAN_TURNS)) ? 6.0 : GPS_HORIZONTAL_STD_M;
|
||||
}
|
||||
|
||||
/*
|
||||
* Longitudinal speed and yaw rate.
|
||||
*
|
||||
* The vehicle case is the requested 0 to 100 km/h in 30 s, a constant
|
||||
* 0.926 m/s^2, followed by open-road cruise. Cruise is where specific force
|
||||
* returns to gravity alone and the platform becomes inertially
|
||||
* indistinguishable from parked.
|
||||
*/
|
||||
static void profile_command(profile_kind_t profile, double time_s,
|
||||
double* speed_mps, double* yaw_rate_rps,
|
||||
double* acceleration_mps2,
|
||||
bool* constant_velocity)
|
||||
{
|
||||
*yaw_rate_rps = 0.0;
|
||||
*acceleration_mps2 = 0.0;
|
||||
*constant_velocity = false;
|
||||
|
||||
if(time_s < STATIONARY_UNTIL_S)
|
||||
{
|
||||
*speed_mps = 0.0;
|
||||
*constant_velocity = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if(profile == PROFILE_PEDESTRIAN)
|
||||
{
|
||||
/* Ramp to a 1.4 m/s walk, then hold it with slow course changes. */
|
||||
double elapsed = time_s - ACCEL_START_S;
|
||||
if(elapsed < 3.0)
|
||||
{
|
||||
*speed_mps = 1.4 * (elapsed / 3.0);
|
||||
*acceleration_mps2 = 1.4 / 3.0;
|
||||
return;
|
||||
}
|
||||
*speed_mps = 1.4;
|
||||
*constant_velocity = true;
|
||||
/* A gentle serpentine keeps the gyro genuinely excited. */
|
||||
*yaw_rate_rps = 0.05 * sin(2.0 * PI * (time_s - 13.0) / 40.0);
|
||||
return;
|
||||
}
|
||||
|
||||
if(profile == PROFILE_URBAN_TURNS)
|
||||
{
|
||||
double elapsed = time_s - ACCEL_START_S;
|
||||
if(elapsed < 8.0)
|
||||
{
|
||||
*acceleration_mps2 = 4.0 / 8.0;
|
||||
*speed_mps = *acceleration_mps2 * elapsed;
|
||||
return;
|
||||
}
|
||||
*speed_mps = 4.0;
|
||||
/* A 90 degree turn every 12 s, each swept over 3 s at 30 deg/s --
|
||||
* an ordinary city block, and a large signal next to the 0.0092 deg/s
|
||||
* gyro noise measured on this part. */
|
||||
double cycle = fmod(time_s - 18.0, 12.0);
|
||||
if((cycle >= 0.0) && (cycle < 3.0))
|
||||
{
|
||||
*yaw_rate_rps = (PI / 2.0) / 3.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
*constant_velocity = true;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if((profile == PROFILE_SLOW_CITY) || (profile == PROFILE_ISOLATED_CITY))
|
||||
{
|
||||
/* Gentle pull-away to a 4 m/s urban cruise. */
|
||||
double elapsed = time_s - ACCEL_START_S;
|
||||
if(elapsed < 8.0)
|
||||
{
|
||||
*acceleration_mps2 = 4.0 / 8.0;
|
||||
*speed_mps = *acceleration_mps2 * elapsed;
|
||||
return;
|
||||
}
|
||||
*speed_mps = 4.0;
|
||||
*constant_velocity = true;
|
||||
*yaw_rate_rps = 0.010 * sin(2.0 * PI * (time_s - 18.0) / 90.0);
|
||||
return;
|
||||
}
|
||||
|
||||
/* 0 -> 100 km/h (27.78 m/s) in 30 s, then constant-velocity cruise. */
|
||||
double target_mps = 100.0 / 3.6;
|
||||
double ramp_s = 30.0;
|
||||
double elapsed = time_s - ACCEL_START_S;
|
||||
if(elapsed < ramp_s)
|
||||
{
|
||||
*acceleration_mps2 = target_mps / ramp_s;
|
||||
*speed_mps = *acceleration_mps2 * elapsed;
|
||||
return;
|
||||
}
|
||||
*speed_mps = target_mps;
|
||||
*constant_velocity = true;
|
||||
/* A long highway curve; small enough that lateral acceleration stays
|
||||
* well under the rest detector's thresholds. */
|
||||
*yaw_rate_rps = 0.004 * sin(2.0 * PI * (time_s - 40.0) / 120.0);
|
||||
}
|
||||
|
||||
static void propagate_truth(truth_t* truth, profile_kind_t profile,
|
||||
double start_s, double dt_s)
|
||||
{
|
||||
if(dt_s <= 0.0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
double speed_mps;
|
||||
double yaw_rate_rps;
|
||||
double acceleration_mps2;
|
||||
bool constant_velocity;
|
||||
profile_command(profile, start_s + 0.5 * dt_s, &speed_mps, &yaw_rate_rps,
|
||||
&acceleration_mps2, &constant_velocity);
|
||||
|
||||
truth->yaw_rad += yaw_rate_rps * dt_s;
|
||||
truth->yaw_rate_rps = yaw_rate_rps;
|
||||
truth->speed_mps = speed_mps;
|
||||
truth->constant_velocity = constant_velocity;
|
||||
|
||||
double forward_e = cos(truth->yaw_rad);
|
||||
double forward_n = sin(truth->yaw_rad);
|
||||
double previous_velocity[3];
|
||||
memcpy(previous_velocity, truth->velocity_enu_mps,
|
||||
sizeof(previous_velocity));
|
||||
|
||||
truth->velocity_enu_mps[0] = speed_mps * forward_e;
|
||||
truth->velocity_enu_mps[1] = speed_mps * forward_n;
|
||||
truth->velocity_enu_mps[2] = 0.0;
|
||||
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
truth->acceleration_enu_mps2[axis] =
|
||||
(truth->velocity_enu_mps[axis] - previous_velocity[axis]) / dt_s;
|
||||
truth->position_enu_m[axis] += 0.5 * dt_s *
|
||||
(previous_velocity[axis] + truth->velocity_enu_mps[axis]);
|
||||
}
|
||||
(void)acceleration_mps2;
|
||||
}
|
||||
|
||||
/*
|
||||
* Body-frame specific force and angular rate for a level platform.
|
||||
*
|
||||
* The accelerometer senses a - g_vec, so with ENU navigation and g_vec
|
||||
* pointing down the vertical channel carries +g. Yaw rotates the horizontal
|
||||
* components into the body frame.
|
||||
*/
|
||||
static fusion_imu_measurement_t make_imu(
|
||||
profile_kind_t profile, const truth_t* truth, double time_s,
|
||||
uint64_t ticks, double gravity_mps2, unsigned int sequence)
|
||||
{
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks = ticks;
|
||||
|
||||
double cos_yaw = cos(truth->yaw_rad);
|
||||
double sin_yaw = sin(truth->yaw_rad);
|
||||
double east = truth->acceleration_enu_mps2[0];
|
||||
double north = truth->acceleration_enu_mps2[1];
|
||||
|
||||
imu.accel_mps2[0] = east * cos_yaw + north * sin_yaw;
|
||||
imu.accel_mps2[1] = -east * sin_yaw + north * cos_yaw;
|
||||
imu.accel_mps2[2] = gravity_mps2;
|
||||
imu.gyro_rps[0] = 0.0;
|
||||
imu.gyro_rps[1] = 0.0;
|
||||
imu.gyro_rps[2] = truth->yaw_rate_rps;
|
||||
|
||||
if((profile == PROFILE_PEDESTRIAN) && (truth->speed_mps > 0.1))
|
||||
{
|
||||
/* Step cadence near 1.9 Hz: strong vertical bob plus lateral sway.
|
||||
* This is the signature that keeps a walker out of the rest state. */
|
||||
double phase = 2.0 * PI * 1.9 * time_s;
|
||||
imu.accel_mps2[2] += 1.30 * sin(phase);
|
||||
imu.accel_mps2[1] += 0.55 * sin(0.5 * phase);
|
||||
imu.accel_mps2[0] += 0.30 * sin(phase + 0.7);
|
||||
imu.gyro_rps[0] += 0.12 * sin(0.5 * phase);
|
||||
imu.gyro_rps[1] += 0.09 * cos(phase);
|
||||
}
|
||||
|
||||
bool rolling = truth->speed_mps > 0.5;
|
||||
double mount = (profile == PROFILE_ISOLATED_CITY) ? 0.05 : 1.0;
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
imu.accel_mps2[axis] +=
|
||||
ACCEL_BIAS_MPS2 + ACCEL_NOISE_MPS2 * gaussian_random();
|
||||
imu.gyro_rps[axis] +=
|
||||
GYRO_BIAS_RPS + GYRO_NOISE_RPS * gaussian_random();
|
||||
if(rolling)
|
||||
{
|
||||
imu.accel_mps2[axis] +=
|
||||
mount * ROAD_VIBRATION_MPS2 * gaussian_random();
|
||||
imu.gyro_rps[axis] += mount * ROAD_VIBRATION_RPS * gaussian_random();
|
||||
}
|
||||
}
|
||||
imu.temperature_c = 25.0;
|
||||
imu.sequence = sequence;
|
||||
|
||||
/* vehicle_at_rest is filled in by the caller from the real supervisor. */
|
||||
return imu;
|
||||
}
|
||||
|
||||
static fusion_navigation_measurement_t make_navigation(
|
||||
const truth_t* truth, uint64_t ticks, double horizontal_std_m)
|
||||
{
|
||||
fusion_navigation_measurement_t navigation;
|
||||
memset(&navigation, 0, sizeof(navigation));
|
||||
navigation.position.timestamp_ticks = ticks;
|
||||
navigation.position.position_enu_m[0] =
|
||||
truth->position_enu_m[0] + horizontal_std_m * gaussian_random();
|
||||
navigation.position.position_enu_m[1] =
|
||||
truth->position_enu_m[1] + horizontal_std_m * gaussian_random();
|
||||
navigation.position.position_enu_m[2] =
|
||||
truth->position_enu_m[2] + GPS_VERTICAL_STD_M * gaussian_random();
|
||||
navigation.position.covariance_enu_m2[0] = horizontal_std_m * horizontal_std_m;
|
||||
navigation.position.covariance_enu_m2[4] = horizontal_std_m * horizontal_std_m;
|
||||
navigation.position.covariance_enu_m2[8] =
|
||||
GPS_VERTICAL_STD_M * GPS_VERTICAL_STD_M;
|
||||
navigation.position.source_quality = 85.0;
|
||||
navigation.position.source_valid = true;
|
||||
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
navigation.velocity_enu_mps[axis] = truth->velocity_enu_mps[axis] +
|
||||
GPS_VELOCITY_STD_MPS * gaussian_random();
|
||||
}
|
||||
navigation.velocity_covariance_enu_mps2[0] =
|
||||
GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS;
|
||||
navigation.velocity_covariance_enu_mps2[4] =
|
||||
GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS;
|
||||
navigation.velocity_covariance_enu_mps2[8] =
|
||||
4.0 * GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS;
|
||||
navigation.velocity_valid = true;
|
||||
return navigation;
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
double outage_max_error_m;
|
||||
double error_at_outage_end_m;
|
||||
double error_after_recovery_m;
|
||||
double max_state_step_m;
|
||||
double sigma_at_outage_end_m;
|
||||
double heading_error_deg;
|
||||
double truth_path_length_m;
|
||||
double drift_sigma_ratio;
|
||||
double max_cruise_speed_error_mps;
|
||||
double recovery_seconds;
|
||||
uint64_t zero_updates_during_cruise;
|
||||
uint64_t false_rest_samples;
|
||||
uint64_t gps_accepted;
|
||||
uint64_t gps_rejected;
|
||||
uint64_t recoveries;
|
||||
bool finite;
|
||||
bool recovered;
|
||||
bool reached_running;
|
||||
} metrics_t;
|
||||
|
||||
static metrics_t run_case(profile_kind_t profile, double outage_s)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
metrics_t metrics;
|
||||
memset(&metrics, 0, sizeof(metrics));
|
||||
metrics.finite = true;
|
||||
|
||||
fusion_engine_config_t config;
|
||||
fusion_imu_quality_profile_t quality;
|
||||
fusion_engine_default_config(&config, TIMEBASE_HZ);
|
||||
if(!imu_fusion_quality_profile_for_backend(
|
||||
IMU_SENSOR_TDK_ICM42688, &quality) ||
|
||||
!fusion_engine_apply_imu_quality(&config, &quality))
|
||||
{
|
||||
return metrics;
|
||||
}
|
||||
memset(&engine, 0, sizeof(engine));
|
||||
if(!fusion_engine_init(&engine, &config))
|
||||
{
|
||||
return metrics;
|
||||
}
|
||||
|
||||
random_state = 0x51ed270bu ^ ((uint32_t)profile << 16) ^
|
||||
(uint32_t)(outage_s);
|
||||
/* The same supervisor navigation_fusion.c feeds the engine, so whatever it
|
||||
* concludes about a smoothly cruising vehicle is what the engine sees. */
|
||||
fusion_rest_detector_t rest_detector;
|
||||
fusion_rest_detector_config_t rest_config;
|
||||
fusion_rest_detector_default_config(&rest_config);
|
||||
if(!fusion_rest_detector_init(&rest_detector, &rest_config, TIMEBASE_HZ, true))
|
||||
{
|
||||
return metrics;
|
||||
}
|
||||
|
||||
truth_t truth;
|
||||
memset(&truth, 0, sizeof(truth));
|
||||
|
||||
double outage_end_s = OUTAGE_START_S + outage_s;
|
||||
double duration_s = outage_end_s + RECOVERY_OBSERVE_S;
|
||||
uint64_t imu_period_ticks = TIMEBASE_HZ / IMU_HZ;
|
||||
uint64_t gps_period_ticks = (uint64_t)GPS_PERIOD_MS * 1000u;
|
||||
uint64_t end_ticks = (uint64_t)(duration_s * (double)TIMEBASE_HZ);
|
||||
uint64_t next_imu_ticks = imu_period_ticks;
|
||||
uint64_t next_gps_ticks = gps_period_ticks;
|
||||
uint64_t last_ticks = 0u;
|
||||
unsigned int sequence = 0u;
|
||||
|
||||
double previous_state[3] = {0.0, 0.0, 0.0};
|
||||
bool have_previous_state = false;
|
||||
double recovery_started_s = -1.0;
|
||||
uint64_t zero_updates_at_cruise_start = 0u;
|
||||
bool cruise_started = false;
|
||||
|
||||
while(true)
|
||||
{
|
||||
uint64_t ticks = (next_imu_ticks < next_gps_ticks)
|
||||
? next_imu_ticks : next_gps_ticks;
|
||||
if(ticks > end_ticks)
|
||||
{
|
||||
break;
|
||||
}
|
||||
double start_s = (double)last_ticks / (double)TIMEBASE_HZ;
|
||||
double dt_s = (double)(ticks - last_ticks) / (double)TIMEBASE_HZ;
|
||||
propagate_truth(&truth, profile, start_s, dt_s);
|
||||
last_ticks = ticks;
|
||||
double time_s = (double)ticks / (double)TIMEBASE_HZ;
|
||||
bool in_outage = (time_s >= OUTAGE_START_S) && (time_s < outage_end_s);
|
||||
|
||||
if(ticks == next_gps_ticks)
|
||||
{
|
||||
if(!in_outage)
|
||||
{
|
||||
fusion_navigation_measurement_t navigation =
|
||||
make_navigation(&truth, ticks,
|
||||
profile_gps_horizontal_std_m(profile));
|
||||
(void)fusion_engine_push_navigation(
|
||||
&engine, &navigation, NULL, NULL, NULL, NULL);
|
||||
}
|
||||
next_gps_ticks += gps_period_ticks;
|
||||
}
|
||||
|
||||
if(ticks == next_imu_ticks)
|
||||
{
|
||||
fusion_imu_measurement_t imu = make_imu(
|
||||
profile, &truth, time_s, ticks, config.eskf.gravity_mps2,
|
||||
sequence++);
|
||||
imu_sample_t sample;
|
||||
memset(&sample, 0, sizeof(sample));
|
||||
sample.timestamp_ticks = imu.timestamp_ticks;
|
||||
memcpy(sample.accel_mps2, imu.accel_mps2, sizeof(sample.accel_mps2));
|
||||
memcpy(sample.gyro_rps, imu.gyro_rps, sizeof(sample.gyro_rps));
|
||||
sample.temperature_c = imu.temperature_c;
|
||||
sample.sequence = imu.sequence;
|
||||
(void)fusion_rest_detector_update(&rest_detector, &sample);
|
||||
imu.vehicle_at_rest = rest_detector.at_rest;
|
||||
if(rest_detector.at_rest && (truth.speed_mps > 1.0))
|
||||
{
|
||||
metrics.false_rest_samples++;
|
||||
}
|
||||
(void)fusion_engine_push_imu(&engine, &imu);
|
||||
next_imu_ticks += imu_period_ticks;
|
||||
}
|
||||
|
||||
if(engine.state != FUSION_ENGINE_RUNNING)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
metrics.reached_running = true;
|
||||
|
||||
fusion_runtime_t runtime;
|
||||
if(!fusion_engine_get_runtime(&engine, &runtime))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
const double* state = runtime.filter.position_m;
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
if(!isfinite(state[axis]))
|
||||
{
|
||||
metrics.finite = false;
|
||||
}
|
||||
}
|
||||
if(!metrics.finite)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
double error = hypot(state[0] - truth.position_enu_m[0],
|
||||
state[1] - truth.position_enu_m[1]);
|
||||
if(have_previous_state)
|
||||
{
|
||||
double step = hypot(state[0] - previous_state[0],
|
||||
state[1] - previous_state[1]);
|
||||
/* Re-anchoring after an outage is a deliberate discontinuity;
|
||||
* only the free-running interval must stay smooth. */
|
||||
if(in_outage && (step > metrics.max_state_step_m))
|
||||
{
|
||||
metrics.max_state_step_m = step;
|
||||
}
|
||||
}
|
||||
memcpy(previous_state, state, sizeof(previous_state));
|
||||
have_previous_state = true;
|
||||
|
||||
if(in_outage)
|
||||
{
|
||||
metrics.truth_path_length_m += truth.speed_mps * dt_s;
|
||||
if(error > metrics.outage_max_error_m)
|
||||
{
|
||||
metrics.outage_max_error_m = error;
|
||||
}
|
||||
metrics.error_at_outage_end_m = error;
|
||||
/*
|
||||
* Horizontal one-sigma from the filter's own error covariance
|
||||
* (position occupies error states 0..2). Judging drift against
|
||||
* this rather than against a fixed distance is what distinguishes
|
||||
* honest dead reckoning from divergence: a filter that knows it is
|
||||
* uncertain is behaving; one whose error outruns its covariance is
|
||||
* not. The absolute drift is dominated by whatever vibration and
|
||||
* bias the platform actually has, so no fixed metre bound
|
||||
* generalises across profiles.
|
||||
*/
|
||||
const double* q = runtime.filter.body_to_nav_quaternion;
|
||||
double yaw = atan2(2.0 * (q[0] * q[3] + q[1] * q[2]),
|
||||
1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]));
|
||||
double heading_error = yaw - truth.yaw_rad;
|
||||
while(heading_error > PI) { heading_error -= 2.0 * PI; }
|
||||
while(heading_error < -PI) { heading_error += 2.0 * PI; }
|
||||
metrics.heading_error_deg = fabs(heading_error) * 180.0 / PI;
|
||||
double sigma = sqrt(fmax(0.0, runtime.filter.covariance[0][0]) +
|
||||
fmax(0.0, runtime.filter.covariance[1][1]));
|
||||
metrics.sigma_at_outage_end_m = sigma;
|
||||
metrics.drift_sigma_ratio = (sigma > 1.0e-6) ? (error / sigma) : 0.0;
|
||||
}
|
||||
else if(time_s >= outage_end_s)
|
||||
{
|
||||
if(recovery_started_s < 0.0)
|
||||
{
|
||||
recovery_started_s = time_s;
|
||||
}
|
||||
if(!metrics.recovered && (error <= 15.0))
|
||||
{
|
||||
metrics.recovered = true;
|
||||
metrics.recovery_seconds = time_s - recovery_started_s;
|
||||
}
|
||||
metrics.error_after_recovery_m = error;
|
||||
}
|
||||
|
||||
/* Cruise: constant velocity with GNSS present, the regime that used to
|
||||
* collapse into a false zero-velocity update. The zero-update counter
|
||||
* is sampled across this window only; zero-velocity updates during the
|
||||
* genuinely stationary opening phase are correct and must not be
|
||||
* charged against it. */
|
||||
if(!(truth.constant_velocity && (truth.speed_mps > 1.0) && !in_outage))
|
||||
{
|
||||
cruise_started = false;
|
||||
}
|
||||
/* Skip the re-convergence transient right after GNSS returns; that
|
||||
* interval is judged by the recovery checks instead. */
|
||||
if(truth.constant_velocity && (truth.speed_mps > 1.0) && !in_outage &&
|
||||
((time_s < OUTAGE_START_S) || (time_s > outage_end_s + 10.0)))
|
||||
{
|
||||
double speed_error =
|
||||
fabs(eskf_speed_mps(&runtime.filter) - truth.speed_mps);
|
||||
if(speed_error > metrics.max_cruise_speed_error_mps)
|
||||
{
|
||||
metrics.max_cruise_speed_error_mps = speed_error;
|
||||
}
|
||||
uint64_t zero_updates =
|
||||
fusion_engine_diagnostics(&engine)->zero_velocity_update_count;
|
||||
if(cruise_started)
|
||||
{
|
||||
metrics.zero_updates_during_cruise +=
|
||||
zero_updates - zero_updates_at_cruise_start;
|
||||
}
|
||||
cruise_started = true;
|
||||
zero_updates_at_cruise_start = zero_updates;
|
||||
}
|
||||
}
|
||||
|
||||
const fusion_engine_diagnostics_t* diagnostics =
|
||||
fusion_engine_diagnostics(&engine);
|
||||
metrics.gps_accepted = diagnostics->position_accepted;
|
||||
metrics.gps_rejected = diagnostics->position_nis_rejected;
|
||||
metrics.recoveries = diagnostics->position_recovery_count;
|
||||
return metrics;
|
||||
}
|
||||
|
||||
static void report(profile_kind_t profile, double outage_s,
|
||||
const metrics_t* metrics)
|
||||
{
|
||||
printf("%-11s outage=%3.0fs drift_max=%8.1fm drift_end=%8.1fm "
|
||||
"recovered=%s in %4.1fs (err=%6.2fm) cruise_speed_err=%5.2fm/s "
|
||||
"sigma_end=%8.1fm err/sigma=%4.2f hdg_err=%5.2fdeg path=%7.0fm "
|
||||
"zupt=%lu gps=%lu/%lu recov=%lu step_max=%.2fm false_rest=%lu "
|
||||
"finite=%d\n",
|
||||
profile_name(profile), outage_s,
|
||||
metrics->outage_max_error_m, metrics->error_at_outage_end_m,
|
||||
metrics->recovered ? "yes" : "NO ", metrics->recovery_seconds,
|
||||
metrics->error_after_recovery_m,
|
||||
metrics->max_cruise_speed_error_mps,
|
||||
metrics->sigma_at_outage_end_m,
|
||||
metrics->drift_sigma_ratio,
|
||||
metrics->heading_error_deg,
|
||||
metrics->truth_path_length_m,
|
||||
(unsigned long)metrics->zero_updates_during_cruise,
|
||||
(unsigned long)metrics->gps_accepted,
|
||||
(unsigned long)metrics->gps_rejected,
|
||||
(unsigned long)metrics->recoveries,
|
||||
metrics->max_state_step_m,
|
||||
(unsigned long)metrics->false_rest_samples,
|
||||
metrics->finite ? 1 : 0);
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
const profile_kind_t profiles[] = {
|
||||
PROFILE_PEDESTRIAN, PROFILE_GENTLE_CAR, PROFILE_SLOW_CITY,
|
||||
PROFILE_ISOLATED_CITY, PROFILE_URBAN_TURNS};
|
||||
const double outages_s[] = {60.0, 300.0};
|
||||
|
||||
printf("Pedestrian and gentle-vehicle trajectories across GNSS outages\n");
|
||||
printf("IMU 50 Hz, GNSS 5 Hz, bench-measured ICM-42688-P noise\n");
|
||||
|
||||
for(unsigned int p = 0u ; p < PROFILE_COUNT ; p++)
|
||||
{
|
||||
for(unsigned int o = 0u ; o < 2u ; o++)
|
||||
{
|
||||
profile_kind_t profile = profiles[p];
|
||||
double outage_s = outages_s[o];
|
||||
metrics_t metrics = run_case(profile, outage_s);
|
||||
report(profile, outage_s, &metrics);
|
||||
|
||||
char message[192];
|
||||
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: engine must finish calibration",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.reached_running, message);
|
||||
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: every published state must stay finite",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.finite, message);
|
||||
|
||||
/*
|
||||
* Dead reckoning must stay smooth. A step larger than a few
|
||||
* metres between consecutive IMU samples means the filter jumped,
|
||||
* not that it drifted.
|
||||
*/
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: no position discontinuity while free-running",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.max_state_step_m < 5.0, message);
|
||||
|
||||
/*
|
||||
* Drift must stay consistent with the filter's own uncertainty.
|
||||
* Five minutes of inertial-only travel legitimately reaches
|
||||
* kilometres on this hardware; what must never happen is the error
|
||||
* outrunning the covariance, which is the signature of divergence
|
||||
* rather than of honest coasting.
|
||||
*/
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: drift must stay within 5 sigma of the filter's "
|
||||
"own covariance", profile_name(profile), outage_s);
|
||||
CHECK(metrics.drift_sigma_ratio < 5.0, message);
|
||||
|
||||
/* The covariance must actually grow while unaided, otherwise the
|
||||
* filter would stay overconfident and reject the returning fixes --
|
||||
* the innovation-gate lockout seen in the field. */
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: covariance must grow while unaided",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.sigma_at_outage_end_m > 10.0, message);
|
||||
|
||||
/*
|
||||
* Heading is integrated from a directly measured rate, so course
|
||||
* changes must survive an unaided minute far better than position
|
||||
* does. This is what makes the dead-reckoned path the right shape
|
||||
* even once it has drifted a long way from the truth.
|
||||
*/
|
||||
if(outage_s <= 60.0)
|
||||
{
|
||||
/*
|
||||
* A walker is the hard case: gait swings the sensor through
|
||||
* large angles at frequencies a 50 Hz strapdown integration
|
||||
* resolves poorly, so heading degrades several times faster
|
||||
* than on a vehicle. Pedestrian navigation is better served
|
||||
* by step detection than by strapdown integration, and this
|
||||
* looser bound records that rather than hiding it.
|
||||
*/
|
||||
double heading_limit_deg =
|
||||
(profile == PROFILE_PEDESTRIAN) ? 10.0 : 5.0;
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: heading must survive the outage",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.heading_error_deg < heading_limit_deg, message);
|
||||
}
|
||||
|
||||
/* The lockout regression: GNSS must be able to re-anchor. */
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: must re-converge after GNSS returns",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.recovered, message);
|
||||
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: re-convergence must take under 30 s",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.recovery_seconds < 30.0, message);
|
||||
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: must hold position after re-converging",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.error_after_recovery_m < 25.0, message);
|
||||
|
||||
/*
|
||||
* The false-rest regression. With GNSS present, a filter that
|
||||
* accepted the bogus at-rest flag would pin speed near zero while
|
||||
* the vehicle cruised at 27.8 m/s.
|
||||
*/
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: cruise speed must track truth, not collapse",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.max_cruise_speed_error_mps < 3.0, message);
|
||||
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: no zero-velocity update during aided cruise",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.zero_updates_during_cruise == 0u, message);
|
||||
|
||||
if(profile == PROFILE_ISOLATED_CITY)
|
||||
{
|
||||
/* Precondition: without a genuinely fooled supervisor this
|
||||
* case would prove nothing, so assert the trap is armed. */
|
||||
snprintf(message, sizeof(message),
|
||||
"%s/%.0fs: rest supervisor must actually be fooled "
|
||||
"for this case to mean anything",
|
||||
profile_name(profile), outage_s);
|
||||
CHECK(metrics.false_rest_samples > 100u, message);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(failures != 0)
|
||||
{
|
||||
fprintf(stderr, "FAIL: %d trajectory check(s) failed\n", failures);
|
||||
return 1;
|
||||
}
|
||||
printf("PASS: pedestrian and gentle-vehicle trajectories stayed bounded "
|
||||
"through 1 and 5 minute outages and re-converged on GNSS return.\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
TARGET := tdk_noise_test
|
||||
|
||||
GPS_SYSTEM_DIR ?= ../gps_system
|
||||
|
||||
# The production driver is compiled in directly so the sensor is configured
|
||||
# exactly as the flight build configures it.
|
||||
vpath %.c $(GPS_SYSTEM_DIR)/src/drivers/imu
|
||||
|
||||
APP_SRCS := \
|
||||
main.c \
|
||||
tdk_icm42688.c
|
||||
|
||||
BSP_SRCS := init.c uart.c plic.c irq.c pmod_spi.c
|
||||
|
||||
# Genesys2 fabric/UART and CLINT defaults, matching the gps_system app.
|
||||
BSP_UART_CLOCK_HZ ?= 50000000
|
||||
BSP_UART_BAUDRATE ?= 460800
|
||||
BSP_CLINT_TIMEBASE_HZ ?= 500000
|
||||
|
||||
# Mirrors gps_system IMU_ODR_HZ; override together with it.
|
||||
NOISE_ODR_HZ ?= 50
|
||||
NOISE_DISCARD_SAMPLES ?= 100
|
||||
NOISE_BIAS_SAMPLES ?= 1000
|
||||
NOISE_MEASURE_SAMPLES ?= 3000
|
||||
# Set to 1 to refuse the polled fallback, so a broken INT1 route fails loudly.
|
||||
NOISE_REQUIRE_IRQ ?= 0
|
||||
# Set to 1 to run only the INT1 -> PLIC continuity test and exit.
|
||||
NOISE_PIN_TEST ?= 0
|
||||
|
||||
include ../common/app.mk
|
||||
|
||||
CFLAGS += -I$(GPS_SYSTEM_DIR)/header
|
||||
CFLAGS += -DNOISE_ODR_HZ=$(NOISE_ODR_HZ)u \
|
||||
-DNOISE_DISCARD_SAMPLES=$(NOISE_DISCARD_SAMPLES)u \
|
||||
-DNOISE_BIAS_SAMPLES=$(NOISE_BIAS_SAMPLES)u \
|
||||
-DNOISE_MEASURE_SAMPLES=$(NOISE_MEASURE_SAMPLES)u \
|
||||
-DNOISE_REQUIRE_IRQ=$(NOISE_REQUIRE_IRQ) \
|
||||
-DNOISE_PIN_TEST=$(NOISE_PIN_TEST)
|
||||
LDLIBS += -lm
|
||||
@@ -0,0 +1,805 @@
|
||||
/*
|
||||
* Stationary noise characterization for the TDK ICM-42688-P.
|
||||
*
|
||||
* The board must stay still for the whole run.
|
||||
*
|
||||
* This app drives the production tdk_icm42688 driver, so ODR, full-scale
|
||||
* ranges, UI filter configuration and LSB scaling are identical to the flight
|
||||
* build. Measuring through a hand-rolled register setup would characterize a
|
||||
* different bandwidth and produce thresholds that do not transfer.
|
||||
*
|
||||
* It reports exactly the three quantities fusion_motion_update() compares
|
||||
* against its thresholds:
|
||||
*
|
||||
* accel_norm = || accel - mean(accel) || linear acceleration at rest, i.e.
|
||||
* what remains once the ESKF has
|
||||
* absorbed gravity and accel bias
|
||||
* gyro_norm = || gyro - mean(gyro) || bias-removed angular rate
|
||||
* jerk_norm = || d/dt IIR(accel_res) || same 0.10 s time constant the
|
||||
* detector uses
|
||||
*
|
||||
* A stationary limit must sit above the observed quiescent distribution or the
|
||||
* platform is never recognised as still; a motion threshold must sit above it
|
||||
* by enough margin that noise alone cannot trip it. Both therefore follow
|
||||
* from the numbers printed here rather than from datasheet noise densities,
|
||||
* which describe the bare part and not this board, mounting or bandwidth.
|
||||
*
|
||||
* All values print as integers in micro-units to keep the report independent
|
||||
* of newlib float formatting: um/s^2, urad/s, um/s^3.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "bsp/irq.h"
|
||||
#include "bsp/mmio.h"
|
||||
#include "bsp/plic.h"
|
||||
#include "bsp/pmod_spi.h"
|
||||
#include "bsp/soc.h"
|
||||
#include "bsp/uart.h"
|
||||
|
||||
#include "drivers/imu/tdk_icm42688.h"
|
||||
|
||||
/* Mirrors soc/software/gps_system/Makefile IMU_ODR_HZ. */
|
||||
#ifndef NOISE_ODR_HZ
|
||||
#define NOISE_ODR_HZ 50u
|
||||
#endif
|
||||
/* Discards sensor and UI-filter settling after start. */
|
||||
#ifndef NOISE_DISCARD_SAMPLES
|
||||
#define NOISE_DISCARD_SAMPLES 100u
|
||||
#endif
|
||||
#ifndef NOISE_BIAS_SAMPLES
|
||||
#define NOISE_BIAS_SAMPLES 1000u
|
||||
#endif
|
||||
#ifndef NOISE_MEASURE_SAMPLES
|
||||
#define NOISE_MEASURE_SAMPLES 3000u
|
||||
#endif
|
||||
|
||||
/* Matches fusion_motion_default_config()->jerk_filter_time_constant_s. */
|
||||
#define JERK_TIME_CONSTANT_S 0.10
|
||||
|
||||
#define HISTOGRAM_BINS 400u
|
||||
/* Full-scale of each histogram; samples above it still count in the last bin
|
||||
* and the exact maximum is tracked separately, so an overflow is visible. */
|
||||
#define ACCEL_HISTOGRAM_FULL_SCALE 0.40 /* m/s^2 -> 1.0 mm/s^2 per bin */
|
||||
#define GYRO_HISTOGRAM_FULL_SCALE 0.040 /* rad/s -> 100 urad/s per bin */
|
||||
#define JERK_HISTOGRAM_FULL_SCALE 8.0 /* m/s^3 -> 20 mm/s^3 per bin */
|
||||
|
||||
typedef struct {
|
||||
const char* name;
|
||||
const char* unit;
|
||||
double full_scale;
|
||||
double sum;
|
||||
double sum_squares;
|
||||
double maximum;
|
||||
uint32_t count;
|
||||
uint32_t overflow;
|
||||
uint32_t bins[HISTOGRAM_BINS];
|
||||
} statistic_t;
|
||||
|
||||
static volatile uint64_t unexpected_irq_count;
|
||||
static volatile uint64_t unexpected_trap_cause;
|
||||
|
||||
/*
|
||||
* Must carry interrupt("machine"): without it GCC ends the function with ret
|
||||
* instead of mret, so mstatus.MIE -- cleared by hardware on trap entry -- is
|
||||
* never restored and exactly one interrupt is ever taken. aligned(4) is
|
||||
* required because mtvec holds the address.
|
||||
*/
|
||||
void tdk_noise_trap_handler(void)
|
||||
__attribute__((interrupt("machine"), aligned(4)));
|
||||
|
||||
void tdk_noise_trap_handler(void)
|
||||
{
|
||||
asm volatile(
|
||||
".option push\n"
|
||||
".option norelax\n"
|
||||
"la gp, __global_pointer$\n"
|
||||
".option pop\n"
|
||||
: : : "memory");
|
||||
|
||||
const uint64_t mcause = bsp_read_mcause();
|
||||
if(((mcause >> 63) == 0u) ||
|
||||
((mcause & 0xfffu) != BSP_MCAUSE_MACHINE_EXTERNAL))
|
||||
{
|
||||
unexpected_trap_cause = mcause;
|
||||
return;
|
||||
}
|
||||
const uint32_t irq_id = bsp_plic_claim();
|
||||
if(irq_id == 0u)
|
||||
{
|
||||
__atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
|
||||
return;
|
||||
}
|
||||
if(irq_id == BSP_PMOD_IRQ)
|
||||
{
|
||||
tdk_icm42688_irq_handler();
|
||||
}
|
||||
else
|
||||
{
|
||||
__atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
|
||||
}
|
||||
bsp_plic_complete(irq_id);
|
||||
}
|
||||
|
||||
static void statistic_init(statistic_t* statistic, const char* name,
|
||||
const char* unit, double full_scale)
|
||||
{
|
||||
memset(statistic, 0, sizeof(*statistic));
|
||||
statistic->name = name;
|
||||
statistic->unit = unit;
|
||||
statistic->full_scale = full_scale;
|
||||
}
|
||||
|
||||
static void statistic_add(statistic_t* statistic, double value)
|
||||
{
|
||||
statistic->sum += value;
|
||||
statistic->sum_squares += value * value;
|
||||
if(value > statistic->maximum)
|
||||
{
|
||||
statistic->maximum = value;
|
||||
}
|
||||
statistic->count++;
|
||||
double scaled = value / statistic->full_scale * (double)HISTOGRAM_BINS;
|
||||
if(!(scaled >= 0.0))
|
||||
{
|
||||
scaled = 0.0;
|
||||
}
|
||||
unsigned int bin = (unsigned int)scaled;
|
||||
if(bin >= HISTOGRAM_BINS)
|
||||
{
|
||||
bin = HISTOGRAM_BINS - 1u;
|
||||
statistic->overflow++;
|
||||
}
|
||||
statistic->bins[bin]++;
|
||||
}
|
||||
|
||||
/* Upper edge of the bin holding the requested quantile. */
|
||||
static double statistic_percentile(const statistic_t* statistic,
|
||||
double fraction)
|
||||
{
|
||||
if(statistic->count == 0u)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
uint32_t target = (uint32_t)(fraction * (double)statistic->count);
|
||||
uint32_t cumulative = 0u;
|
||||
for(unsigned int bin = 0u ; bin < HISTOGRAM_BINS ; bin++)
|
||||
{
|
||||
cumulative += statistic->bins[bin];
|
||||
if(cumulative >= target)
|
||||
{
|
||||
return (double)(bin + 1u) * statistic->full_scale /
|
||||
(double)HISTOGRAM_BINS;
|
||||
}
|
||||
}
|
||||
return statistic->full_scale;
|
||||
}
|
||||
|
||||
static long micro(double value)
|
||||
{
|
||||
double scaled = value * 1.0e6;
|
||||
if(scaled > 2.0e9)
|
||||
{
|
||||
scaled = 2.0e9;
|
||||
}
|
||||
if(scaled < -2.0e9)
|
||||
{
|
||||
scaled = -2.0e9;
|
||||
}
|
||||
return (long)scaled;
|
||||
}
|
||||
|
||||
static void statistic_report(const statistic_t* statistic)
|
||||
{
|
||||
double mean = (statistic->count != 0u)
|
||||
? statistic->sum / (double)statistic->count : 0.0;
|
||||
double variance = (statistic->count > 1u)
|
||||
? (statistic->sum_squares / (double)statistic->count) -
|
||||
(mean * mean)
|
||||
: 0.0;
|
||||
if(variance < 0.0)
|
||||
{
|
||||
variance = 0.0;
|
||||
}
|
||||
printf("STAT,%s,unit=%s,n=%lu,mean=%ld,std=%ld,"
|
||||
"p50=%ld,p95=%ld,p99=%ld,p999=%ld,max=%ld,overflow=%lu\n",
|
||||
statistic->name, statistic->unit,
|
||||
(unsigned long)statistic->count,
|
||||
micro(mean), micro(sqrt(variance)),
|
||||
micro(statistic_percentile(statistic, 0.50)),
|
||||
micro(statistic_percentile(statistic, 0.95)),
|
||||
micro(statistic_percentile(statistic, 0.99)),
|
||||
micro(statistic_percentile(statistic, 0.999)),
|
||||
micro(statistic->maximum),
|
||||
(unsigned long)statistic->overflow);
|
||||
}
|
||||
|
||||
/*
|
||||
* Polled fallback.
|
||||
*
|
||||
* The data-ready INT1 route is not delivering on this bitstream (the driver
|
||||
* reports a single spurious IRQ and then silence), so the interrupt path
|
||||
* cannot feed the characterization. Polling INT_STATUS reads the identical
|
||||
* registers the ISR would read, at the identical ODR, full scale and UI
|
||||
* filter, so the noise statistics are unaffected; only the delivery mechanism
|
||||
* differs. Sample timestamps still come from CLINT.
|
||||
*/
|
||||
#define ICM_INT_STATUS_REG 0x2du
|
||||
#define ICM_TEMP_DATA1_REG 0x1du
|
||||
#define ICM_SPI_READ_BIT 0x80u
|
||||
#define ICM_SPI_DUMMY_BYTE 0xffu
|
||||
#define ICM_DATA_READY_BIT 0x08u
|
||||
|
||||
static bool polled_mode;
|
||||
|
||||
static int16_t big_endian_i16(const uint8_t* bytes)
|
||||
{
|
||||
return (int16_t)(((uint16_t)bytes[0] << 8) | bytes[1]);
|
||||
}
|
||||
|
||||
static bool poll_for_sample(imu_sample_t* sample, uint64_t deadline)
|
||||
{
|
||||
while(bsp_read64(BSP_CLINT_MTIME) < deadline)
|
||||
{
|
||||
uint8_t status = bsp_pmod_spi_read_reg8(
|
||||
ICM_INT_STATUS_REG | ICM_SPI_READ_BIT, ICM_SPI_DUMMY_BYTE);
|
||||
if((status & ICM_DATA_READY_BIT) == 0u)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
uint64_t timestamp = bsp_read64(BSP_CLINT_MTIME);
|
||||
uint8_t bytes[14];
|
||||
bsp_pmod_spi_read_regs(ICM_TEMP_DATA1_REG | ICM_SPI_READ_BIT,
|
||||
ICM_SPI_DUMMY_BYTE, bytes, sizeof(bytes));
|
||||
/* Same scaling the driver applies: full scale over a 15-bit half range. */
|
||||
double accel_scale = 4.0 * 9.80665 / 32768.0;
|
||||
double gyro_scale = 250.0 * 0.017453292519943295769236907684886 / 32768.0;
|
||||
sample->timestamp_ticks = timestamp;
|
||||
sample->temperature_c = (double)big_endian_i16(&bytes[0]) / 132.48 + 25.0;
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
sample->accel_mps2[axis] =
|
||||
(double)big_endian_i16(&bytes[2u + 2u * axis]) * accel_scale;
|
||||
sample->gyro_rps[axis] =
|
||||
(double)big_endian_i16(&bytes[8u + 2u * axis]) * gyro_scale;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* Why the data-ready interrupt stopped after the first one.
|
||||
*
|
||||
* Three causes produce the same silence and are told apart by whether the
|
||||
* sensor is still generating a data-ready condition at all:
|
||||
*
|
||||
* sensor stopped PWR_MGMT0 or the ODR fields no longer hold their
|
||||
* configured values, and DRDY never sets
|
||||
* routing lost DRDY sets in INT_STATUS but INT_SOURCE0 no longer routes
|
||||
* it to the pin
|
||||
* latch stuck DRDY sets and routing is intact, so the pin is asserted
|
||||
* but the PLIC is not seeing a fresh edge/level
|
||||
*/
|
||||
/*
|
||||
* Continuity test for the sensor INT1 -> FPGA pmod_irq -> PLIC path.
|
||||
*
|
||||
* Everything measured so far depends on the sensor raising a data-ready edge,
|
||||
* which conflates "the line is broken" with "the source never fired". This
|
||||
* separates them by driving INT1 to a static level the sensor holds
|
||||
* regardless of data-ready, and reading the PLIC pending bit directly.
|
||||
*
|
||||
* INT_CONFIG bit 0 selects INT1 polarity. Clearing it makes the pin active
|
||||
* low, so its idle state becomes high and it stays there. If pmod_irq is
|
||||
* really wired to this pin, the PLIC gateway then reports pending forever; if
|
||||
* the input is floating, its pulldown keeps it clear. The core's external
|
||||
* interrupts stay masked throughout, so a permanently asserted line cannot
|
||||
* storm the ISR -- the pending bit is polled, never taken.
|
||||
*/
|
||||
#define PLIC_PENDING_WORD0 (BSP_PLIC_BASE + 0x1000u)
|
||||
#define ICM_INT_CONFIG_REG 0x14u
|
||||
#define ICM_INT_SOURCE0_REG 0x65u
|
||||
#define ICM_INT1_ACTIVE_HIGH_PP_LATCHED 0x07u
|
||||
#define ICM_INT1_ACTIVE_LOW_PP_LATCHED 0x06u
|
||||
#define ICM_UI_DRDY_INT1 0x08u
|
||||
|
||||
static uint32_t sample_pending_percent(uint64_t window_ticks)
|
||||
{
|
||||
/* The gateway latches; claiming clears it so what follows reflects the
|
||||
* line's present level rather than a stale assertion. */
|
||||
uint32_t claimed = bsp_plic_claim();
|
||||
if(claimed != 0u)
|
||||
{
|
||||
bsp_plic_complete(claimed);
|
||||
}
|
||||
uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) + window_ticks;
|
||||
uint32_t samples = 0u;
|
||||
uint32_t asserted = 0u;
|
||||
while(bsp_read64(BSP_CLINT_MTIME) < deadline)
|
||||
{
|
||||
if((bsp_read32(PLIC_PENDING_WORD0) & (1u << BSP_PMOD_IRQ)) != 0u)
|
||||
{
|
||||
asserted++;
|
||||
uint32_t id = bsp_plic_claim();
|
||||
if(id != 0u)
|
||||
{
|
||||
bsp_plic_complete(id);
|
||||
}
|
||||
}
|
||||
samples++;
|
||||
}
|
||||
return (samples != 0u) ? ((asserted * 100u) / samples) : 0u;
|
||||
}
|
||||
|
||||
static void run_pin_continuity_test(void)
|
||||
{
|
||||
uint64_t window = BSP_CLINT_TIMEBASE_HZ / 5u; /* 200 ms */
|
||||
|
||||
bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
|
||||
bsp_plic_enable(BSP_PMOD_IRQ);
|
||||
|
||||
/* Baseline: nothing routed to the pin, so it should rest inactive-low. */
|
||||
bsp_pmod_spi_write_reg8(ICM_INT_SOURCE0_REG, 0x00u);
|
||||
bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
|
||||
ICM_INT1_ACTIVE_HIGH_PP_LATCHED);
|
||||
uint32_t idle_low = sample_pending_percent(window);
|
||||
|
||||
/* Forced high: polarity inverted, so the idle state is asserted. */
|
||||
bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
|
||||
ICM_INT1_ACTIVE_LOW_PP_LATCHED);
|
||||
uint32_t forced_high = sample_pending_percent(window);
|
||||
|
||||
/* Restore and let data-ready drive the pin normally. */
|
||||
bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
|
||||
ICM_INT1_ACTIVE_HIGH_PP_LATCHED);
|
||||
bsp_pmod_spi_write_reg8(ICM_INT_SOURCE0_REG, ICM_UI_DRDY_INT1);
|
||||
uint32_t data_ready = sample_pending_percent(window);
|
||||
|
||||
printf("PIN_TEST,idle_low=%lu%%,forced_high=%lu%%,data_ready=%lu%%\n",
|
||||
(unsigned long)idle_low, (unsigned long)forced_high,
|
||||
(unsigned long)data_ready);
|
||||
printf("PIN_TEST,int_config=0x%02x,int_source0=0x%02x\n",
|
||||
bsp_pmod_spi_read_reg8(ICM_INT_CONFIG_REG | ICM_SPI_READ_BIT,
|
||||
ICM_SPI_DUMMY_BYTE),
|
||||
bsp_pmod_spi_read_reg8(ICM_INT_SOURCE0_REG | ICM_SPI_READ_BIT,
|
||||
ICM_SPI_DUMMY_BYTE));
|
||||
|
||||
if(forced_high >= 90u)
|
||||
{
|
||||
printf("VERDICT,INT1 reaches PLIC %lu. The pin is wired correctly; "
|
||||
"any remaining silence is the source not re-arming, not the "
|
||||
"wire.\n", (unsigned long)BSP_PMOD_IRQ);
|
||||
}
|
||||
else if(forced_high <= 10u)
|
||||
{
|
||||
printf("VERDICT,INT1 does NOT reach PLIC %lu. Holding the sensor pin "
|
||||
"statically asserted produced no pending bit, so the FPGA input "
|
||||
"(package pin AK30) is not connected to the sensor INT1 pin. "
|
||||
"Confirm which physical PMOD position AK30 is in Digilent's "
|
||||
"Genesys2 master XDC -- the comment in genesys2_pmod_irq.xdc is "
|
||||
"not authoritative.\n", (unsigned long)BSP_PMOD_IRQ);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("VERDICT,intermittent. A marginal or noisy connection.\n");
|
||||
}
|
||||
bsp_plic_disable(BSP_PMOD_IRQ);
|
||||
}
|
||||
|
||||
/* MEIE (bit 11) tells us whether the hart is actually accepting the PLIC's
|
||||
* external interrupt, which a pending bit alone does not. */
|
||||
static uint64_t bsp_read_mie(void)
|
||||
{
|
||||
uint64_t value;
|
||||
asm volatile("csrr %0, mie" : "=r"(value));
|
||||
return value;
|
||||
}
|
||||
|
||||
static void delay_ticks(uint64_t ticks)
|
||||
{
|
||||
uint64_t start = bsp_read64(BSP_CLINT_MTIME);
|
||||
while((bsp_read64(BSP_CLINT_MTIME) - start) < ticks)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
static void dump_sensor_state(void)
|
||||
{
|
||||
static const struct { uint8_t address; const char* name; } registers[] = {
|
||||
{0x14u, "INT_CONFIG"},
|
||||
{0x2du, "INT_STATUS"},
|
||||
{0x4eu, "PWR_MGMT0"},
|
||||
{0x4fu, "GYRO_CONFIG0"},
|
||||
{0x50u, "ACCEL_CONFIG0"},
|
||||
{0x63u, "INT_CONFIG0"},
|
||||
{0x64u, "INT_CONFIG1"},
|
||||
{0x65u, "INT_SOURCE0"},
|
||||
{0x75u, "WHO_AM_I"}
|
||||
};
|
||||
for(unsigned int i = 0u ;
|
||||
i < sizeof(registers) / sizeof(registers[0]) ; i++)
|
||||
{
|
||||
printf("REG,%s,0x%02x=0x%02x\n", registers[i].name,
|
||||
registers[i].address,
|
||||
bsp_pmod_spi_read_reg8(registers[i].address | ICM_SPI_READ_BIT,
|
||||
ICM_SPI_DUMMY_BYTE));
|
||||
}
|
||||
|
||||
/*
|
||||
* Sample INT_STATUS over several ODR periods. Reading it also clears it,
|
||||
* so a healthy sensor sets DATA_RDY again on the very next sample and this
|
||||
* count lands near the number of periods observed.
|
||||
*/
|
||||
uint64_t window_ticks = (BSP_CLINT_TIMEBASE_HZ * 10u) / NOISE_ODR_HZ;
|
||||
uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) + window_ticks;
|
||||
uint32_t polls = 0u;
|
||||
uint32_t drdy_seen = 0u;
|
||||
while(bsp_read64(BSP_CLINT_MTIME) < deadline)
|
||||
{
|
||||
uint8_t status = bsp_pmod_spi_read_reg8(
|
||||
ICM_INT_STATUS_REG | ICM_SPI_READ_BIT, ICM_SPI_DUMMY_BYTE);
|
||||
polls++;
|
||||
if((status & ICM_DATA_READY_BIT) != 0u)
|
||||
{
|
||||
drdy_seen++;
|
||||
}
|
||||
}
|
||||
printf("DRDY_POLL,window_periods=10,polls=%lu,data_ready=%lu\n",
|
||||
(unsigned long)polls, (unsigned long)drdy_seen);
|
||||
}
|
||||
|
||||
static bool wait_for_sample(imu_sample_t* sample)
|
||||
{
|
||||
/* The bound covers roughly ten ODR periods. */
|
||||
uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) +
|
||||
(BSP_CLINT_TIMEBASE_HZ * 10u) / NOISE_ODR_HZ +
|
||||
BSP_CLINT_TIMEBASE_HZ / 10u;
|
||||
if(polled_mode)
|
||||
{
|
||||
return poll_for_sample(sample, deadline);
|
||||
}
|
||||
while(bsp_read64(BSP_CLINT_MTIME) < deadline)
|
||||
{
|
||||
if(tdk_icm42688_take_measurement(sample))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static double norm3(const double vector[3])
|
||||
{
|
||||
return sqrt(vector[0] * vector[0] + vector[1] * vector[1] +
|
||||
vector[2] * vector[2]);
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
printf("\nTDK ICM-42688-P stationary noise characterization\n");
|
||||
printf("KEEP THE BOARD COMPLETELY STILL FOR THE ENTIRE RUN\n");
|
||||
|
||||
tdk_icm42688_config_t config;
|
||||
tdk_icm42688_default_config(&config);
|
||||
config.odr_hz = NOISE_ODR_HZ;
|
||||
|
||||
bsp_plic_disable_all();
|
||||
bsp_plic_set_threshold(0u);
|
||||
bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
|
||||
/* Recover the PMOD LevelGateway if a previous GDB-loaded image stopped
|
||||
* between claim and complete. */
|
||||
bsp_plic_complete(BSP_PMOD_IRQ);
|
||||
bsp_set_mtvec(tdk_noise_trap_handler);
|
||||
|
||||
if(!tdk_icm42688_init(&config))
|
||||
{
|
||||
tdk_icm42688_diagnostics_t diagnostics;
|
||||
tdk_icm42688_get_diagnostics(&diagnostics);
|
||||
printf("INIT FAIL: who_am_i=0x%02x (expected 0x47)\n",
|
||||
diagnostics.who_am_i);
|
||||
bsp_uart_wait_tx_idle();
|
||||
return 2;
|
||||
}
|
||||
|
||||
#if NOISE_PIN_TEST
|
||||
/* Runs with the core's external interrupts still masked, so a statically
|
||||
* asserted line is polled rather than taken. */
|
||||
printf("CONFIG,odr_hz=%lu,accel_fs_g=%d,gyro_fs_mdps=%d,ui_filter=0x%02x\n",
|
||||
(unsigned long)config.odr_hz, (int)config.accel_full_scale_g,
|
||||
(int)config.gyro_full_scale_mdps, config.ui_filter_config);
|
||||
run_pin_continuity_test();
|
||||
tdk_icm42688_stop();
|
||||
bsp_uart_wait_tx_idle();
|
||||
return 0;
|
||||
#endif
|
||||
|
||||
bsp_enable_machine_external_interrupts();
|
||||
|
||||
/*
|
||||
* Separate a stale PLIC pending bit from a real interrupt.
|
||||
*
|
||||
* tdk_icm42688_init() leaves INT_SOURCE0 clear, so the sensor is not
|
||||
* driving INT1 yet. Enabling the PLIC route now and counting what arrives
|
||||
* measures everything that is not the sensor: a pending bit left latched
|
||||
* by a previous GDB-loaded image, or a floating input picking up noise.
|
||||
* Without this baseline a single stale interrupt looks exactly like a
|
||||
* working data-ready line, which is how one sample was mistaken for a
|
||||
* healthy route.
|
||||
*/
|
||||
bsp_plic_enable(BSP_PMOD_IRQ);
|
||||
delay_ticks(BSP_CLINT_TIMEBASE_HZ / 2u);
|
||||
tdk_icm42688_diagnostics_t baseline;
|
||||
tdk_icm42688_get_diagnostics(&baseline);
|
||||
|
||||
if(!tdk_icm42688_start())
|
||||
{
|
||||
printf("START FAIL\n");
|
||||
bsp_uart_wait_tx_idle();
|
||||
return 3;
|
||||
}
|
||||
/*
|
||||
* Sample the PLIC pending bit while the sensor is armed and the core's
|
||||
* external interrupts are unmasked. This separates the two remaining
|
||||
* layers: pending asserting without the trap count moving means the
|
||||
* gateway sees the line but delivery to the hart is broken, whereas
|
||||
* pending never asserting means the gateway is not reacting to a pin the
|
||||
* continuity test already proved is toggling.
|
||||
*/
|
||||
uint64_t armed_deadline = bsp_read64(BSP_CLINT_MTIME) + BSP_CLINT_TIMEBASE_HZ;
|
||||
uint32_t pending_samples = 0u;
|
||||
uint32_t pending_asserted = 0u;
|
||||
while(bsp_read64(BSP_CLINT_MTIME) < armed_deadline)
|
||||
{
|
||||
if((bsp_read32(PLIC_PENDING_WORD0) & (1u << BSP_PMOD_IRQ)) != 0u)
|
||||
{
|
||||
pending_asserted++;
|
||||
}
|
||||
pending_samples++;
|
||||
}
|
||||
tdk_icm42688_diagnostics_t armed;
|
||||
tdk_icm42688_get_diagnostics(&armed);
|
||||
printf("PLIC_PENDING,armed_samples=%lu,asserted=%lu,percent=%lu,"
|
||||
"mie_meie=%lu,threshold_ok=1\n",
|
||||
(unsigned long)pending_samples, (unsigned long)pending_asserted,
|
||||
(unsigned long)((pending_samples != 0u)
|
||||
? (pending_asserted * 100u) / pending_samples : 0u),
|
||||
(unsigned long)((bsp_read_mie() >> 11) & 1u));
|
||||
printf("IRQ_PROBE,quiet_500ms_irq=%lu,armed_1s_irq=%lu,armed_samples=%lu,"
|
||||
"spurious=%lu,expected_hz=%lu\n",
|
||||
(unsigned long)baseline.irq_count,
|
||||
(unsigned long)(armed.irq_count - baseline.irq_count),
|
||||
(unsigned long)armed.sample_count,
|
||||
(unsigned long)armed.spurious_irq_count,
|
||||
(unsigned long)NOISE_ODR_HZ);
|
||||
|
||||
printf("CONFIG,odr_hz=%lu,accel_fs_g=%d,gyro_fs_mdps=%d,"
|
||||
"ui_filter=0x%02x,tick_hz=%lu\n",
|
||||
(unsigned long)config.odr_hz,
|
||||
(int)config.accel_full_scale_g,
|
||||
(int)config.gyro_full_scale_mdps,
|
||||
config.ui_filter_config,
|
||||
(unsigned long)BSP_CLINT_TIMEBASE_HZ);
|
||||
printf("PLAN,discard=%lu,bias=%lu,measure=%lu,seconds=%lu\n",
|
||||
(unsigned long)NOISE_DISCARD_SAMPLES,
|
||||
(unsigned long)NOISE_BIAS_SAMPLES,
|
||||
(unsigned long)NOISE_MEASURE_SAMPLES,
|
||||
(unsigned long)((NOISE_DISCARD_SAMPLES + NOISE_BIAS_SAMPLES +
|
||||
NOISE_MEASURE_SAMPLES) / NOISE_ODR_HZ));
|
||||
|
||||
imu_sample_t sample;
|
||||
uint32_t dropouts = 0u;
|
||||
|
||||
/*
|
||||
* Decide on the sustained rate measured above, not on a single sample.
|
||||
* A lone interrupt is exactly what a stale pending bit produces, and
|
||||
* whether it happens to carry DATA_RDY is chance -- so one sample was
|
||||
* previously enough to report a healthy route that did not exist.
|
||||
*/
|
||||
{
|
||||
uint64_t observed_hz = armed.sample_count;
|
||||
uint64_t stale_hz = baseline.irq_count;
|
||||
if(observed_hz < (NOISE_ODR_HZ / 2u))
|
||||
{
|
||||
#if NOISE_REQUIRE_IRQ
|
||||
printf("DELIVERY,mode=FAILED,reason=rate_too_low,"
|
||||
"observed_hz=%lu,expected_hz=%lu,stale_irq=%lu\n",
|
||||
(unsigned long)observed_hz, (unsigned long)NOISE_ODR_HZ,
|
||||
(unsigned long)stale_hz);
|
||||
if((stale_hz != 0u) && (observed_hz == 0u))
|
||||
{
|
||||
printf("VERDICT,the only interrupts arrived before the sensor "
|
||||
"was driving INT1, so they came from a stale PLIC "
|
||||
"pending bit or a floating input -- not from the "
|
||||
"sensor. INT1 is still not reaching PLIC %lu.\n",
|
||||
(unsigned long)BSP_PMOD_IRQ);
|
||||
}
|
||||
else if(observed_hz == 0u)
|
||||
{
|
||||
printf("VERDICT,no interrupts at all. INT1 is not reaching "
|
||||
"PLIC %lu; check the pin named in "
|
||||
"hardware/fpga/genesys2/constraints/"
|
||||
"genesys2_pmod_irq.xdc.\n",
|
||||
(unsigned long)BSP_PMOD_IRQ);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("VERDICT,the route works but the source is not "
|
||||
"re-arming at the configured rate.\n");
|
||||
}
|
||||
dump_sensor_state();
|
||||
tdk_icm42688_stop();
|
||||
bsp_uart_wait_tx_idle();
|
||||
return 5;
|
||||
#else
|
||||
polled_mode = true;
|
||||
printf("DELIVERY,mode=polled,reason=rate_too_low,"
|
||||
"observed_hz=%lu,expected_hz=%lu,stale_irq=%lu\n",
|
||||
(unsigned long)observed_hz, (unsigned long)NOISE_ODR_HZ,
|
||||
(unsigned long)stale_hz);
|
||||
/* Drop only the PLIC route. INT_SOURCE0 stays enabled so the
|
||||
* DATA_RDY condition keeps setting INT_STATUS for polling, and
|
||||
* no ISR competes to read (and clear) that register. */
|
||||
bsp_plic_disable(BSP_PMOD_IRQ);
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("DELIVERY,mode=interrupt,rate_hz=%lu,stale_irq=%lu\n",
|
||||
(unsigned long)observed_hz, (unsigned long)stale_hz);
|
||||
}
|
||||
}
|
||||
|
||||
for(uint32_t i = 0u ; i < NOISE_DISCARD_SAMPLES ; i++)
|
||||
{
|
||||
if(!wait_for_sample(&sample))
|
||||
{
|
||||
dropouts++;
|
||||
}
|
||||
}
|
||||
|
||||
/* Pass 1: the stationary mean is gravity plus accel bias, and gyro bias.
|
||||
* Removing it reproduces the residual the ESKF sees once calibrated. */
|
||||
double accel_sum[3] = {0.0, 0.0, 0.0};
|
||||
double gyro_sum[3] = {0.0, 0.0, 0.0};
|
||||
double accel_sum_squares[3] = {0.0, 0.0, 0.0};
|
||||
double gyro_sum_squares[3] = {0.0, 0.0, 0.0};
|
||||
uint32_t bias_count = 0u;
|
||||
for(uint32_t i = 0u ; i < NOISE_BIAS_SAMPLES ; i++)
|
||||
{
|
||||
if(!wait_for_sample(&sample))
|
||||
{
|
||||
dropouts++;
|
||||
continue;
|
||||
}
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
accel_sum[axis] += sample.accel_mps2[axis];
|
||||
gyro_sum[axis] += sample.gyro_rps[axis];
|
||||
accel_sum_squares[axis] +=
|
||||
sample.accel_mps2[axis] * sample.accel_mps2[axis];
|
||||
gyro_sum_squares[axis] +=
|
||||
sample.gyro_rps[axis] * sample.gyro_rps[axis];
|
||||
}
|
||||
bias_count++;
|
||||
}
|
||||
if(bias_count == 0u)
|
||||
{
|
||||
printf("NO SAMPLES: check PMOD wiring and INT1 routing\n");
|
||||
bsp_uart_wait_tx_idle();
|
||||
return 4;
|
||||
}
|
||||
|
||||
double accel_mean[3];
|
||||
double gyro_mean[3];
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
accel_mean[axis] = accel_sum[axis] / (double)bias_count;
|
||||
gyro_mean[axis] = gyro_sum[axis] / (double)bias_count;
|
||||
double accel_variance =
|
||||
(accel_sum_squares[axis] / (double)bias_count) -
|
||||
(accel_mean[axis] * accel_mean[axis]);
|
||||
double gyro_variance =
|
||||
(gyro_sum_squares[axis] / (double)bias_count) -
|
||||
(gyro_mean[axis] * gyro_mean[axis]);
|
||||
printf("AXIS,%u,accel_mean=%ld,accel_std=%ld,"
|
||||
"gyro_mean=%ld,gyro_std=%ld\n",
|
||||
axis, micro(accel_mean[axis]),
|
||||
micro(sqrt(accel_variance > 0.0 ? accel_variance : 0.0)),
|
||||
micro(gyro_mean[axis]),
|
||||
micro(sqrt(gyro_variance > 0.0 ? gyro_variance : 0.0)));
|
||||
}
|
||||
printf("GRAVITY,measured=%ld,expected=9806650\n", micro(norm3(accel_mean)));
|
||||
|
||||
/* Pass 2: the detector inputs themselves. */
|
||||
statistic_t accel_statistic;
|
||||
statistic_t gyro_statistic;
|
||||
statistic_t jerk_statistic;
|
||||
statistic_init(&accel_statistic, "accel_norm", "um/s2",
|
||||
ACCEL_HISTOGRAM_FULL_SCALE);
|
||||
statistic_init(&gyro_statistic, "gyro_norm", "urad/s",
|
||||
GYRO_HISTOGRAM_FULL_SCALE);
|
||||
statistic_init(&jerk_statistic, "jerk_norm", "um/s3",
|
||||
JERK_HISTOGRAM_FULL_SCALE);
|
||||
|
||||
double filtered[3] = {0.0, 0.0, 0.0};
|
||||
bool have_filtered = false;
|
||||
uint64_t previous_ticks = 0u;
|
||||
|
||||
for(uint32_t i = 0u ; i < NOISE_MEASURE_SAMPLES ; i++)
|
||||
{
|
||||
if(!wait_for_sample(&sample))
|
||||
{
|
||||
dropouts++;
|
||||
continue;
|
||||
}
|
||||
double residual_accel[3];
|
||||
double residual_gyro[3];
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
residual_accel[axis] = sample.accel_mps2[axis] - accel_mean[axis];
|
||||
residual_gyro[axis] = sample.gyro_rps[axis] - gyro_mean[axis];
|
||||
}
|
||||
statistic_add(&accel_statistic, norm3(residual_accel));
|
||||
statistic_add(&gyro_statistic, norm3(residual_gyro));
|
||||
|
||||
if(previous_ticks != 0u)
|
||||
{
|
||||
double dt_s = (double)(sample.timestamp_ticks - previous_ticks) /
|
||||
(double)BSP_CLINT_TIMEBASE_HZ;
|
||||
if(dt_s > 0.0)
|
||||
{
|
||||
double alpha = dt_s / (JERK_TIME_CONSTANT_S + dt_s);
|
||||
double delta[3];
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
double previous = filtered[axis];
|
||||
filtered[axis] += alpha * (residual_accel[axis] - previous);
|
||||
delta[axis] = filtered[axis] - previous;
|
||||
}
|
||||
if(have_filtered)
|
||||
{
|
||||
statistic_add(&jerk_statistic, norm3(delta) / dt_s);
|
||||
}
|
||||
have_filtered = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(filtered, residual_accel, sizeof(filtered));
|
||||
}
|
||||
previous_ticks = sample.timestamp_ticks;
|
||||
|
||||
if(((i + 1u) % (NOISE_ODR_HZ * 10u)) == 0u)
|
||||
{
|
||||
printf("PROGRESS,%lu/%lu\n", (unsigned long)(i + 1u),
|
||||
(unsigned long)NOISE_MEASURE_SAMPLES);
|
||||
}
|
||||
}
|
||||
|
||||
statistic_report(&accel_statistic);
|
||||
statistic_report(&gyro_statistic);
|
||||
statistic_report(&jerk_statistic);
|
||||
|
||||
tdk_icm42688_diagnostics_t diagnostics;
|
||||
tdk_icm42688_get_diagnostics(&diagnostics);
|
||||
printf("HEALTH,irq=%lu,samples=%lu,invalid=%lu,spurious=%lu,"
|
||||
"overflow=%lu,missed=%lu,dropouts=%lu,unexpected_irq=%lu,"
|
||||
"trap_cause=%lu\n",
|
||||
(unsigned long)diagnostics.irq_count,
|
||||
(unsigned long)diagnostics.sample_count,
|
||||
(unsigned long)diagnostics.invalid_sample_count,
|
||||
(unsigned long)diagnostics.spurious_irq_count,
|
||||
(unsigned long)diagnostics.queue_overflow_count,
|
||||
(unsigned long)diagnostics.estimated_missed_sample_count,
|
||||
(unsigned long)dropouts,
|
||||
(unsigned long)unexpected_irq_count,
|
||||
(unsigned long)unexpected_trap_cause);
|
||||
printf("DONE\n");
|
||||
|
||||
tdk_icm42688_stop();
|
||||
bsp_uart_wait_tx_idle();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user