Integrate asynchronous GPS IMU fusion

soc/software/gps_system/src/adapters/gps_fusion_publisher.c

@@ -0,0 +1,30 @@
+#include "adapters/gps_fusion_publisher.h"
+
+#include <stddef.h>
+#include <string.h>
+
+bool gps_fusion_build_measurement(
+	const gps_nav_solution_t* solution,
+	uint64_t measurement_timestamp_ticks,
+	fusion_gps_measurement_t* measurement_out)
+{
+	if((solution == NULL) || (measurement_timestamp_ticks == 0u) ||
+	   (measurement_out == NULL))
+	{
+		return false;
+	}
+	memset(measurement_out, 0, sizeof(*measurement_out));
+	measurement_out->timestamp_ticks = measurement_timestamp_ticks;
+	measurement_out->latitude_deg = solution->latitude_deg;
+	measurement_out->longitude_deg = solution->longitude_deg;
+	measurement_out->height_m = solution->height_m;
+	memcpy(measurement_out->covariance_enu_m2, solution->covariance_enu_m2,
+	       sizeof(measurement_out->covariance_enu_m2));
+	measurement_out->hacc_m = solution->hacc_m;
+	measurement_out->vacc_m = solution->vacc_m;
+	measurement_out->residual_rms_m = solution->residual_rms_m;
+	measurement_out->chi_square_ratio = solution->chi_square_ratio;
+	measurement_out->used_satellites = solution->used_satellite_count;
+	measurement_out->solution_valid = solution->solution_valid;
+	return true;
+}

soc/software/gps_system/src/adapters/gps_system_uart.c

@@ -0,0 +1,112 @@
+#include "adapters/gps_system_uart.h"
+
+#include <bsp/soc.h>
+#include <stddef.h>
+
+#include "gps/gps_macros.h"
+#include "gps/gps_receiver.h"
+#include "imu/imu_sensor.h"
+
+void gps_system_uart_publish_location(
+	const char* source,
+	double latitude_deg,
+	double longitude_deg,
+	double height_m,
+	const gps_nav_solution_t* gps)
+{
+	if((source == NULL) || (gps == NULL))
+	{
+		return;
+	}
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
+	        "source=%s lat=%.20lf lon=%.20lf height=%.3lf "
+	        "gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf "
+	        "tracked=%u used=%u\r\n",
+	        source, latitude_deg, longitude_deg, height_m,
+	        gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop,
+	        gps->tracked_satellite_count, gps->used_satellite_count);
+}
+
+void gps_system_uart_publish_startup(
+	bool fusion_enabled,
+	bool fusion_initialized,
+	bool imu_initialized,
+	bool imu_running)
+{
+	imu_sensor_diagnostics_t imu;
+	imu_sensor_get_diagnostics(&imu);
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
+	        "init eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
+	        "running=%u target_hz=%lu actual_hz=%lu nav_ms=%u timebase_hz=%lu\r\n",
+	        fusion_enabled ? 1u : 0u,
+	        fusion_initialized ? 1u : 0u,
+	        imu_initialized ? 1u : 0u,
+	        imu_sensor_backend_name(imu.backend),
+	        (unsigned int)imu.who_am_i,
+	        imu_running ? 1u : 0u,
+	        (unsigned long)imu.requested_odr_hz,
+	        (unsigned long)imu.configured_odr_hz,
+	        GPS_NAV_SOLVE_PERIOD_MS,
+	        (unsigned long)BSP_CLINT_TIMEBASE_HZ);
+}
+
+void gps_system_uart_publish_periodic_fusion_status(
+	navigation_fusion_output_t* output,
+	uint64_t* last_status_tick)
+{
+	if((output == NULL) || (last_status_tick == NULL))
+	{
+		return;
+	}
+	uint64_t now = gps_receiver_status_ticks();
+	if((now - *last_status_tick) < gps_receiver_status_ticks_per_second())
+	{
+		return;
+	}
+	*last_status_tick = now;
+
+	imu_sensor_diagnostics_t imu;
+	imu_sensor_get_diagnostics(&imu);
+	if(!navigation_fusion_get_output(output))
+	{
+		return;
+	}
+
+	if(output->global_valid)
+	{
+		GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+		        "state=%s lat=%.10lf lon=%.10lf height=%.3lf "
+		        "speed=%.3lf nis=%.3lf gps=%lu/%lu recoveries=%lu "
+		        "zupt=%lu replay=%lu "
+		        "sigma=%.2lf,%.2lf,%.2lf imu=%lu fused_imu=%lu "
+		        "overflow=%lu missed=%lu\r\n",
+		        fusion_motion_state_name(output->motion_state),
+		        output->latitude_deg, output->longitude_deg, output->height_m,
+		        output->speed_mps, output->last_position_nis,
+		        (unsigned long)output->gps_accepted,
+		        (unsigned long)output->gps_nis_rejected,
+		        (unsigned long)output->gps_position_recoveries,
+		        (unsigned long)output->zero_update_count,
+		        (unsigned long)output->replay_count,
+		        output->position_std_m[0], output->position_std_m[1],
+		        output->position_std_m[2],
+		        (unsigned long)imu.sample_count,
+		        (unsigned long)output->imu_processed_count,
+		        (unsigned long)imu.queue_overflow_count,
+		        (unsigned long)imu.estimated_missed_sample_count);
+		return;
+	}
+
+	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+	        "state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
+	        "imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
+	        navigation_fusion_state_name(output->engine_state),
+	        output->calibration_sample_count,
+	        (unsigned long)output->gps_accepted,
+	        (unsigned long)output->gps_nis_rejected,
+	        (unsigned long)output->gps_position_recoveries,
+	        (unsigned long)imu.sample_count,
+	        (unsigned long)output->imu_processed_count,
+	        (unsigned long)imu.queue_overflow_count,
+	        (unsigned long)imu.estimated_missed_sample_count);
+}

soc/software/gps_system/src/adapters/imu_fusion_profile.c

@@ -1,4 +1,4 @@
-#include "imu_sensor.h"
+#include "adapters/imu_fusion_profile.h"
 
 #include <stddef.h>
 
@@ -13,7 +13,7 @@ static void set_all(double values[3], double value)
 	}
 }
 
-bool imu_sensor_quality_profile_for_backend(
+bool imu_fusion_quality_profile_for_backend(
 	imu_sensor_backend_t backend,
 	fusion_imu_quality_profile_t* profile_out)
 {

soc/software/gps_system/src/drivers/imu/st_lsm6dsox.c

@@ -1,4 +1,4 @@
-#include "st_lsm6dsox.h"
+#include "drivers/imu/st_lsm6dsox.h"
 
 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -437,7 +437,7 @@ void st_lsm6dsox_service(void)
 }
 
 bool st_lsm6dsox_take_measurement(
-	fusion_imu_measurement_t* measurement_out)
+	imu_sample_t* measurement_out)
 {
 	if(measurement_out == NULL)
 	{

soc/software/gps_system/src/drivers/imu/tdk_icm42688.c

@@ -1,4 +1,4 @@
-#include "tdk_icm42688.h"
+#include "drivers/imu/tdk_icm42688.h"
 
 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -342,7 +342,7 @@ void tdk_icm42688_irq_handler(void)
 	__atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED);
 }
 
-bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out)
+bool tdk_icm42688_take_measurement(imu_sample_t* measurement_out)
 {
 	if(measurement_out == NULL)
 	{

soc/software/gps_system/src/fusion/eskf.c

@@ -1,4 +1,4 @@
-#include "eskf.h"
+#include "fusion/eskf.h"
 
 #include <float.h>
 #include <math.h>
@@ -325,21 +325,41 @@ bool eskf_initialize(eskf_t* filter,
 
 static bool eskf_propagate_step(eskf_t* filter,
 	                            const eskf_config_t* config,
-	                            const double accel_mps2[3],
-	                            const double gyro_rps[3],
+	                            const double start_accel_mps2[3],
+	                            const double start_gyro_rps[3],
+	                            const double end_accel_mps2[3],
+	                            const double end_gyro_rps[3],
 	                            double dt_s)
 {
 	double specific_force[3];
 	double angular_rate[3];
 	for(unsigned int i = 0 ; i < 3u ; i++)
 	{
-		specific_force[i] = accel_mps2[i] - filter->accel_bias_mps2[i];
-		angular_rate[i] = gyro_rps[i] - filter->gyro_bias_rps[i];
+		specific_force[i] =
+			0.5 * (start_accel_mps2[i] + end_accel_mps2[i]) -
+			filter->accel_bias_mps2[i];
+		angular_rate[i] =
+			0.5 * (start_gyro_rps[i] + end_gyro_rps[i]) -
+			filter->gyro_bias_rps[i];
 		filter->last_unbiased_gyro_rps[i] = angular_rate[i];
 	}
 
+	double half_rotation_vector[3] = {
+		0.5 * angular_rate[0] * dt_s,
+		0.5 * angular_rate[1] * dt_s,
+		0.5 * angular_rate[2] * dt_s
+	};
+	double half_delta_quaternion[4];
+	double midpoint_quaternion[4];
+	rotation_vector_quaternion(half_rotation_vector, half_delta_quaternion);
+	quaternion_multiply(filter->body_to_nav_quaternion, half_delta_quaternion,
+	                    midpoint_quaternion);
+	if(!quaternion_normalize(midpoint_quaternion))
+	{
+		return false;
+	}
 	double rotation[3][3];
-	quaternion_rotation_matrix(filter->body_to_nav_quaternion, rotation);
+	quaternion_rotation_matrix(midpoint_quaternion, rotation);
 	double linear_accel[3] = {0.0, 0.0, -config->gravity_mps2};
 	for(unsigned int row = 0 ; row < 3u ; row++)
 	{
@@ -412,22 +432,36 @@ static bool eskf_propagate_step(eskf_t* filter,
 	double propagated[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
-		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double coefficient = transition[row][k];
+			if(coefficient == 0.0)
+			{
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 			{
 				intermediate[row][col] +=
-					transition[row][k] * filter->covariance[k][col];
+					coefficient * filter->covariance[k][col];
 			}
 		}
 	}
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
-		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double value = intermediate[row][k];
+			if(value == 0.0)
 			{
-				propagated[row][col] += intermediate[row][k] * transition[col][k];
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			{
+				double coefficient = transition[col][k];
+				if(coefficient != 0.0)
+				{
+					propagated[row][col] += value * coefficient;
+				}
 			}
 		}
 	}
@@ -459,14 +493,20 @@ static bool eskf_propagate_step(eskf_t* filter,
 	return eskf_state_is_finite(filter);
 }
 
-bool eskf_propagate(eskf_t* filter,
-	                const eskf_config_t* config,
-	                const double accel_mps2[3],
-	                const double gyro_rps[3],
-	                double dt_s)
+bool eskf_propagate_interval(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double start_accel_mps2[3],
+	const double start_gyro_rps[3],
+	const double end_accel_mps2[3],
+	const double end_gyro_rps[3],
+	double dt_s)
 {
 	if((filter == NULL) || !filter->initialized || (config == NULL) ||
-	   !vector_is_finite(accel_mps2, 3u) || !vector_is_finite(gyro_rps, 3u) ||
+	   !vector_is_finite(start_accel_mps2, 3u) ||
+	   !vector_is_finite(start_gyro_rps, 3u) ||
+	   !vector_is_finite(end_accel_mps2, 3u) ||
+	   !vector_is_finite(end_gyro_rps, 3u) ||
 	   !isfinite(dt_s) || (dt_s < 0.0) ||
 	   !(config->max_propagation_step_s > 0.0))
 	{
@@ -484,7 +524,28 @@ bool eskf_propagate(eskf_t* filter,
 	double step_s = dt_s / (double)steps;
 	for(unsigned int step = 0 ; step < steps ; step++)
 	{
-		if(!eskf_propagate_step(filter, config, accel_mps2, gyro_rps, step_s))
+		double start_fraction = (double)step / (double)steps;
+		double end_fraction = (double)(step + 1u) / (double)steps;
+		double step_start_accel[3];
+		double step_start_gyro[3];
+		double step_end_accel[3];
+		double step_end_gyro[3];
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			double accel_delta = end_accel_mps2[axis] - start_accel_mps2[axis];
+			double gyro_delta = end_gyro_rps[axis] - start_gyro_rps[axis];
+			step_start_accel[axis] =
+				start_accel_mps2[axis] + start_fraction * accel_delta;
+			step_end_accel[axis] =
+				start_accel_mps2[axis] + end_fraction * accel_delta;
+			step_start_gyro[axis] =
+				start_gyro_rps[axis] + start_fraction * gyro_delta;
+			step_end_gyro[axis] =
+				start_gyro_rps[axis] + end_fraction * gyro_delta;
+		}
+		if(!eskf_propagate_step(filter, config,
+		                        step_start_accel, step_start_gyro,
+		                        step_end_accel, step_end_gyro, step_s))
 		{
 			return false;
 		}
@@ -492,6 +553,17 @@ bool eskf_propagate(eskf_t* filter,
 	return true;
 }
 
+bool eskf_propagate(eskf_t* filter,
+	                const eskf_config_t* config,
+	                const double accel_mps2[3],
+	                const double gyro_rps[3],
+	                double dt_s)
+{
+	return eskf_propagate_interval(filter, config,
+	                               accel_mps2, gyro_rps,
+	                               accel_mps2, gyro_rps, dt_s);
+}
+
 static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STATE_DIM])
 {
 	for(unsigned int i = 0 ; i < 3u ; i++)
@@ -538,21 +610,35 @@ static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STAT
 	double reset_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
-		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double coefficient = reset[row][k];
+			if(coefficient == 0.0)
 			{
-				intermediate[row][col] += reset[row][k] * filter->covariance[k][col];
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			{
+				intermediate[row][col] += coefficient * filter->covariance[k][col];
 			}
 		}
 	}
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
-		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double value = intermediate[row][k];
+			if(value == 0.0)
 			{
-				reset_covariance[row][col] += intermediate[row][k] * reset[col][k];
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			{
+				double coefficient = reset[col][k];
+				if(coefficient != 0.0)
+				{
+					reset_covariance[row][col] += value * coefficient;
+				}
 			}
 		}
 	}
@@ -646,12 +732,48 @@ static bool eskf_selected_update(eskf_t* filter,
 	double updated_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
 	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
 	{
-		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double coefficient = joseph_left[row][k];
+			if(coefficient == 0.0)
+			{
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 			{
 				intermediate[row][col] +=
-					joseph_left[row][k] * filter->covariance[k][col];
+					coefficient * filter->covariance[k][col];
+			}
+		}
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+		{
+			double value = intermediate[row][k];
+			if(value == 0.0)
+			{
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			{
+				double coefficient = joseph_left[col][k];
+				if(coefficient != 0.0)
+				{
+					updated_covariance[row][col] += value * coefficient;
+				}
+			}
+		}
+	}
+	double gain_times_covariance[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int second = 0 ; second < 3u ; second++)
+		{
+			for(unsigned int first = 0 ; first < 3u ; first++)
+			{
+				gain_times_covariance[row][second] += gain[row][first] *
+					measurement_covariance[first * 3u + second];
 			}
 		}
 	}
@@ -659,18 +781,10 @@ static bool eskf_selected_update(eskf_t* filter,
 	{
 		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			for(unsigned int second = 0 ; second < 3u ; second++)
 			{
 				updated_covariance[row][col] +=
-					intermediate[row][k] * joseph_left[col][k];
-			}
-			for(unsigned int first = 0 ; first < 3u ; first++)
-			{
-				for(unsigned int second = 0 ; second < 3u ; second++)
-				{
-					updated_covariance[row][col] += gain[row][first] *
-						measurement_covariance[first * 3u + second] * gain[col][second];
-				}
+					gain_times_covariance[row][second] * gain[col][second];
 			}
 		}
 	}
@@ -729,6 +843,50 @@ eskf_update_result_t eskf_position_update(
 	return ESKF_UPDATE_ACCEPTED;
 }
 
+bool eskf_reset_position(
+	eskf_t* filter,
+	const double position_m[3],
+	const double covariance_m2[9])
+{
+	if((filter == NULL) || !filter->initialized ||
+	   !vector_is_finite(position_m, 3u) ||
+	   !vector_is_finite(covariance_m2, 9u))
+	{
+		return false;
+	}
+
+	/* inverse_symmetric3() also rejects singular/non-positive-definite input. */
+	double unused_inverse[9];
+	if(!inverse_symmetric3(covariance_m2, unused_inverse))
+	{
+		return false;
+	}
+
+	memcpy(filter->position_m, position_m, 3u * sizeof(double));
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		unsigned int state = ESKF_POSITION_INDEX + axis;
+		for(unsigned int other = 0u ; other < ESKF_ERROR_STATE_DIM ; other++)
+		{
+			filter->covariance[state][other] = 0.0;
+			filter->covariance[other][state] = 0.0;
+		}
+	}
+	for(unsigned int row = 0u ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0u ; col < 3u ; col++)
+		{
+			filter->covariance[ESKF_POSITION_INDEX + row]
+			                  [ESKF_POSITION_INDEX + col] =
+				covariance_m2[row * 3u + col];
+		}
+	}
+	covariance_symmetrize_and_floor(filter, 1.0e-12);
+	filter->last_position_nis = NAN;
+	filter->position_update_accepted_count++;
+	return eskf_state_is_finite(filter);
+}
+
 bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps)
 {
 	if((filter == NULL) || !filter->initialized ||

soc/software/gps_system/src/fusion/fusion_bus.c

@@ -1,4 +1,4 @@
-#include "fusion_bus.h"
+#include "fusion/fusion_bus.h"
 
 #include <stddef.h>
 #include <string.h>

soc/software/gps_system/src/fusion/fusion_engine.c

@@ -1,4 +1,4 @@
-#include "fusion_engine.h"
+#include "fusion/fusion_engine.h"
 
 #include <float.h>
 #include <math.h>
@@ -123,16 +123,26 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
 	config->calibration_gyro_limit_rps = 2.0 * FUSION_PI / 180.0;
 	config->calibration_accel_std_limit_mps2 = 0.12;
 	config->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
-	config->zero_update_period_s = 0.05;
-	config->zero_velocity_std_mps = 0.03;
+	/* PX4 EKF2 fuses aligned zero velocity at 5 Hz with 0.2 m/s sigma. */
+	config->zero_update_period_s = 0.20;
+	config->zero_velocity_std_mps = 0.20;
 	for(unsigned int i = 0 ; i < 3u ; i++)
 	{
 		config->zero_gyro_std_rps[i] = 0.01 * FUSION_PI / 180.0;
 	}
 	config->gnss_motion_speed_threshold_mps = 0.50;
 	config->gnss_displacement_nis_gate = 9.21034037197618; /* 2 DOF, 99%. */
+	config->gnss_motion_window_s = 1.0;
 	config->gnss_moving_confirmations = 2u;
 	config->gnss_still_confirmations = 2u;
+	/* A hard innovation gate protects against isolated GNSS outliers, but it
+	 * must not permanently lock the filter to a bad startup position.  After a
+	 * full second of mutually continuous rejected fixes, re-anchor position
+	 * while preserving inertial velocity, attitude and bias states. */
+	config->position_recovery_timeout_s = 7.0;
+	config->position_recovery_max_speed_mps = 100.0;
+	config->position_recovery_sigma_multiplier = 5.0;
+	config->position_recovery_min_samples = 3u;
 }
 
 bool fusion_engine_apply_imu_quality(
@@ -178,8 +188,17 @@ static bool config_valid(const fusion_engine_config_t* config)
 	       isfinite(config->zero_velocity_std_mps) &&
 	       (config->zero_velocity_std_mps > 0.0) &&
 	       finite_values(config->zero_gyro_std_rps, 3u) &&
+	       isfinite(config->gnss_motion_window_s) &&
+	       (config->gnss_motion_window_s > 0.0) &&
 	       (config->gnss_moving_confirmations != 0u) &&
-	       (config->gnss_still_confirmations != 0u);
+	       (config->gnss_still_confirmations != 0u) &&
+	       isfinite(config->position_recovery_timeout_s) &&
+	       (config->position_recovery_timeout_s > 0.0) &&
+	       isfinite(config->position_recovery_max_speed_mps) &&
+	       (config->position_recovery_max_speed_mps > 0.0) &&
+	       isfinite(config->position_recovery_sigma_multiplier) &&
+	       (config->position_recovery_sigma_multiplier > 0.0) &&
+	       (config->position_recovery_min_samples >= 2u);
 }
 
 bool fusion_engine_init(fusion_engine_t* engine,
@@ -322,26 +341,32 @@ static void history_drop_through(fusion_engine_t* engine, uint64_t timestamp_tic
 
 static bool runtime_propagate_to(fusion_engine_t* engine,
 	                             fusion_runtime_t* runtime,
-	                             uint64_t timestamp_ticks,
-	                             const fusion_imu_measurement_t* input)
+	                             const fusion_imu_measurement_t* end_input)
 {
-	if((runtime == NULL) || (input == NULL) || !runtime->filter.initialized ||
-	   (timestamp_ticks < runtime->filter_timestamp_ticks))
+	if((runtime == NULL) || (end_input == NULL) ||
+	   !runtime->filter.initialized || !runtime->have_held_imu ||
+	   (runtime->filter_timestamp_ticks != runtime->held_imu.timestamp_ticks) ||
+	   (end_input->timestamp_ticks < runtime->filter_timestamp_ticks))
 	{
 		return false;
 	}
-	if(timestamp_ticks == runtime->filter_timestamp_ticks)
+	if(end_input->timestamp_ticks == runtime->filter_timestamp_ticks)
 	{
+		runtime->held_imu = *end_input;
 		return true;
 	}
 	double dt_s = ticks_to_seconds(engine,
-	                              timestamp_ticks - runtime->filter_timestamp_ticks);
-	if(!eskf_propagate(&runtime->filter, &engine->config.eskf,
-	                   input->accel_mps2, input->gyro_rps, dt_s))
+	                              end_input->timestamp_ticks -
+	                              runtime->filter_timestamp_ticks);
+	if(!eskf_propagate_interval(
+		&runtime->filter, &engine->config.eskf,
+		runtime->held_imu.accel_mps2, runtime->held_imu.gyro_rps,
+		end_input->accel_mps2, end_input->gyro_rps, dt_s))
 	{
 		return false;
 	}
-	runtime->filter_timestamp_ticks = timestamp_ticks;
+	runtime->filter_timestamp_ticks = end_input->timestamp_ticks;
+	runtime->held_imu = *end_input;
 	return true;
 }
 
@@ -364,7 +389,8 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
 	}
 	double motion_dt_s = ticks_to_seconds(
 		engine, measurement->timestamp_ticks - runtime->held_imu.timestamp_ticks);
-	if(!runtime_propagate_to(engine, runtime, measurement->timestamp_ticks, measurement))
+	fusion_imu_measurement_t start_measurement = runtime->held_imu;
+	if(!runtime_propagate_to(engine, runtime, measurement))
 	{
 		return false;
 	}
@@ -388,31 +414,82 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
 		runtime->gnss_moving_count = 0u;
 		runtime->gnss_still_count = 0u;
 	}
-	runtime->held_imu = *measurement;
-
+	runtime->vehicle_at_rest = measurement->vehicle_at_rest;
+	bool at_rest_continuing = measurement->vehicle_at_rest &&
+	                          start_measurement.vehicle_at_rest;
+	if(measurement->vehicle_at_rest)
+	{
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			double mean_gyro = 0.5 *
+				(start_measurement.gyro_rps[axis] + measurement->gyro_rps[axis]);
+			runtime->zero_gyro_integral_rad[axis] += mean_gyro * motion_dt_s;
+		}
+		runtime->zero_gyro_duration_s += motion_dt_s;
+	}
+	else
+	{
+		memset(runtime->zero_gyro_integral_rad, 0,
+		       sizeof(runtime->zero_gyro_integral_rad));
+		runtime->zero_gyro_duration_s = 0.0;
+	}
 	uint64_t zero_period_ticks =
 		seconds_to_ticks(engine, engine->config.zero_update_period_s);
-	if(fusion_motion_allows_zero_update(&runtime->motion) &&
+	if(at_rest_continuing &&
 	   ((measurement->timestamp_ticks - runtime->last_zero_update_ticks) >=
 	    zero_period_ticks))
 	{
-		if(!eskf_zero_velocity_update(&runtime->filter,
-		                              engine->config.zero_velocity_std_mps) ||
-		   !eskf_zero_angular_rate_update(&runtime->filter,
-		                                  measurement->gyro_rps,
-		                                  engine->config.zero_gyro_std_rps))
+		double mean_gyro_rps[3];
+		if(!(runtime->zero_gyro_duration_s > 0.0))
 		{
 			return false;
 		}
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			mean_gyro_rps[axis] = runtime->zero_gyro_integral_rad[axis] /
+			                      runtime->zero_gyro_duration_s;
+		}
+		if(!eskf_zero_velocity_update(
+			&runtime->filter, engine->config.zero_velocity_std_mps) ||
+		   !eskf_zero_angular_rate_update(
+			&runtime->filter, mean_gyro_rps, engine->config.zero_gyro_std_rps))
+		{
+			return false;
+		}
+		memset(runtime->zero_gyro_integral_rad, 0,
+		       sizeof(runtime->zero_gyro_integral_rad));
+		runtime->zero_gyro_duration_s = 0.0;
 		runtime->last_zero_update_ticks = measurement->timestamp_ticks;
 		if(count_zero_updates)
 		{
 			engine->diagnostics.zero_velocity_update_count++;
+			engine->diagnostics.zero_gyro_update_count++;
 		}
 	}
 	return true;
 }
 
