132 lines
4.3 KiB
C
132 lines
4.3 KiB
C
#include "fusion/fusion_rest_detector.h"
|
|
|
|
#include <math.h>
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static int failures;
|
|
|
|
#define CHECK(condition, message) do { \
|
|
if(!(condition)) { \
|
|
fprintf(stderr, "FAIL: %s\n", message); \
|
|
failures++; \
|
|
} \
|
|
} while(0)
|
|
|
|
static imu_sample_t sample_at(uint64_t ticks, uint64_t timebase_hz,
|
|
double gyro_x_rps, unsigned int sequence)
|
|
{
|
|
imu_sample_t sample;
|
|
memset(&sample, 0, sizeof(sample));
|
|
sample.timestamp_ticks = ticks;
|
|
double time_s = (double)ticks / (double)timebase_hz;
|
|
sample.accel_mps2[0] = 0.004 * sin(11.0 * time_s);
|
|
sample.accel_mps2[1] = 0.004 * cos(13.0 * time_s);
|
|
sample.accel_mps2[2] = 9.80665 + 0.004 * sin(17.0 * time_s);
|
|
sample.gyro_rps[0] = gyro_x_rps;
|
|
sample.gyro_rps[1] = 0.0002 * sin(7.0 * time_s);
|
|
sample.gyro_rps[2] = 0.0002 * cos(9.0 * time_s);
|
|
sample.temperature_c = 25.0;
|
|
sample.sequence = sequence;
|
|
return sample;
|
|
}
|
|
|
|
static void test_rate(unsigned int rate_hz)
|
|
{
|
|
const uint64_t timebase_hz = 1000000u;
|
|
fusion_rest_detector_config_t config;
|
|
fusion_rest_detector_t detector;
|
|
fusion_rest_detector_default_config(&config);
|
|
CHECK(fusion_rest_detector_init(
|
|
&detector, &config, timebase_hz, true),
|
|
"rest detector initializes in unarmed/landed mode");
|
|
|
|
bool initial_rest_seen = false;
|
|
bool impulse_cleared_rest = false;
|
|
double reentry_time_s = NAN;
|
|
uint64_t sample_count = 6u * rate_hz;
|
|
for(uint64_t index = 0u ; index <= sample_count ; index++)
|
|
{
|
|
uint64_t ticks = (index * timebase_hz + rate_hz / 2u) / rate_hz;
|
|
double time_s = (double)ticks / (double)timebase_hz;
|
|
double gyro_x = ((time_s >= 2.0) &&
|
|
(time_s < 2.0 + 2.0 / (double)rate_hz)) ? 4.0 : 0.0;
|
|
imu_sample_t sample = sample_at(
|
|
ticks, timebase_hz, gyro_x, (unsigned int)index);
|
|
CHECK(fusion_rest_detector_update(&detector, &sample),
|
|
"monotonic IMU sample updates rest detector");
|
|
if((time_s >= 1.0) && (time_s < 2.0) && detector.at_rest)
|
|
{
|
|
initial_rest_seen = true;
|
|
}
|
|
if((time_s >= 2.0) && (time_s < 2.2) && !detector.at_rest)
|
|
{
|
|
impulse_cleared_rest = true;
|
|
}
|
|
if((time_s > 2.0) && !isfinite(reentry_time_s) && detector.at_rest)
|
|
{
|
|
reentry_time_s = time_s;
|
|
}
|
|
}
|
|
CHECK(initial_rest_seen, "quiet landed platform reaches at-rest after dwell");
|
|
CHECK(impulse_cleared_rest, "high-frequency gyro impulse clears at-rest");
|
|
CHECK(isfinite(reentry_time_s) && (reentry_time_s >= 3.0) &&
|
|
(reentry_time_s <= 5.5),
|
|
"rest returns only after vibration decay and one-second dwell");
|
|
printf("rest-detector %3u Hz: reentry=%.3f s accel-vibe=%.6f gyro-vibe=%.6f\n",
|
|
rate_hz, reentry_time_s,
|
|
detector.accel_vibration_metric_mps2,
|
|
detector.gyro_vibration_metric_rps);
|
|
}
|
|
|
|
static void test_platform_gate_and_timestamps(void)
|
|
{
|
|
const uint64_t timebase_hz = 1000000u;
|
|
fusion_rest_detector_config_t config;
|
|
fusion_rest_detector_t detector;
|
|
fusion_rest_detector_default_config(&config);
|
|
CHECK(fusion_rest_detector_init(
|
|
&detector, &config, timebase_hz, false),
|
|
"rest detector initializes in mobile/not-landed mode");
|
|
for(unsigned int index = 0u ; index <= 150u ; index++)
|
|
{
|
|
imu_sample_t sample = sample_at(
|
|
(uint64_t)index * 10000u, timebase_hz, 0.0, index);
|
|
CHECK(fusion_rest_detector_update(&detector, &sample),
|
|
"mobile quiet samples remain valid");
|
|
}
|
|
CHECK(!detector.at_rest,
|
|
"quiet IMU cannot authorize ZUPT while platform is not landed");
|
|
|
|
imu_sample_t duplicate = sample_at(1500000u, timebase_hz, 0.0, 151u);
|
|
CHECK(!fusion_rest_detector_update(&detector, &duplicate),
|
|
"duplicate timestamp is rejected");
|
|
fusion_rest_detector_set_landed(&detector, true, 1500000u);
|
|
for(unsigned int index = 151u ; index <= 260u ; index++)
|
|
{
|
|
imu_sample_t sample = sample_at(
|
|
(uint64_t)index * 10000u, timebase_hz, 0.0, index);
|
|
CHECK(fusion_rest_detector_update(&detector, &sample),
|
|
"landed quiet samples update rest detector");
|
|
}
|
|
CHECK(detector.at_rest,
|
|
"external landed fact plus quiet dwell authorizes at-rest");
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
test_rate(52u);
|
|
test_rate(104u);
|
|
test_rate(208u);
|
|
test_platform_gate_and_timestamps();
|
|
if(failures != 0)
|
|
{
|
|
fprintf(stderr, "%d rest-detector test(s) failed\n", failures);
|
|
return 1;
|
|
}
|
|
printf("PX4-style platform rest detector tests passed.\n");
|
|
return 0;
|
|
}
|