60 lines
1.8 KiB
C
60 lines
1.8 KiB
C
#include <stdio.h>
|
|
#include "gps_ip_defines.h"
|
|
#include "linked_list.h"
|
|
#include <stddef.h>
|
|
#include <float.h>
|
|
#include "matrixLib.h"
|
|
#include "gps_macros.h"
|
|
#include "navigation_fusion.h"
|
|
#include "tdk_icm42688.h"
|
|
|
|
int main()
|
|
{
|
|
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();
|
|
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;
|
|
}
|