+static fusion_imu_measurement_t interpolate_imu(
+	const fusion_imu_measurement_t* start,
+	const fusion_imu_measurement_t* end,
+	uint64_t timestamp_ticks)
+{
+	fusion_imu_measurement_t interpolated = *start;
+	interpolated.timestamp_ticks = timestamp_ticks;
+	double fraction = (double)(timestamp_ticks - start->timestamp_ticks) /
+	                  (double)(end->timestamp_ticks - start->timestamp_ticks);
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		interpolated.accel_mps2[axis] = start->accel_mps2[axis] + fraction *
+			(end->accel_mps2[axis] - start->accel_mps2[axis]);
+		interpolated.gyro_rps[axis] = start->gyro_rps[axis] + fraction *
+			(end->gyro_rps[axis] - start->gyro_rps[axis]);
+	}
+	interpolated.temperature_c = start->temperature_c + fraction *
+		(end->temperature_c - start->temperature_c);
+	return interpolated;
+}
+
 static bool finish_calibration(fusion_engine_t* engine,
 	                           const fusion_imu_measurement_t* last_measurement)
 {
@@ -461,6 +538,8 @@ static bool finish_calibration(fusion_engine_t* engine,
 	engine->current.have_held_imu = true;
 	engine->current.filter_timestamp_ticks = last_measurement->timestamp_ticks;
 	engine->current.last_zero_update_ticks = last_measurement->timestamp_ticks;
+	engine->current.last_position_fuse_ticks = last_measurement->timestamp_ticks;
+	engine->current.vehicle_at_rest = last_measurement->vehicle_at_rest;
 	engine->current.gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
 	if(engine->have_initial_position)
 	{
@@ -514,8 +593,13 @@ static bool process_calibration_imu(fusion_engine_t* engine,
 static fusion_gnss_motion_evidence_t position_motion_evidence(
 	const fusion_engine_t* engine,
 	const fusion_runtime_t* runtime,
-	const fusion_position_measurement_t* measurement)
+	const fusion_position_measurement_t* measurement,
+	bool* window_complete_out)
 {
+	if(window_complete_out != NULL)
+	{
+		*window_complete_out = false;
+	}
 	if(!runtime->have_accepted_position ||
 	   (measurement->timestamp_ticks <= runtime->last_accepted_position_ticks))
 	{
@@ -527,6 +611,14 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
 	{
 		return FUSION_GNSS_MOTION_UNKNOWN;
 	}
+	if(dt_s < engine->config.gnss_motion_window_s)
+	{
+		return FUSION_GNSS_MOTION_UNKNOWN;
+	}
+	if(window_complete_out != NULL)
+	{
+		*window_complete_out = true;
+	}
 	double delta_e = measurement->position_enu_m[0] - runtime->last_accepted_position_m[0];
 	double delta_n = measurement->position_enu_m[1] - runtime->last_accepted_position_m[1];
 	double s00 = measurement->covariance_enu_m2[0] +
@@ -556,6 +648,114 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
 	return FUSION_GNSS_MOTION_STILL;
 }
 
+static void position_recovery_clear(fusion_engine_t* engine)
+{
+	engine->position_recovery_active = false;
+	engine->position_recovery_start_ticks = 0u;
+	engine->position_recovery_consistent_count = 0u;
+	memset(&engine->position_recovery_candidate, 0,
+	       sizeof(engine->position_recovery_candidate));
+}
+
+static void position_recovery_begin(
+	fusion_engine_t* engine,
+	const fusion_position_measurement_t* measurement)
+{
+	engine->position_recovery_candidate = *measurement;
+	engine->position_recovery_start_ticks = measurement->timestamp_ticks;
+	engine->position_recovery_consistent_count = 1u;
+	engine->position_recovery_active = true;
+}
+
+static bool position_recovery_is_continuous(
+	const fusion_engine_t* engine,
+	const fusion_position_measurement_t* previous,
+	const fusion_position_measurement_t* current)
+{
+	if(current->timestamp_ticks <= previous->timestamp_ticks)
+	{
+		return false;
+	}
+	double dt_s = ticks_to_seconds(
+		engine, current->timestamp_ticks - previous->timestamp_ticks);
+	double displacement_squared = 0.0;
+	double combined_variance = 0.0;
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		double delta = current->position_enu_m[axis] -
+		               previous->position_enu_m[axis];
+		displacement_squared += delta * delta;
+		double variance = current->covariance_enu_m2[axis * 3u + axis] +
+		                  previous->covariance_enu_m2[axis * 3u + axis];
+		if(!(variance > 0.0) || !isfinite(variance))
+		{
+			return false;
+		}
+		combined_variance += variance;
+	}
+	double allowed_displacement =
+		engine->config.position_recovery_max_speed_mps * dt_s +
+		engine->config.position_recovery_sigma_multiplier *
+			sqrt(combined_variance);
+	return isfinite(displacement_squared) &&
+	       (displacement_squared <= square(allowed_displacement));
+}
+
+static bool position_recovery_ready(
+	fusion_engine_t* engine,
+	const fusion_runtime_t* runtime,
+	const fusion_position_measurement_t* measurement)
+{
+	if(!engine->position_recovery_active)
+	{
+		position_recovery_begin(engine, measurement);
+		return false;
+	}
+	if(!position_recovery_is_continuous(
+		engine, &engine->position_recovery_candidate, measurement))
+	{
+		/* A discontinuity is an isolated outlier or a new candidate track; it
+		 * must earn a complete recovery interval of its own. */
+		position_recovery_begin(engine, measurement);
+		return false;
+	}
+	engine->position_recovery_candidate = *measurement;
+	if(engine->position_recovery_consistent_count < UINT32_MAX)
+	{
+		engine->position_recovery_consistent_count++;
+	}
+	if(engine->position_recovery_consistent_count <
+	   engine->config.position_recovery_min_samples)
+	{
+		return false;
+	}
+	if(measurement->timestamp_ticks < runtime->last_position_fuse_ticks)
+	{
+		return false;
+	}
+	uint64_t elapsed_ticks = measurement->timestamp_ticks -
+	                         runtime->last_position_fuse_ticks;
+	return elapsed_ticks >= seconds_to_ticks(
+		engine, engine->config.position_recovery_timeout_s);
+}
+
+static void runtime_record_recovered_position(
+	fusion_runtime_t* runtime,
+	const fusion_position_measurement_t* measurement)
+{
+	runtime->have_accepted_position = true;
+	runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
+	runtime->last_position_fuse_ticks = measurement->timestamp_ticks;
+	memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
+	       sizeof(runtime->last_accepted_position_m));
+	memcpy(runtime->last_accepted_position_covariance_m2,
+	       measurement->covariance_enu_m2,
+	       sizeof(runtime->last_accepted_position_covariance_m2));
+	runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
+	runtime->gnss_moving_count = 0u;
+	runtime->gnss_still_count = 0u;
+}
+
 static eskf_update_result_t runtime_apply_position(
 	fusion_engine_t* engine,
 	fusion_runtime_t* runtime,
@@ -567,8 +767,11 @@ static eskf_update_result_t runtime_apply_position(
 		measurement->position_enu_m, measurement->covariance_enu_m2, nis_out);
 	if(result == ESKF_UPDATE_ACCEPTED)
 	{
+		runtime->last_position_fuse_ticks = measurement->timestamp_ticks;
+		bool motion_window_complete = false;
 		fusion_gnss_motion_evidence_t raw_evidence =
-			position_motion_evidence(engine, runtime, measurement);
+			position_motion_evidence(engine, runtime, measurement,
+			                         &motion_window_complete);
 		if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
 		{
 			runtime->gnss_still_count = 0u;
@@ -593,13 +796,19 @@ static eskf_update_result_t runtime_apply_position(
 				runtime->gnss_evidence = FUSION_GNSS_MOTION_STILL;
 			}
 		}
-		runtime->have_accepted_position = true;
-		runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
-		memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
-		       sizeof(runtime->last_accepted_position_m));
-		memcpy(runtime->last_accepted_position_covariance_m2,
-		       measurement->covariance_enu_m2,
-		       sizeof(runtime->last_accepted_position_covariance_m2));
+		/* Keep a long-enough displacement baseline.  At a 200 ms GNSS period,
+		 * replacing the reference on every fix would classify statistically
+		 * inconclusive 10--20 cm steps as stationary evidence. */
+		if(!runtime->have_accepted_position || motion_window_complete)
+		{
+			runtime->have_accepted_position = true;
+			runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
+			memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
+			       sizeof(runtime->last_accepted_position_m));
+			memcpy(runtime->last_accepted_position_covariance_m2,
+			       measurement->covariance_enu_m2,
+			       sizeof(runtime->last_accepted_position_covariance_m2));
+		}
 	}
 	return result;
 }
