Add bare-metal TDK IMU ESKF fusion

soc/software/gps_system/gps_test.c

@@ -5,21 +5,54 @@
 #include <float.h>
 #include "matrixLib.h"
 #include "gps_macros.h"
+#include "navigation_fusion.h"
+#include "tdk_icm42688.h"
 
 int main()
 {
-		
-	gps_startup_initialization(15);
+	tdk_icm42688_config_t imu_config;
+	tdk_icm42688_default_config(&imu_config);
+	bool imu_initialized = tdk_icm42688_init(&imu_config);
+	bool fusion_initialized = navigation_fusion_init(BSP_CLINT_TIMEBASE_HZ);
+
+	gps_startup_initialization(GPS_NAV_SOLVE_TIMER_TICKS);
+	bool imu_running = imu_initialized && fusion_initialized && tdk_icm42688_start();
+	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+	        "init imu=%u running=%u odr_hz=%u timebase_hz=%lu\r\n",
+	        imu_initialized ? 1u : 0u, imu_running ? 1u : 0u,
+	        imu_config.odr_hz, (unsigned long)BSP_CLINT_TIMEBASE_HZ);
 
 	acq_start_loop();
+	uint64_t last_fusion_status_tick = 0u;
 
 	while(1)
 	{
 		asm volatile("wfi"); 
 		gps_background_task();
 		calculate_position();
-		// TODO: implement calculate position nav solve
-		//calculate_position_nav_solve();
+		if(fusion_initialized)
+		{
+			(void)navigation_fusion_service();
+			uint64_t now = gps_get_systick();
+			if((now - last_fusion_status_tick) >= GPS_SYSTICK_HZ)
+			{
+				navigation_fusion_output_t output;
+				if(navigation_fusion_get_output(&output))
+				{
+					GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+					        "state=%s lat=%.10lf lon=%.10lf height=%.3lf "
+					        "speed=%.3lf nis=%.3lf gps=%lu/%lu sigma=%.2lf,%.2lf,%.2lf\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,
+					        output.position_std_m[0], output.position_std_m[1],
+					        output.position_std_m[2]);
+				}
+				last_fusion_status_tick = now;
+			}
+		}
 	}
 
 	return 0;