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

Integrate asynchronous GPS IMU fusion

Browse Source
This commit is contained in:
onur
2026-07-16 23:49:43 +03:00
parent 38a96dd88f
commit 7cc3eb2a54
80 changed files with 3901 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/adapters/gps_fusion_publisher.c
+30
View File
@@ -0,0 +1,30 @@
#include "adapters/gps_fusion_publisher.h"
#include <stddef.h>
#include <string.h>
bool gps_fusion_build_measurement(
const gps_nav_solution_t* solution,
uint64_t measurement_timestamp_ticks,
fusion_gps_measurement_t* measurement_out)
{
if((solution == NULL) || (measurement_timestamp_ticks == 0u) ||
(measurement_out == NULL))
{
return false;
}
memset(measurement_out, 0, sizeof(*measurement_out));
measurement_out->timestamp_ticks = measurement_timestamp_ticks;
measurement_out->latitude_deg = solution->latitude_deg;
measurement_out->longitude_deg = solution->longitude_deg;
measurement_out->height_m = solution->height_m;
memcpy(measurement_out->covariance_enu_m2, solution->covariance_enu_m2,
sizeof(measurement_out->covariance_enu_m2));
measurement_out->hacc_m = solution->hacc_m;
measurement_out->vacc_m = solution->vacc_m;
measurement_out->residual_rms_m = solution->residual_rms_m;
measurement_out->chi_square_ratio = solution->chi_square_ratio;
measurement_out->used_satellites = solution->used_satellite_count;
measurement_out->solution_valid = solution->solution_valid;
return true;
}
soc/software/gps_system/src/adapters/gps_system_uart.c
+112
View File
@@ -0,0 +1,112 @@
#include "adapters/gps_system_uart.h"
#include <bsp/soc.h>
#include <stddef.h>
#include "gps/gps_macros.h"
#include "gps/gps_receiver.h"
#include "imu/imu_sensor.h"
void gps_system_uart_publish_location(
const char* source,
double latitude_deg,
double longitude_deg,
double height_m,
const gps_nav_solution_t* gps)
{
if((source == NULL) || (gps == NULL))
{
return;
}
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
"source=%s lat=%.20lf lon=%.20lf height=%.3lf "
"gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf "
"tracked=%u used=%u\r\n",
source, latitude_deg, longitude_deg, height_m,
gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop,
gps->tracked_satellite_count, gps->used_satellite_count);
}
void gps_system_uart_publish_startup(
bool fusion_enabled,
bool fusion_initialized,
bool imu_initialized,
bool imu_running)
{
imu_sensor_diagnostics_t imu;
imu_sensor_get_diagnostics(&imu);
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
"init eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
"running=%u target_hz=%lu actual_hz=%lu nav_ms=%u timebase_hz=%lu\r\n",
fusion_enabled ? 1u : 0u,
fusion_initialized ? 1u : 0u,
imu_initialized ? 1u : 0u,
imu_sensor_backend_name(imu.backend),
(unsigned int)imu.who_am_i,
imu_running ? 1u : 0u,
(unsigned long)imu.requested_odr_hz,
(unsigned long)imu.configured_odr_hz,
GPS_NAV_SOLVE_PERIOD_MS,
(unsigned long)BSP_CLINT_TIMEBASE_HZ);
}
void gps_system_uart_publish_periodic_fusion_status(
navigation_fusion_output_t* output,
uint64_t* last_status_tick)
{
if((output == NULL) || (last_status_tick == NULL))
{
return;
}
uint64_t now = gps_receiver_status_ticks();
if((now - *last_status_tick) < gps_receiver_status_ticks_per_second())
{
return;
}
*last_status_tick = now;
imu_sensor_diagnostics_t imu;
imu_sensor_get_diagnostics(&imu);
if(!navigation_fusion_get_output(output))
{
return;
}
if(output->global_valid)
{
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
"state=%s lat=%.10lf lon=%.10lf height=%.3lf "
"speed=%.3lf nis=%.3lf gps=%lu/%lu recoveries=%lu "
"zupt=%lu replay=%lu "
"sigma=%.2lf,%.2lf,%.2lf imu=%lu fused_imu=%lu "
"overflow=%lu missed=%lu\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,
(unsigned long)output->gps_position_recoveries,
(unsigned long)output->zero_update_count,
(unsigned long)output->replay_count,
output->position_std_m[0], output->position_std_m[1],
output->position_std_m[2],
(unsigned long)imu.sample_count,
(unsigned long)output->imu_processed_count,
(unsigned long)imu.queue_overflow_count,
(unsigned long)imu.estimated_missed_sample_count);
return;
}
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
"state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
"imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
navigation_fusion_state_name(output->engine_state),
output->calibration_sample_count,
(unsigned long)output->gps_accepted,
(unsigned long)output->gps_nis_rejected,
(unsigned long)output->gps_position_recoveries,
(unsigned long)imu.sample_count,
(unsigned long)output->imu_processed_count,
(unsigned long)imu.queue_overflow_count,
(unsigned long)imu.estimated_missed_sample_count);
}
soc/software/gps_system/src/adapters/imu_fusion_profile.c
+74
View File
@@ -0,0 +1,74 @@
#include "adapters/imu_fusion_profile.h"
#include <stddef.h>
#define PROFILE_GRAVITY_MPS2 9.80665
#define PROFILE_DEGREES_TO_RADIANS 0.017453292519943295769236907684886
static void set_all(double values[3], double value)
{
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
values[axis] = value;
}
}
bool imu_fusion_quality_profile_for_backend(
imu_sensor_backend_t backend,
fusion_imu_quality_profile_t* profile_out)
{
if(profile_out == NULL)
{
return false;
}
fusion_imu_quality_default(profile_out);
switch(backend)
{
case IMU_SENSOR_TDK_ICM42688:
profile_out->name = "tdk-icm42688";
/* ICM-42688-P: 65/65/70 ug/sqrt(Hz), 2.8 mdps/sqrt(Hz). */
profile_out->nominal_accel_noise_density_mps2_sqrt_hz[0] =
65.0e-6 * PROFILE_GRAVITY_MPS2;
profile_out->nominal_accel_noise_density_mps2_sqrt_hz[1] =
65.0e-6 * PROFILE_GRAVITY_MPS2;
profile_out->nominal_accel_noise_density_mps2_sqrt_hz[2] =
70.0e-6 * PROFILE_GRAVITY_MPS2;
set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz,
2.8e-3 * PROFILE_DEGREES_TO_RADIANS);
/* Conservative fusion values; tune bias RW with long Allan logs. */
set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.025);
set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0022);
set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0015);
set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s,
0.002 * PROFILE_DEGREES_TO_RADIANS);
profile_out->calibration_accel_std_limit_mps2 = 0.09;
profile_out->calibration_gyro_std_limit_rps =
0.10 * PROFILE_DEGREES_TO_RADIANS;
set_all(profile_out->zero_gyro_measurement_std_rps,
0.02 * PROFILE_DEGREES_TO_RADIANS);
break;
case IMU_SENSOR_ST_LSM6DSOX:
profile_out->name = "st-lsm6dsox";
/* LSM6DSOX at +-2 g: 70 ug/sqrt(Hz), 3.8 mdps/sqrt(Hz). */
set_all(profile_out->nominal_accel_noise_density_mps2_sqrt_hz,
70.0e-6 * PROFILE_GRAVITY_MPS2);
set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz,
3.8e-3 * PROFILE_DEGREES_TO_RADIANS);
set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.03);
set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0030);
set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0025);
set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s,
0.004 * PROFILE_DEGREES_TO_RADIANS);
profile_out->calibration_accel_std_limit_mps2 = 0.12;
profile_out->calibration_gyro_std_limit_rps =
0.20 * PROFILE_DEGREES_TO_RADIANS;
set_all(profile_out->zero_gyro_measurement_std_rps,
0.04 * PROFILE_DEGREES_TO_RADIANS);
break;
default:
return false;
}
return fusion_imu_quality_is_valid(profile_out);
}