@@ -617,11 +826,13 @@ static bool process_position_now(fusion_engine_t* engine,
 	}
 	fusion_runtime_t revised = engine->checkpoint;
 	uint64_t replayed = 0u;
+	const fusion_imu_measurement_t* next_imu = NULL;
 	for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
 	{
 		const fusion_imu_measurement_t* imu = history_at(engine, i);
 		if(imu->timestamp_ticks > measurement->timestamp_ticks)
 		{
+			next_imu = imu;
 			break;
 		}
 		if(!runtime_apply_imu(engine, &revised, imu, false))
@@ -633,9 +844,20 @@ static bool process_position_now(fusion_engine_t* engine,
 	}
 	if(revised.filter_timestamp_ticks < measurement->timestamp_ticks)
 	{
-		if(!revised.have_held_imu ||
-		   !runtime_propagate_to(engine, &revised,
-		                         measurement->timestamp_ticks, &revised.held_imu))
+		if(!revised.have_held_imu)
+		{
+			engine->state = FUSION_ENGINE_FAULT;
+			return false;
+		}
+		fusion_imu_measurement_t endpoint = revised.held_imu;
+		endpoint.timestamp_ticks = measurement->timestamp_ticks;
+		if((next_imu != NULL) &&
+		   (next_imu->timestamp_ticks > revised.held_imu.timestamp_ticks))
+		{
+			endpoint = interpolate_imu(&revised.held_imu, next_imu,
+			                           measurement->timestamp_ticks);
+		}
+		if(!runtime_propagate_to(engine, &revised, &endpoint))
 		{
 			engine->state = FUSION_ENGINE_FAULT;
 			return false;
@@ -649,6 +871,23 @@ static bool process_position_now(fusion_engine_t* engine,
 		return false;
 	}
 	if(result == ESKF_UPDATE_ACCEPTED)
+	{
+		position_recovery_clear(engine);
+	}
+	else if((result == ESKF_UPDATE_REJECTED_NIS) &&
+	        position_recovery_ready(engine, &revised, measurement) &&
+	        eskf_reset_position(&revised.filter, measurement->position_enu_m,
+	                            measurement->covariance_enu_m2))
+	{
+		/* This is a deliberate position-state reset, not a Kalman update with
+		 * a disabled gate.  The independent IMU-derived attitude and bias state
+		 * survive, while invalid position cross-covariances are discarded. */
+		runtime_record_recovered_position(&revised, measurement);
+		engine->diagnostics.position_recovery_count++;
+		position_recovery_clear(engine);
+		result = ESKF_UPDATE_ACCEPTED;
+	}
+	if(result == ESKF_UPDATE_ACCEPTED)
 	{
 		engine->diagnostics.position_accepted++;
 	}

soc/software/gps_system/src/fusion/fusion_gps_adapter.c

@@ -1,4 +1,4 @@
-#include "fusion_gps_adapter.h"
+#include "fusion/fusion_gps_adapter.h"
 
 #include <float.h>
 #include <math.h>

soc/software/gps_system/src/fusion/fusion_motion.c

@@ -1,4 +1,4 @@
-#include "fusion_motion.h"
+#include "fusion/fusion_motion.h"
 
 #include <math.h>
 #include <stddef.h>

soc/software/gps_system/src/fusion/fusion_rest_detector.c

@@ -0,0 +1,144 @@
+#include "fusion/fusion_rest_detector.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+static double difference_norm3(const double current[3], const double previous[3])
+{
+	double sum = 0.0;
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		double difference = current[axis] - previous[axis];
+		sum += difference * difference;
+	}
+	return sqrt(sum);
+}
+
+void fusion_rest_detector_default_config(fusion_rest_detector_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	/* Same high-frequency vibration metric and limits used by PX4's current
+	 * VehicleIMU/LandDetector path. */
+	/* PX4's 0.99/0.01 filter at a 250 Hz reference rate, represented as a
+	 * physical time constant so 52/104/208 Hz sensors behave consistently. */
+	config->vibration_filter_time_constant_s =
+		-0.004 / log(0.99);
+	config->accel_vibration_limit_mps2 = 1.2;
+	config->gyro_vibration_limit_rps = 0.02;
+	config->rest_dwell_s = 1.0;
+}
+
+static bool config_valid(const fusion_rest_detector_config_t* config)
+{
+	return (config != NULL) &&
+	       isfinite(config->vibration_filter_time_constant_s) &&
+	       (config->vibration_filter_time_constant_s > 0.0) &&
+	       isfinite(config->accel_vibration_limit_mps2) &&
+	       (config->accel_vibration_limit_mps2 > 0.0) &&
+	       isfinite(config->gyro_vibration_limit_rps) &&
+	       (config->gyro_vibration_limit_rps > 0.0) &&
+	       isfinite(config->rest_dwell_s) && (config->rest_dwell_s > 0.0);
+}
+
+bool fusion_rest_detector_init(
+	fusion_rest_detector_t* detector,
+	const fusion_rest_detector_config_t* config,
+	uint64_t timebase_hz,
+	bool landed)
+{
+	if((detector == NULL) || !config_valid(config) || (timebase_hz == 0u))
+	{
+		return false;
+	}
+	memset(detector, 0, sizeof(*detector));
+	detector->config = *config;
+	detector->timebase_hz = timebase_hz;
+	detector->landed = landed;
+	return true;
+}
+
+void fusion_rest_detector_set_landed(
+	fusion_rest_detector_t* detector,
+	bool landed,
+	uint64_t timestamp_ticks)
+{
+	if(detector == NULL)
+	{
+		return;
+	}
+	detector->landed = landed;
+	if(!landed)
+	{
+		detector->at_rest = false;
+		detector->last_motion_ticks = timestamp_ticks;
+	}
+}
+
+bool fusion_rest_detector_update(
+	fusion_rest_detector_t* detector,
+	const imu_sample_t* sample)
+{
+	if((detector == NULL) || (sample == NULL) ||
+	   !isfinite(sample->accel_mps2[0]) ||
+	   !isfinite(sample->accel_mps2[1]) ||
+	   !isfinite(sample->accel_mps2[2]) ||
+	   !isfinite(sample->gyro_rps[0]) ||
+	   !isfinite(sample->gyro_rps[1]) ||
+	   !isfinite(sample->gyro_rps[2]) ||
+	   (detector->have_previous_sample &&
+	    (sample->timestamp_ticks <= detector->last_sample_ticks)))
+	{
+		return false;
+	}
+
+	if(!detector->have_previous_sample)
+	{
+		memcpy(detector->previous_accel_mps2, sample->accel_mps2,
+		       sizeof(detector->previous_accel_mps2));
+		memcpy(detector->previous_gyro_rps, sample->gyro_rps,
+		       sizeof(detector->previous_gyro_rps));
+		detector->last_sample_ticks = sample->timestamp_ticks;
+		detector->last_motion_ticks = sample->timestamp_ticks;
+		detector->have_previous_sample = true;
+		detector->at_rest = false;
+		return true;
+	}
+
+	double accel_difference = difference_norm3(
+		sample->accel_mps2, detector->previous_accel_mps2);
+	double gyro_difference = difference_norm3(
+		sample->gyro_rps, detector->previous_gyro_rps);
+	double dt_s = (double)(sample->timestamp_ticks - detector->last_sample_ticks) /
+	              (double)detector->timebase_hz;
+	double alpha = 1.0 - exp(
+		-dt_s / detector->config.vibration_filter_time_constant_s);
+	detector->accel_vibration_metric_mps2 =
+		(1.0 - alpha) * detector->accel_vibration_metric_mps2 +
+		alpha * accel_difference;
+	detector->gyro_vibration_metric_rps =
+		(1.0 - alpha) * detector->gyro_vibration_metric_rps +
+		alpha * gyro_difference;
+	memcpy(detector->previous_accel_mps2, sample->accel_mps2,
+	       sizeof(detector->previous_accel_mps2));
+	memcpy(detector->previous_gyro_rps, sample->gyro_rps,
+	       sizeof(detector->previous_gyro_rps));
+	detector->last_sample_ticks = sample->timestamp_ticks;
+
+	if((detector->accel_vibration_metric_mps2 >
+	    detector->config.accel_vibration_limit_mps2) ||
+	   (detector->gyro_vibration_metric_rps >
+	    detector->config.gyro_vibration_limit_rps))
+	{
+		detector->last_motion_ticks = sample->timestamp_ticks;
+	}
+
+	uint64_t dwell_ticks = (uint64_t)(
+		detector->config.rest_dwell_s * (double)detector->timebase_hz + 0.5);
+	detector->at_rest = detector->landed &&
+		((sample->timestamp_ticks - detector->last_motion_ticks) >= dwell_ticks);
+	return true;
+}

soc/software/gps_system/src/fusion/navigation_fusion.c

@@ -0,0 +1,162 @@
+#include "fusion/navigation_fusion.h"
+
+#include "fusion/fusion_gps_adapter.h"
+#include "fusion/fusion_rest_detector.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+static fusion_gps_adapter_t gps_adapter;
+static fusion_engine_t fusion_engine;
+static fusion_rest_detector_t rest_detector;
+static bool fusion_initialized;
+static uint64_t imu_processed_count;
+
+bool navigation_fusion_init(
+	uint64_t local_timebase_hz,
+	const fusion_imu_quality_profile_t* imu_quality,
+	bool platform_landed)
+{
+	fusion_engine_config_t engine_config;
+	fusion_gps_adapter_config_t adapter_config;
+	fusion_rest_detector_config_t rest_config;
+	if((local_timebase_hz == 0u) ||
+	   !fusion_imu_quality_is_valid(imu_quality))
+	{
+		return false;
+	}
+	fusion_engine_default_config(&engine_config, local_timebase_hz);
+	if(!fusion_engine_apply_imu_quality(&engine_config, imu_quality))
+	{
+		return false;
+	}
+	fusion_gps_adapter_default_config(&adapter_config);
+	fusion_rest_detector_default_config(&rest_config);
+	if(!fusion_engine_init(&fusion_engine, &engine_config) ||
+	   !fusion_gps_adapter_init(&gps_adapter, &adapter_config) ||
+	   !fusion_rest_detector_init(
+		&rest_detector, &rest_config, local_timebase_hz, platform_landed))
+	{
+		return false;
+	}
+	imu_processed_count = 0u;
+	fusion_initialized = true;
+	return true;
+}
+
+void navigation_fusion_set_platform_landed(
+	bool platform_landed,
+	uint64_t timestamp_ticks)
+{
+	if(fusion_initialized)
+	{
+		fusion_rest_detector_set_landed(
+			&rest_detector, platform_landed, timestamp_ticks);
+	}
+}
+
+bool navigation_fusion_push_imu(const imu_sample_t* measurement)
+{
+	if(!fusion_initialized || (measurement == NULL))
+	{
+		return false;
+	}
+	if(!fusion_rest_detector_update(&rest_detector, measurement))
+	{
+		return false;
+	}
+	fusion_imu_measurement_t fusion_measurement;
+	memset(&fusion_measurement, 0, sizeof(fusion_measurement));
+	fusion_measurement.timestamp_ticks = measurement->timestamp_ticks;
+	memcpy(fusion_measurement.accel_mps2, measurement->accel_mps2,
+	       sizeof(fusion_measurement.accel_mps2));
+	memcpy(fusion_measurement.gyro_rps, measurement->gyro_rps,
+	       sizeof(fusion_measurement.gyro_rps));
+	fusion_measurement.temperature_c = measurement->temperature_c;
+	fusion_measurement.sequence = measurement->sequence;
+	fusion_measurement.vehicle_at_rest = rest_detector.at_rest;
+	if(!fusion_engine_push_imu(&fusion_engine, &fusion_measurement))
+	{
+		return false;
+	}
+	imu_processed_count++;
+	return true;
+}
+
+bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement)
+{
+	if(!fusion_initialized)
+	{
+		return false;
+	}
+	fusion_position_measurement_t position;
+	if(!fusion_gps_adapter_convert(&gps_adapter, measurement, &position))
+	{
+		return false;
+	}
+	return fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
+}
+
+bool navigation_fusion_get_output(navigation_fusion_output_t* output)
+{
+	if((output == NULL) || !fusion_initialized)
+	{
+		return false;
+	}
+	memset(output, 0, sizeof(*output));
+	output->engine_state = fusion_engine.state;
+	output->calibration_sample_count = fusion_engine.calibration.sample_count;
+	output->gps_accepted = fusion_engine.diagnostics.position_accepted;
+	output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
+	output->gps_position_recoveries =
+		fusion_engine.diagnostics.position_recovery_count;
+	output->zero_update_count =
+		fusion_engine.diagnostics.zero_velocity_update_count;
+	output->replay_count = fusion_engine.diagnostics.replay_count;
+	output->stale_gps_count =
+		fusion_engine.diagnostics.position_stale_rejected;
+	output->imu_processed_count = imu_processed_count;
+	if(fusion_engine.state != FUSION_ENGINE_RUNNING)
+	{
+		return true;
+	}
+	const fusion_runtime_t* runtime = &fusion_engine.current;
+	output->timestamp_ticks = runtime->filter_timestamp_ticks;
+	memcpy(output->position_enu_m, runtime->filter.position_m,
+	       sizeof(output->position_enu_m));
+	memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps,
+	       sizeof(output->velocity_enu_mps));
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		output->position_std_m[axis] =
+			sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
+	}
+	output->speed_mps = eskf_speed_mps(&runtime->filter);
+	output->last_position_nis = runtime->filter.last_position_nis;
+	output->motion_state = runtime->motion.state;
+	output->vehicle_at_rest = runtime->vehicle_at_rest;
+	output->local_valid = true;
+	if(!fusion_gps_adapter_local_to_geodetic(
+		&gps_adapter, runtime->filter.position_m,
+		&output->latitude_deg, &output->longitude_deg, &output->height_m))
+	{
+		/* IMU propagation is still a valid local dead-reckoning state.  Without
+		 * an externally established origin it must not be labelled as LLH. */
+		return true;
+	}
+	output->global_valid = true;
+	output->valid = true;
+	return true;
+}
+
+const char* navigation_fusion_state_name(fusion_engine_state_t state)
+{
+	switch(state)
+	{
+		case FUSION_ENGINE_CALIBRATING: return "calibrating";
+		case FUSION_ENGINE_RUNNING: return "running";
+		case FUSION_ENGINE_FAULT: return "fault";
+		default: return "unknown";
+	}
+}

