Integrate asynchronous GPS IMU fusion

2026-07-16 23:49:43 +03:00
parent 38a96dd88f
commit 7cc3eb2a54
80 changed files with 3901 additions and 553 deletions
@@ -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);
+}