onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Keep fused location streaming through GPS rejects

Browse Source
This commit is contained in:
onur
2026-07-23 23:20:23 +03:00
parent fc242b2883
commit 9ff895600b
14 changed files with 173 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/fusion/tests/platform_scenario_matrix_test.c
+36 -6
View File
@@ -474,14 +474,14 @@ static void print_metrics(scenario_kind_t scenario, unsigned int gps_ms,
static int run_matrix(void)
{
static const unsigned int gps_periods_ms[] = {200u, 500u, 1000u};
static const unsigned int imu_periods_ms[] = {19u, 20u, 200u, 300u};
static const unsigned int imu_periods_ms[] = {19u, 20u, 100u, 200u, 300u};
bool passed = true;
printf("Four-satellite asynchronous platform matrix; 3 s outage and 2 s coherent GNSS fault\n");
for(unsigned int scenario = 0u; scenario < SCENARIO_COUNT; scenario++)
{
for(unsigned int gps_index = 0u; gps_index < 3u; gps_index++)
{
for(unsigned int imu_index = 0u; imu_index < 4u; imu_index++)
for(unsigned int imu_index = 0u; imu_index < 5u; imu_index++)
{
metrics_t metrics = run_case((scenario_kind_t)scenario,
gps_periods_ms[gps_index], imu_periods_ms[imu_index], NULL);
@@ -504,7 +504,7 @@ static int run_matrix(void)
fprintf(stderr, "FAIL: platform scenario matrix contract.\n");
return 1;
}
printf("PASS: 19/20 ms production rates stayed bounded; 200/300 ms coarse IMU "
printf("PASS: 19/20 ms production rates stayed bounded; 100/200/300 ms coarse IMU "
"rates remained finite and their dynamic accuracy loss is reported, not hidden.\n");
return 0;
}
@@ -512,6 +512,33 @@ static int run_matrix(void)
int main(int argc, char** argv)
{
if(argc == 1) return run_matrix();
if((argc == 6) && (strcmp(argv[1], "--benchmark") == 0))
{
scenario_kind_t scenario;
if(!parse_scenario(argv[2], &scenario))
{
fprintf(stderr, "unknown scenario: %s\n", argv[2]);
return 2;
}
unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
unsigned int iterations = (unsigned int)strtoul(argv[5], NULL, 10);
if((iterations == 0u) ||
!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
!((imu_ms == 19u) || (imu_ms == 20u) || (imu_ms == 100u) ||
(imu_ms == 200u) || (imu_ms == 300u)))
{
fprintf(stderr, "supported GPS: 200/500/1000 ms; "
"IMU: 19/20/100/200/300 ms; iterations > 0\n");
return 2;
}
metrics_t metrics;
for(unsigned int iteration = 0u; iteration < iterations; iteration++)
metrics = run_case(scenario, gps_ms, imu_ms, NULL);
print_metrics(scenario, gps_ms, imu_ms, &metrics);
printf("completed %u benchmark iterations\n", iterations);
return metrics.finite ? 0 : 1;
}
if((argc == 6) && (strcmp(argv[1], "--log") == 0))
{
scenario_kind_t scenario;
@@ -523,10 +550,10 @@ int main(int argc, char** argv)
unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
if(!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
!((imu_ms == 19u) || (imu_ms == 20u) ||
!((imu_ms == 19u) || (imu_ms == 20u) || (imu_ms == 100u) ||
(imu_ms == 200u) || (imu_ms == 300u)))
{
fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/200/300 ms\n");
fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/100/200/300 ms\n");
return 2;
}
FILE* log_file = fopen(argv[5], "w");
@@ -546,6 +573,9 @@ int main(int argc, char** argv)
return metrics.finite ? 0 : 1;
}
fprintf(stderr, "usage: %s [--log stationary|pedestrian|car|aircraft "
"200|500|1000 19|20|200|300 output.log]\n", argv[0]);
"200|500|1000 19|20|100|200|300 output.log]\n"
" %s [--benchmark stationary|pedestrian|car|aircraft "
"200|500|1000 19|20|100|200|300 iterations]\n",
argv[0], argv[0]);
return 2;
}