soc/software/gps_system/src/gps/NavEquations.c

@@ -1,11 +1,11 @@
 #include <stdlib.h>
-#include "NavEquations.h"
-#include "Navigation.h"
-#include "gps_ip_defines.h"
-#include "gps.h"
+#include "gps/NavEquations.h"
+#include "gps/Navigation.h"
+#include "gps/gps_ip_defines.h"
+#include "gps/gps.h"
 #include <math.h>
 #include <string.h>
-#include "defines.h"
+#include "gps/defines.h"
 
 
 /* Function:  double computeRange(range_t* rho, ephem_t eph, ionoutc_t* ionoutc, gpstime_t g, double xyz[], t_temp_pos* tempPos)
@@ -1199,7 +1199,13 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
 	solution_out->residual_test_valid = residual_test_valid;
 	solution_out->chi_square_pass = chi_square_pass;
 	solution_out->fde_applied = false;
-	solution_out->solution_valid = chi_square_pass;
+	/* Four observations exactly determine x/y/z plus receiver clock bias.  With
+	 * zero residual DOF no chi-square integrity statement is possible, but the
+	 * finite WLS solution and geometry covariance are still a valid minimum fix. */
+	solution_out->solution_valid = solution_out->candidate_valid &&
+		(((solution_out->used_satellite_count == 4u) &&
+		  (solution_out->degrees_of_freedom == 0u)) ||
+		 (solution_out->residual_test_valid && solution_out->chi_square_pass));
 	solution_out->iteration_count = iteration_count;
 	solution_out->candidate_cycles = gps_read_cycle_counter() - candidate_start_cycles;
 
