onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Keep fused location streaming through GPS rejects

Browse Source
This commit is contained in:
onur
2026-07-23 23:20:23 +03:00
parent fc242b2883
commit 9ff895600b
14 changed files with 173 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/gps_system.c
+39 -19
View File
@@ -18,6 +18,9 @@
#error "GPS_ESKF_ENABLE must be 0 or 1"
#endif
#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
#define IMU_SAMPLES_AFTER_GPS_SERVICE 3u
static void initialize_sensor_fusion(gps_system_state_t* system)
{
system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
@@ -43,18 +46,32 @@ static void initialize_sensor_fusion(gps_system_state_t* system)
true); /* Current prototype is the PX4-equivalent unarmed bench mode. */
}
static unsigned int feed_pending_imu_to_eskf(gps_system_state_t* system)
static unsigned int feed_pending_imu_to_eskf(
gps_system_state_t* system,
unsigned int max_samples)
{
if(!system->fusion_initialized)
if(!system->fusion_initialized || (max_samples == 0u))
{
return 0u;
}
unsigned int processed = 0u;
imu_sensor_service();
while(imu_sensor_take_measurement(&system->imu_sample))
while((processed < max_samples) &&
imu_sensor_take_measurement(&system->imu_sample))
{
(void)navigation_fusion_push_imu(&system->imu_sample);
bool accepted = navigation_fusion_push_imu(&system->imu_sample);
if(accepted &&
navigation_fusion_get_output(&system->fused_solution) &&
system->fused_solution.global_valid &&
(system->fused_solution.timestamp_ticks !=
system->last_fused_location_tick))
{
gps_system_uart_publish_fused_location(
&system->fused_solution);
system->last_fused_location_tick =
system->fused_solution.timestamp_ticks;
}
processed++;
}
return processed;
@@ -78,21 +95,33 @@ int main(void)
system.fusion_enabled, system.fusion_initialized,
system.imu_initialized, system.imu_running);
bool imu_backlog_may_remain = false;
for(;;)
{
/* GPS, IMU data-ready and the maintenance timer wake this loop. */
asm volatile("wfi");
/* Skip WFI only while draining a bounded IMU backlog. Every batch
* still services GPS before more prediction work, so a continuously
* non-empty IMU queue cannot starve navigation epochs. */
if(!imu_backlog_may_remain)
{
/* 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);
/* Limit pre-GPS work to one sample for low event latency. */
(void)feed_pending_imu_to_eskf(
&system, IMU_SAMPLES_BEFORE_GPS_SERVICE);
/* 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);
/* WLS may take long enough for more IMU samples to arrive. Drain a
* small batch, then return to GPS service before continuing. */
unsigned int imu_after_gps = feed_pending_imu_to_eskf(
&system, IMU_SAMPLES_AFTER_GPS_SERVICE);
imu_backlog_may_remain =
(imu_after_gps == IMU_SAMPLES_AFTER_GPS_SERVICE);
if(system.gps_epoch_ready && system.gps_solution.solution_valid)
{
@@ -114,15 +143,6 @@ int main(void)
/* 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);
}
}
}