73 lines
2.4 KiB
C
73 lines
2.4 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 "imu_sensor.h"
|
|
#include "navigation_fusion.h"
|
|
|
|
int main()
|
|
{
|
|
imu_sensor_config_t imu_config;
|
|
imu_sensor_default_config(&imu_config);
|
|
imu_config.odr_hz = IMU_ODR_HZ;
|
|
imu_config.accel_full_scale_g = IMU_ACCEL_FS_G;
|
|
imu_config.gyro_full_scale_dps = IMU_GYRO_FS_DPS;
|
|
bool imu_initialized = imu_sensor_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 && imu_sensor_start();
|
|
(void)imu_running;
|
|
imu_sensor_diagnostics_t imu_diagnostics;
|
|
imu_sensor_get_diagnostics(&imu_diagnostics);
|
|
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
|
|
"init imu=%u sensor=%s who=0x%02x running=%u odr_hz=%lu "
|
|
"poll=%u timebase_hz=%lu\r\n",
|
|
imu_initialized ? 1u : 0u,
|
|
imu_sensor_backend_name(imu_diagnostics.backend),
|
|
(unsigned int)imu_diagnostics.who_am_i,
|
|
imu_running ? 1u : 0u,
|
|
(unsigned long)imu_diagnostics.configured_odr_hz,
|
|
imu_diagnostics.foreground_polling ? 1u : 0u,
|
|
(unsigned long)BSP_CLINT_TIMEBASE_HZ);
|
|
|
|
acq_start_loop();
|
|
uint64_t last_fusion_status_tick = 0u;
|
|
|
|
while(1)
|
|
{
|
|
/* GPS and either IMU backend wake the shared machine interrupt path. */
|
|
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;
|
|
}
|