@@ -1386,8 +1392,10 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 	                             ? (gps_read_cycle_counter() - fde_start_cycles) : 0u;
 	selected_solution.total_wls_cycles = gps_read_cycle_counter() - total_wls_start_cycles;
 	selected_solution.solution_valid = selected_solution.candidate_valid &&
-	                                   selected_solution.residual_test_valid &&
-	                                   selected_solution.chi_square_pass;
+		(((selected_solution.used_satellite_count == 4u) &&
+		  (selected_solution.degrees_of_freedom == 0u)) ||
+		 (selected_solution.residual_test_valid &&
+		  selected_solution.chi_square_pass));
 	if(selected_solution.solution_valid)
 	{
 		gps_wls_last_solution[0] = selected_solution.ecef_m[0];
@@ -1398,12 +1406,19 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 	}
 	*solution_out = selected_solution;
 
-	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
-	        "GDOP=%.10lf\r\n", solution_out->gdop);
-	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
-	        "position lat=%.20lf lon=%.20lf height=%.3lf\r\n",
-	        solution_out->latitude_deg, solution_out->longitude_deg,
-	        solution_out->height_m);
+	/* Print only publishable fixes.  A four-satellite minimum fix is publishable,
+	 * although its separate integrity flag remains false because residual DOF=0. */
+	if(solution_out->solution_valid)
+	{
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "DOP gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf\r\n",
+		        solution_out->gdop, solution_out->pdop, solution_out->hdop,
+		        solution_out->vdop, solution_out->tdop);
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "position lat=%.20lf lon=%.20lf height=%.3lf\r\n",
+		        solution_out->latitude_deg, solution_out->longitude_deg,
+		        solution_out->height_m);
+	}
 	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
 	        "NAVSOL valid=%u integrity=%u fde=%u sats=%u/%u excluded=0x%03x "
 	        "residual_rms=%.3lf max_residual=%.3lf weighted_rms=%.3lf "

soc/software/gps_system/src/gps/gps_interrupt_handlers.c

@@ -1,9 +1,8 @@
-#include "linked_list.h"
-#include "gps_interrupt_handlers.h"
-#include "Navigation.h"
-#include "gps_pps.h"
-#include "gps_time_anchor.h"
-#include "imu_sensor.h"
+#include "gps/linked_list.h"
+#include "gps/gps_interrupt_handlers.h"
+#include "gps/Navigation.h"
+#include "gps/gps_pps.h"
+#include "gps/gps_time_anchor.h"
 #include <bsp/irq.h>
 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -74,6 +73,8 @@ static volatile bool gps_timer_maintenance_pending = true;
 static volatile bool gps_nav_solve_period_pending = false;
 static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
 static unsigned int gps_acq_round_robin_prn = 0;
+static unsigned int gps_external_irq_id = 0u;
+static gps_external_irq_handler_t gps_external_irq_handler = NULL;
 
 #define GPS_PLIC_DRAIN_LIMIT (GPS_TRCK_IP_COUNT + GPS_ACQ_IP_COUNT + 4u)
 
@@ -295,6 +296,8 @@ static void gps_runtime_state_init(void)
 	gps_runtime_diagnostics.last_connected_prn = GPS_PRN_INVALID;
 	gps_runtime_diagnostics.last_lost_prn = GPS_PRN_INVALID;
 	gps_acq_round_robin_prn = 0;
+	gps_external_irq_id = 0u;
+	gps_external_irq_handler = NULL;
 
 	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
 	{
@@ -325,6 +328,22 @@ static void gps_runtime_state_init(void)
 
 }
 
