136 lines
4.2 KiB
C
136 lines
4.2 KiB
C
#include <bsp/soc.h>
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#include "adapters/gps_fusion_publisher.h"
|
|
#include "adapters/gps_system_uart.h"
|
|
#include "adapters/imu_fusion_profile.h"
|
|
#include "fusion/navigation_fusion.h"
|
|
#include "gps/gps_receiver.h"
|
|
#include "gps_system.h"
|
|
#include "imu/imu_sensor.h"
|
|
|
|
#ifndef GPS_ESKF_ENABLE
|
|
#define GPS_ESKF_ENABLE 1
|
|
#endif
|
|
|
|
#if (GPS_ESKF_ENABLE != 0) && (GPS_ESKF_ENABLE != 1)
|
|
#error "GPS_ESKF_ENABLE must be 0 or 1"
|
|
#endif
|
|
|
|
static void initialize_sensor_fusion(gps_system_state_t* system)
|
|
{
|
|
system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
|
|
if(!system->fusion_enabled)
|
|
{
|
|
return;
|
|
}
|
|
|
|
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;
|
|
system->imu_initialized = imu_sensor_init(&imu_config);
|
|
|
|
fusion_imu_quality_profile_t imu_quality;
|
|
bool quality_available = system->imu_initialized &&
|
|
imu_fusion_quality_profile_for_backend(
|
|
imu_sensor_active_backend(), &imu_quality);
|
|
system->fusion_initialized = quality_available &&
|
|
navigation_fusion_init(
|
|
BSP_CLINT_TIMEBASE_HZ, &imu_quality,
|
|
true); /* Current prototype is the PX4-equivalent unarmed bench mode. */
|
|
}
|
|
|
|
static unsigned int feed_pending_imu_to_eskf(gps_system_state_t* system)
|
|
{
|
|
if(!system->fusion_initialized)
|
|
{
|
|
return 0u;
|
|
}
|
|
|
|
unsigned int processed = 0u;
|
|
imu_sensor_service();
|
|
while(imu_sensor_take_measurement(&system->imu_sample))
|
|
{
|
|
(void)navigation_fusion_push_imu(&system->imu_sample);
|
|
processed++;
|
|
}
|
|
return processed;
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
gps_system_state_t system = {0};
|
|
|
|
/* GPS and IMU are initialized independently. The application owns their
|
|
* interrupt connection and decides whether the ESKF path is enabled. */
|
|
initialize_sensor_fusion(&system);
|
|
gps_receiver_initialize(GPS_NAV_SOLVE_TIMER_TICKS);
|
|
bool imu_irq_attached = system.fusion_initialized &&
|
|
gps_receiver_attach_external_irq(BSP_PMOD_IRQ,
|
|
imu_sensor_irq_handler);
|
|
system.imu_running = imu_irq_attached && system.fusion_initialized &&
|
|
imu_sensor_start();
|
|
gps_receiver_start_acquisition();
|
|
gps_system_uart_publish_startup(
|
|
system.fusion_enabled, system.fusion_initialized,
|
|
system.imu_initialized, system.imu_running);
|
|
|
|
for(;;)
|
|
{
|
|
/* GPS, IMU data-ready and the maintenance timer wake this loop. */
|
|
asm volatile("wfi");
|
|
|
|
/* IMU prediction runs at the sensor's own timestamped rate. */
|
|
(void)feed_pending_imu_to_eskf(&system);
|
|
|
|
/* GPS remains an independent receiver: events first, then coherent WLS. */
|
|
gps_receiver_process_pending_events();
|
|
system.gps_epoch_ready = gps_receiver_solve_ready_epoch(
|
|
&system.gps_solution, &system.gps_measurement_ticks);
|
|
|
|
/* WLS may take long enough for more IMU samples to arrive. */
|
|
(void)feed_pending_imu_to_eskf(&system);
|
|
|
|
if(system.gps_epoch_ready && system.gps_solution.solution_valid)
|
|
{
|
|
/* Raw WLS latitude/longitude remains visible even with ESKF disabled. */
|
|
gps_system_uart_publish_location("gps",
|
|
system.gps_solution.latitude_deg,
|
|
system.gps_solution.longitude_deg,
|
|
system.gps_solution.height_m,
|
|
&system.gps_solution);
|
|
|
|
/* This is the only GPS -> sensor-fusion hand-off in the application. */
|
|
bool gps_measurement_ready = system.fusion_initialized &&
|
|
gps_fusion_build_measurement(
|
|
&system.gps_solution,
|
|
system.gps_measurement_ticks,
|
|
&system.gps_for_fusion);
|
|
if(gps_measurement_ready)
|
|
{
|
|
/* Delayed GPS update is applied at its capture timestamp; newer
|
|
* IMU history is replayed to produce the current fused state. */
|
|
(void)navigation_fusion_push_gps(&system.gps_for_fusion);
|
|
if(navigation_fusion_get_output(&system.fused_solution) &&
|
|
system.fused_solution.global_valid)
|
|
{
|
|
gps_system_uart_publish_location("imu_corrected",
|
|
system.fused_solution.latitude_deg,
|
|
system.fused_solution.longitude_deg,
|
|
system.fused_solution.height_m,
|
|
&system.gps_solution);
|
|
}
|
|
}
|
|
}
|
|
|
|
if(system.fusion_initialized)
|
|
{
|
|
gps_system_uart_publish_periodic_fusion_status(
|
|
&system.fused_solution, &system.last_status_tick);
|
|
}
|
|
}
|
|
}
|