+bool gps_interrupt_attach_external_source(
+	unsigned int irq_id,
+	gps_external_irq_handler_t handler)
+{
+	if((irq_id == 0u) || (handler == NULL))
+	{
+		return false;
+	}
+
+	uint64_t irq_state = bsp_irq_save();
+	gps_external_irq_id = irq_id;
+	gps_external_irq_handler = handler;
+	bsp_irq_restore(irq_state);
+	return true;
+}
+
 static void gps_dispatch_tracking(unsigned char trck_ip_no)
 {
 	trck_intr_handler(
@@ -395,9 +414,10 @@ void gps_interrupt_init(void)
 
 void gps_dispatch_irq(unsigned int irq_id)
 {
-	if(irq_id == BSP_PMOD_IRQ)
+	if((irq_id == gps_external_irq_id) &&
+	   (gps_external_irq_handler != NULL))
 	{
-		imu_sensor_irq_handler();
+		gps_external_irq_handler();
 		return;
 	}
 

soc/software/gps_system/src/gps/gps_ip.c

@@ -3,14 +3,13 @@
 #include <stdlib.h>
 #include <math.h>
 #include <string.h>
-#include "NavEquations.h"
-#include "Navigation.h"
-#include "gps_ip_defines.h"
-#include "linked_list.h"
-#include "gps_interrupt_handlers.h"
-#include "gps_pps.h"
-#include "gps_fusion_publisher.h"
-#include "defines.h"
+#include "gps/NavEquations.h"
+#include "gps/Navigation.h"
+#include "gps/gps_ip_defines.h"
+#include "gps/linked_list.h"
+#include "gps/gps_interrupt_handlers.h"
+#include "gps/gps_pps.h"
+#include "gps/defines.h"
 #include <bsp/irq.h>
 
 #define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES ((uint32_t)(SAMPLES_PER_CODE * (double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS))
@@ -1086,11 +1085,19 @@ static void gps_log_nav_jump_diagnostic(const gps_nav_solution_t* solution,
 #endif
 
 #ifdef EKF_ENABLE
-void calculate_position(Ekf* g_ekf)
+bool calculate_position(Ekf* g_ekf, gps_nav_solution_t* solution_out,
+	                    uint64_t* measurement_timestamp_ticks_out)
 #else
-void calculate_position()
+bool calculate_position(gps_nav_solution_t* solution_out,
+	                    uint64_t* measurement_timestamp_ticks_out)
 #endif
 {
+	if((solution_out == NULL) || (measurement_timestamp_ticks_out == NULL))
+	{
+		return false;
+	}
+	*measurement_timestamp_ticks_out = 0u;
+	bool result_ready = false;
 	gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT];
 	unsigned char snapshot_ips[GPS_TRCK_IP_COUNT];
 	uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT];
@@ -1186,13 +1193,14 @@ void calculate_position()
 		uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
 		if(nav_ok)
 		{
+			nav_solution.tracked_satellite_count = busy_count;
 			uint64_t measurement_timestamp = gps_nav_median_local_timestamp(
 				snapshot_local_timestamps, satellite_count);
-			if(!gps_fusion_publish_solution(&nav_solution, measurement_timestamp))
+			if(measurement_timestamp != 0u)
 			{
-				GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
-				        "fusion GPS publish skipped timestamp=%lu\r\n",
-				        (unsigned long)measurement_timestamp);
+				*solution_out = nav_solution;
+				*measurement_timestamp_ticks_out = measurement_timestamp;
+				result_ready = true;
 			}
 			uint64_t consume_irq_state = bsp_irq_save();
 			gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count);
@@ -1264,6 +1272,7 @@ void calculate_position()
 			}
 		}
 	}
+	return result_ready;
 }
 
 

soc/software/gps_system/src/gps/gps_pps.c

@@ -1,4 +1,4 @@
-#include "gps_pps.h"
+#include "gps/gps_pps.h"
 
 #include <bsp/soc.h>
 #include <math.h>

soc/software/gps_system/src/gps/gps_receiver.c

@@ -0,0 +1,44 @@
+#include "gps/gps_receiver.h"
+
+#include "gps/NavEquations.h"
+#include "gps/gps_interrupt_handlers.h"
+#include "gps/gps_ip_defines.h"
+
+void gps_receiver_initialize(unsigned int nav_solve_timer_ticks)
+{
+	gps_startup_initialization(nav_solve_timer_ticks);
+}
+
+bool gps_receiver_attach_external_irq(
+	unsigned int irq_id,
+	gps_receiver_external_irq_handler_t handler)
+{
+	return gps_interrupt_attach_external_source(irq_id, handler);
+}
+
+void gps_receiver_start_acquisition(void)
+{
+	acq_start_loop();
+}
+
+void gps_receiver_process_pending_events(void)
+{
+	gps_background_task();
+}
+
+bool gps_receiver_solve_ready_epoch(
+	gps_nav_solution_t* solution_out,
+	uint64_t* measurement_timestamp_ticks_out)
+{
+	return calculate_position(solution_out, measurement_timestamp_ticks_out);
+}
+
+uint64_t gps_receiver_status_ticks(void)
+{
+	return gps_get_systick();
+}
+
+uint64_t gps_receiver_status_ticks_per_second(void)
+{
+	return GPS_SYSTICK_HZ;
+}

soc/software/gps_system/src/gps/gps_time_anchor.c

@@ -1,4 +1,4 @@
-#include "gps_time_anchor.h"
+#include "gps/gps_time_anchor.h"
 
 #include <math.h>
 #include <stddef.h>

soc/software/gps_system/src/gps/linked_list.c

@@ -1,4 +1,4 @@
-#include "linked_list.h"
+#include "gps/linked_list.h"
 
 #include <stdlib.h>
 

soc/software/gps_system/src/gps/matrixLib.c

@@ -5,8 +5,8 @@
  * (c) Copyright by ANKASYS
  **/
 
-#include "matrixLib.h"
-#include "gps_macros.h"
+#include "gps/matrixLib.h"
+#include "gps/gps_macros.h"
 #include <float.h>
 #include <math.h>
 

soc/software/gps_system/src/gps_fusion_publisher.c

@@ -1,30 +0,0 @@
-#include "gps_fusion_publisher.h"
-
-#include "navigation_fusion.h"
-
-#include <stddef.h>
-#include <string.h>
-
-bool gps_fusion_publish_solution(const gps_nav_solution_t* solution,
-	                             uint64_t measurement_timestamp_ticks)
-{
-	if((solution == NULL) || (measurement_timestamp_ticks == 0u))
-	{
-		return false;
-	}
-	fusion_gps_measurement_t measurement;
-	memset(&measurement, 0, sizeof(measurement));
-	measurement.timestamp_ticks = measurement_timestamp_ticks;
-	measurement.latitude_deg = solution->latitude_deg;
-	measurement.longitude_deg = solution->longitude_deg;
-	measurement.height_m = solution->height_m;
-	memcpy(measurement.covariance_enu_m2, solution->covariance_enu_m2,
-	       sizeof(measurement.covariance_enu_m2));
-	measurement.hacc_m = solution->hacc_m;
-	measurement.vacc_m = solution->vacc_m;
-	measurement.residual_rms_m = solution->residual_rms_m;
-	measurement.chi_square_ratio = solution->chi_square_ratio;
-	measurement.used_satellites = solution->used_satellite_count;
-	measurement.solution_valid = solution->solution_valid;
-	return navigation_fusion_publish_gps(&measurement);
-}

soc/software/gps_system/src/imu/imu_calibration.c

@@ -1,4 +1,4 @@
-#include "imu_calibration.h"
+#include "imu/imu_calibration.h"
 
 #include <math.h>
 #include <stddef.h>
@@ -60,7 +60,7 @@ bool imu_calibration_is_valid(const imu_calibration_t* calibration)
 }
 
 bool imu_calibration_apply(const imu_calibration_t* calibration,
-	                       fusion_imu_measurement_t* measurement)
+	                       imu_sample_t* measurement)
 {
 	if(!imu_calibration_is_valid(calibration) || (measurement == NULL) ||
 	   !vector_is_finite(measurement->accel_mps2) ||

soc/software/gps_system/src/imu/imu_sensor.c

@@ -1,14 +1,14 @@
-#include "imu_sensor.h"
+#include "imu/imu_sensor.h"
 
 #include <stddef.h>
 #include <string.h>
 
 #if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
-#include "tdk_icm42688.h"
+#include "drivers/imu/tdk_icm42688.h"
 #endif
 
 #if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
-#include "st_lsm6dsox.h"
+#include "drivers/imu/st_lsm6dsox.h"
 #endif
 
 #if !defined(IMU_SENSOR_BUILD_AUTO) && !defined(IMU_SENSOR_BUILD_TDK) && \
@@ -18,9 +18,47 @@
 
 static imu_sensor_backend_t active_backend;
 static imu_calibration_t active_calibration;
+static uint32_t requested_odr_hz;
 static uint32_t active_odr_hz;
 static uint64_t calibration_rejected_count;
 
+#define IMU_ODR_MAX_ERROR_PERCENT 5u
+
+static bool nearest_supported_odr(uint32_t requested,
+	                              const uint32_t* supported,
+	                              unsigned int count,
+	                              uint32_t* selected)
+{
+	if((supported == NULL) || (count == 0u) || (selected == NULL))
+	{
+		return false;
+	}
+	if(requested == 0u)
+	{
+		*selected = 0u;
+		return true;
+	}
+	uint32_t best = supported[0];
+	uint32_t best_error = (best > requested) ? best - requested : requested - best;
+	for(unsigned int i = 1u ; i < count ; i++)
+	{
+		uint32_t error = (supported[i] > requested)
+		               ? supported[i] - requested : requested - supported[i];
+		if(error < best_error)
+		{
+			best = supported[i];
+			best_error = error;
+		}
+	}
+	if(((uint64_t)best_error * 100u) >
+	   ((uint64_t)requested * IMU_ODR_MAX_ERROR_PERCENT))
+	{
+		return false;
+	}
+	*selected = best;
+	return true;
+}
+
 void imu_sensor_default_config(imu_sensor_config_t* config)
 {
 	if(config == NULL)
@@ -72,6 +110,9 @@ static bool tdk_gyro_range(unsigned int full_scale_dps,
 
 static bool try_tdk(const imu_sensor_config_t* config)
 {
+	static const uint32_t supported_odr_hz[] = {
+		25u, 50u, 100u, 200u, 500u, 1000u
+	};
 	if(!imu_calibration_is_valid(
 		&config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688]))
 	{
@@ -79,9 +120,17 @@ static bool try_tdk(const imu_sensor_config_t* config)
 	}
 	tdk_icm42688_config_t backend_config;
 	tdk_icm42688_default_config(&backend_config);
-	if(config->odr_hz != 0u)
+	uint32_t selected_odr_hz;
+	if(!nearest_supported_odr(config->odr_hz, supported_odr_hz,
+	                          sizeof(supported_odr_hz) /
+	                          sizeof(supported_odr_hz[0]),
+	                          &selected_odr_hz))
 	{
-		backend_config.odr_hz = config->odr_hz;
+		return false;
+	}
+	if(selected_odr_hz != 0u)
+	{
+		backend_config.odr_hz = selected_odr_hz;
 	}
 	if((config->accel_full_scale_g != 0u) &&
 	   !tdk_accel_range(config->accel_full_scale_g,
@@ -110,6 +159,7 @@ static bool try_tdk(const imu_sensor_config_t* config)
 #if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
 static bool try_st(const imu_sensor_config_t* config)
 {
+	static const uint32_t supported_odr_hz[] = {52u, 104u, 208u};
 	if(!imu_calibration_is_valid(
 		&config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX]))
 	{
@@ -117,9 +167,17 @@ static bool try_st(const imu_sensor_config_t* config)
 	}
 	st_lsm6dsox_config_t backend_config;
 	st_lsm6dsox_default_config(&backend_config);
-	if(config->odr_hz != 0u)
+	uint32_t selected_odr_hz;
+	if(!nearest_supported_odr(config->odr_hz, supported_odr_hz,
+	                          sizeof(supported_odr_hz) /
+	                          sizeof(supported_odr_hz[0]),
+	                          &selected_odr_hz))
 	{
-		backend_config.odr_hz = config->odr_hz;
+		return false;
+	}
+	if(selected_odr_hz != 0u)
+	{
+		backend_config.odr_hz = selected_odr_hz;
 	}
 	if(config->accel_full_scale_g != 0u)
 	{
@@ -144,12 +202,14 @@ static bool try_st(const imu_sensor_config_t* config)
 bool imu_sensor_init(const imu_sensor_config_t* config)
 {
 	active_backend = IMU_SENSOR_NONE;
+	requested_odr_hz = 0u;
 	active_odr_hz = 0u;
 	calibration_rejected_count = 0u;
 	if(config == NULL)
 	{
 		return false;
 	}
+	requested_odr_hz = config->odr_hz;
 
 #if defined(IMU_SENSOR_BUILD_TDK)
 	return try_tdk(config);
@@ -232,7 +292,7 @@ bool imu_sensor_requires_foreground_service(void)
 	return false;
 }
 
-bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out)
+bool imu_sensor_take_measurement(imu_sample_t* measurement_out)
 {
 	if(measurement_out == NULL)
 	{
@@ -276,6 +336,7 @@ void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out)
 	}
 	memset(diagnostics_out, 0, sizeof(*diagnostics_out));
 	diagnostics_out->backend = active_backend;
+	diagnostics_out->requested_odr_hz = requested_odr_hz;
 	diagnostics_out->configured_odr_hz = active_odr_hz;
 	diagnostics_out->foreground_polling =
 		imu_sensor_requires_foreground_service();
@@ -333,12 +394,6 @@ imu_sensor_backend_t imu_sensor_active_backend(void)
 	return active_backend;
 }
 
-bool imu_sensor_active_quality_profile(
-	fusion_imu_quality_profile_t* profile_out)
-{
-	return imu_sensor_quality_profile_for_backend(active_backend, profile_out);
-}
-
 const char* imu_sensor_backend_name(imu_sensor_backend_t backend)
 {
 	switch(backend)

soc/software/gps_system/src/navigation_fusion.c

@@ -1,115 +0,0 @@
-#include "navigation_fusion.h"
-
-#include "fusion_bus.h"
-#include "fusion_gps_adapter.h"
-#include "imu_sensor.h"
-
-#include <math.h>
-#include <stddef.h>
-#include <string.h>
-
-static fusion_bus_t measurement_bus;
-static fusion_gps_adapter_t gps_adapter;
-static fusion_engine_t fusion_engine;
-static bool fusion_initialized;
-
-bool navigation_fusion_init(uint64_t local_timebase_hz)
-{
-	fusion_engine_config_t engine_config;
-	fusion_gps_adapter_config_t adapter_config;
-	if(local_timebase_hz == 0u)
-	{
-		return false;
-	}
-	fusion_bus_init(&measurement_bus);
-	fusion_engine_default_config(&engine_config, local_timebase_hz);
-	fusion_imu_quality_profile_t imu_profile;
-	if(imu_sensor_active_quality_profile(&imu_profile) &&
-	   !fusion_engine_apply_imu_quality(&engine_config, &imu_profile))
-	{
-		return false;
-	}
-	fusion_gps_adapter_default_config(&adapter_config);
-	if(!fusion_engine_init(&fusion_engine, &engine_config) ||
-	   !fusion_gps_adapter_init(&gps_adapter, &adapter_config))
-	{
-		return false;
-	}
-	fusion_initialized = true;
-	return true;
-}
-
-bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement)
-{
-	return fusion_initialized &&
-	       fusion_bus_publish_gps(&measurement_bus, measurement);
-}
-
-unsigned int navigation_fusion_service(void)
-{
-	if(!fusion_initialized)
-	{
-		return 0u;
-	}
-	unsigned int processed = 0u;
-	imu_sensor_service();
-	fusion_imu_measurement_t imu;
-	while(imu_sensor_take_measurement(&imu))
-	{
-		(void)fusion_engine_push_imu(&fusion_engine, &imu);
-		processed++;
-	}
-
-	/* GPS is intentionally consumed after all currently queued IMU samples.
-	 * A delayed WLS result is applied at its own timestamp by fixed-lag replay. */
-	fusion_gps_measurement_t gps;
-	while(fusion_bus_take_gps(&measurement_bus, &gps))
-	{
-		fusion_position_measurement_t position;
-		if(fusion_gps_adapter_convert(&gps_adapter, &gps, &position))
-		{
-			(void)fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
-		}
-		processed++;
-	}
-	return processed;
-}
-
-bool navigation_fusion_get_output(navigation_fusion_output_t* output)
-{
-	if((output == NULL) || !fusion_initialized ||
-	   (fusion_engine.state != FUSION_ENGINE_RUNNING))
-	{
-		return false;
-	}
-	memset(output, 0, sizeof(*output));
-	const fusion_runtime_t* runtime = &fusion_engine.current;
-	output->timestamp_ticks = runtime->filter_timestamp_ticks;
-	memcpy(output->position_enu_m, runtime->filter.position_m,
-	       sizeof(output->position_enu_m));
-	memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps,
-	       sizeof(output->velocity_enu_mps));
-	for(unsigned int axis = 0 ; axis < 3u ; axis++)
-	{
-		output->position_std_m[axis] =
-			sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
-	}
-	output->speed_mps = eskf_speed_mps(&runtime->filter);
-	output->last_position_nis = runtime->filter.last_position_nis;
-	output->motion_state = runtime->motion.state;
-	output->gps_accepted = fusion_engine.diagnostics.position_accepted;
-	output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
-	if(!fusion_gps_adapter_local_to_geodetic(
-		&gps_adapter, runtime->filter.position_m,
-		&output->latitude_deg, &output->longitude_deg, &output->height_m))
-	{
-		return false;
-	}
-	output->valid = true;
-	return true;
-}
-
-const fusion_engine_t* navigation_fusion_engine(void)
-{
-	return fusion_initialized ? &fusion_engine : NULL;
-}