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

Add generic IMU fusion and PMOD IRQ verification

Browse Source
This commit is contained in:
onur
2026-07-14 11:03:13 +03:00
parent e77651dac3
commit dd6569c025
35 changed files with 2680 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/fusion_engine.c
+86
View File
@@ -48,6 +48,64 @@ static uint64_t seconds_to_ticks(const fusion_engine_t* engine, double seconds)
return (uint64_t)(ticks + 0.5);
}
void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile)
{
if(profile == NULL)
{
return;
}
memset(profile, 0, sizeof(*profile));
profile->name = "generic-conservative";
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
/* Nominal fields are informational for the generic fallback. */
profile->nominal_accel_noise_density_mps2_sqrt_hz[axis] = 0.001;
profile->nominal_gyro_noise_density_rps_sqrt_hz[axis] = 0.0001;
profile->process_accel_noise_density_mps2_sqrt_hz[axis] = 0.03;
profile->process_gyro_noise_density_rps_sqrt_hz[axis] = 0.003;
profile->accel_bias_random_walk_mps2_sqrt_s[axis] = 0.002;
profile->gyro_bias_random_walk_rps_sqrt_s[axis] =
0.002 * FUSION_PI / 180.0;
profile->zero_gyro_measurement_std_rps[axis] =
0.01 * FUSION_PI / 180.0;
}
profile->calibration_accel_std_limit_mps2 = 0.12;
profile->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
}
bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile)
{
if((profile == NULL) || (profile->name == NULL) ||
!isfinite(profile->calibration_accel_std_limit_mps2) ||
!(profile->calibration_accel_std_limit_mps2 > 0.0) ||
!isfinite(profile->calibration_gyro_std_limit_rps) ||
!(profile->calibration_gyro_std_limit_rps > 0.0))
{
return false;
}
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
const double values[] = {
profile->nominal_accel_noise_density_mps2_sqrt_hz[axis],
profile->nominal_gyro_noise_density_rps_sqrt_hz[axis],
profile->process_accel_noise_density_mps2_sqrt_hz[axis],
profile->process_gyro_noise_density_rps_sqrt_hz[axis],
profile->accel_bias_random_walk_mps2_sqrt_s[axis],
profile->gyro_bias_random_walk_rps_sqrt_s[axis],
profile->zero_gyro_measurement_std_rps[axis]
};
for(unsigned int value = 0u ; value < sizeof(values) / sizeof(values[0]);
value++)
{
if(!isfinite(values[value]) || !(values[value] > 0.0))
{
return false;
}
}
}
return true;
}
void fusion_engine_default_config(fusion_engine_config_t* config,
uint64_t timebase_hz)
{
@@ -77,6 +135,34 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
config->gnss_still_confirmations = 2u;
}
bool fusion_engine_apply_imu_quality(
fusion_engine_config_t* config,
const fusion_imu_quality_profile_t* profile)
{
if((config == NULL) || !fusion_imu_quality_is_valid(profile))
{
return false;
}
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
config->eskf.accel_noise_density[axis] =
profile->process_accel_noise_density_mps2_sqrt_hz[axis];
config->eskf.gyro_noise_density[axis] =
profile->process_gyro_noise_density_rps_sqrt_hz[axis];
config->eskf.accel_bias_random_walk[axis] =
profile->accel_bias_random_walk_mps2_sqrt_s[axis];
config->eskf.gyro_bias_random_walk[axis] =
profile->gyro_bias_random_walk_rps_sqrt_s[axis];
config->zero_gyro_std_rps[axis] =
profile->zero_gyro_measurement_std_rps[axis];
}
config->calibration_accel_std_limit_mps2 =
profile->calibration_accel_std_limit_mps2;
config->calibration_gyro_std_limit_rps =
profile->calibration_gyro_std_limit_rps;
return true;
}
static bool config_valid(const fusion_engine_config_t* config)
{
return (config != NULL) && (config->timebase_hz != 0u) &&
soc/software/gps_system/src/gps_interrupt_handlers.c
+2 -2
View File
@@ -3,7 +3,7 @@
#include "Navigation.h"
#include "gps_pps.h"
#include "gps_time_anchor.h"
#include "tdk_icm42688.h"
#include "imu_sensor.h"
#include <bsp/irq.h>
#include <bsp/mmio.h>
#include <bsp/plic.h>
@@ -397,7 +397,7 @@ void gps_dispatch_irq(unsigned int irq_id)
{
if(irq_id == BSP_PMOD_IRQ)
{
tdk_icm42688_irq_handler();
imu_sensor_irq_handler();
return;
}
soc/software/gps_system/src/imu_calibration.c
+113
View File
@@ -0,0 +1,113 @@
#include "imu_calibration.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
static bool vector_is_finite(const double vector[3])
{
return isfinite(vector[0]) && isfinite(vector[1]) && isfinite(vector[2]);
}
static bool matrix_is_finite_and_invertible(const double matrix[3][3])
{
for(unsigned int row = 0 ; row < 3u ; row++)
{
if(!vector_is_finite(matrix[row]))
{
return false;
}
}
double determinant =
matrix[0][0] * (matrix[1][1] * matrix[2][2] -
matrix[1][2] * matrix[2][1]) -
matrix[0][1] * (matrix[1][0] * matrix[2][2] -
matrix[1][2] * matrix[2][0]) +
matrix[0][2] * (matrix[1][0] * matrix[2][1] -
matrix[1][1] * matrix[2][0]);
return isfinite(determinant) && (fabs(determinant) > 1.0e-12);
}
void imu_calibration_default(imu_calibration_t* calibration)
{
if(calibration == NULL)
{
return;
}
memset(calibration, 0, sizeof(*calibration));
for(unsigned int axis = 0 ; axis < 3u ; axis++)
{
calibration->accel_transform[axis][axis] = 1.0;
calibration->gyro_transform[axis][axis] = 1.0;
}
calibration->reference_temperature_c = 25.0;
}
bool imu_calibration_is_valid(const imu_calibration_t* calibration)
{
if((calibration == NULL) ||
!vector_is_finite(calibration->accel_bias_mps2) ||
!vector_is_finite(calibration->gyro_bias_rps) ||
!matrix_is_finite_and_invertible(calibration->accel_transform) ||
!matrix_is_finite_and_invertible(calibration->gyro_transform) ||
!isfinite(calibration->reference_temperature_c) ||
!vector_is_finite(calibration->accel_temperature_coefficient_mps2_per_c) ||
!vector_is_finite(calibration->gyro_temperature_coefficient_rps_per_c))
{
return false;
}
return true;
}
bool imu_calibration_apply(const imu_calibration_t* calibration,
fusion_imu_measurement_t* measurement)
{
if(!imu_calibration_is_valid(calibration) || (measurement == NULL) ||
!vector_is_finite(measurement->accel_mps2) ||
!vector_is_finite(measurement->gyro_rps))
{
return false;
}
double temperature_delta_c = 0.0;
if(calibration->temperature_compensation_enabled)
{
if(!isfinite(measurement->temperature_c))
{
return false;
}
temperature_delta_c =
measurement->temperature_c - calibration->reference_temperature_c;
}
double accel_unbiased[3];
double gyro_unbiased[3];
for(unsigned int axis = 0 ; axis < 3u ; axis++)
{
accel_unbiased[axis] =
measurement->accel_mps2[axis] - calibration->accel_bias_mps2[axis] -
calibration->accel_temperature_coefficient_mps2_per_c[axis] *
temperature_delta_c;
gyro_unbiased[axis] =
measurement->gyro_rps[axis] - calibration->gyro_bias_rps[axis] -
calibration->gyro_temperature_coefficient_rps_per_c[axis] *
temperature_delta_c;
}
double accel_corrected[3] = {0.0};
double gyro_corrected[3] = {0.0};
for(unsigned int row = 0 ; row < 3u ; row++)
{
for(unsigned int col = 0 ; col < 3u ; col++)
{
accel_corrected[row] +=
calibration->accel_transform[row][col] * accel_unbiased[col];
gyro_corrected[row] +=
calibration->gyro_transform[row][col] * gyro_unbiased[col];
}
}
memcpy(measurement->accel_mps2, accel_corrected,
sizeof(measurement->accel_mps2));
memcpy(measurement->gyro_rps, gyro_corrected,
sizeof(measurement->gyro_rps));
return true;
}
soc/software/gps_system/src/imu_sensor.c
+350
View File
@@ -0,0 +1,350 @@
#include "imu_sensor.h"
#include <stddef.h>
#include <string.h>
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
#include "tdk_icm42688.h"
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
#include "st_lsm6dsox.h"
#endif
#if !defined(IMU_SENSOR_BUILD_AUTO) && !defined(IMU_SENSOR_BUILD_TDK) && \
!defined(IMU_SENSOR_BUILD_ST)
#error "Select IMU_SENSOR_BUILD_AUTO, IMU_SENSOR_BUILD_TDK or IMU_SENSOR_BUILD_ST"
#endif
static imu_sensor_backend_t active_backend;
static imu_calibration_t active_calibration;
static uint32_t active_odr_hz;
static uint64_t calibration_rejected_count;
void imu_sensor_default_config(imu_sensor_config_t* config)
{
if(config == NULL)
{
return;
}
memset(config, 0, sizeof(*config));
for(unsigned int backend = 0u ; backend < IMU_SENSOR_BACKEND_COUNT ; backend++)
{
imu_calibration_default(&config->calibration_by_backend[backend]);
}
}
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
static bool tdk_accel_range(unsigned int full_scale_g,
tdk_accel_full_scale_t* range)
{
if(range == NULL)
{
return false;
}
switch(full_scale_g)
{
case 2u: *range = TDK_ACCEL_FS_2G; return true;
case 4u: *range = TDK_ACCEL_FS_4G; return true;
case 8u: *range = TDK_ACCEL_FS_8G; return true;
case 16u: *range = TDK_ACCEL_FS_16G; return true;
default: return false;
}
}
static bool tdk_gyro_range(unsigned int full_scale_dps,
tdk_gyro_full_scale_mdps_t* range)
{
if(range == NULL)
{
return false;
}
switch(full_scale_dps)
{
case 125u: *range = TDK_GYRO_FS_125; return true;
case 250u: *range = TDK_GYRO_FS_250; return true;
case 500u: *range = TDK_GYRO_FS_500; return true;
case 1000u: *range = TDK_GYRO_FS_1000; return true;
case 2000u: *range = TDK_GYRO_FS_2000; return true;
default: return false;
}
}
static bool try_tdk(const imu_sensor_config_t* config)
{
if(!imu_calibration_is_valid(
&config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688]))
{
return false;
}
tdk_icm42688_config_t backend_config;
tdk_icm42688_default_config(&backend_config);
if(config->odr_hz != 0u)
{
backend_config.odr_hz = config->odr_hz;
}
if((config->accel_full_scale_g != 0u) &&
!tdk_accel_range(config->accel_full_scale_g,
&backend_config.accel_full_scale_g))
{
return false;
}
if((config->gyro_full_scale_dps != 0u) &&
!tdk_gyro_range(config->gyro_full_scale_dps,
&backend_config.gyro_full_scale_mdps))
{
return false;
}
if(!tdk_icm42688_init(&backend_config))
{
return false;
}
active_backend = IMU_SENSOR_TDK_ICM42688;
active_calibration =
config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688];
active_odr_hz = backend_config.odr_hz;
return true;
}
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
static bool try_st(const imu_sensor_config_t* config)
{
if(!imu_calibration_is_valid(
&config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX]))
{
return false;
}
st_lsm6dsox_config_t backend_config;
st_lsm6dsox_default_config(&backend_config);
if(config->odr_hz != 0u)
{
backend_config.odr_hz = config->odr_hz;
}
if(config->accel_full_scale_g != 0u)
{
backend_config.accel_full_scale_g = config->accel_full_scale_g;
}
if(config->gyro_full_scale_dps != 0u)
{
backend_config.gyro_full_scale_dps = config->gyro_full_scale_dps;
}
if(!st_lsm6dsox_init(&backend_config))
{
return false;
}
active_backend = IMU_SENSOR_ST_LSM6DSOX;
active_calibration =
config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX];
active_odr_hz = backend_config.odr_hz;
return true;
}
#endif
bool imu_sensor_init(const imu_sensor_config_t* config)
{
active_backend = IMU_SENSOR_NONE;
active_odr_hz = 0u;
calibration_rejected_count = 0u;
if(config == NULL)
{
return false;
}
#if defined(IMU_SENSOR_BUILD_TDK)
return try_tdk(config);
#elif defined(IMU_SENSOR_BUILD_ST)
return try_st(config);
#else
/* WHO_AM_I probing is read-only until a matching backend is found. */
if(try_tdk(config))
{
return true;
}
return try_st(config);
#endif
}
bool imu_sensor_start(void)
{
switch(active_backend)
{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
case IMU_SENSOR_TDK_ICM42688:
return tdk_icm42688_start();
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
case IMU_SENSOR_ST_LSM6DSOX:
return st_lsm6dsox_start();
#endif
default:
return false;
}
}
void imu_sensor_stop(void)
{
switch(active_backend)
{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
case IMU_SENSOR_TDK_ICM42688:
tdk_icm42688_stop();
break;
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
case IMU_SENSOR_ST_LSM6DSOX:
st_lsm6dsox_stop();
break;
#endif
default:
break;
}
}
void imu_sensor_irq_handler(void)
{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
if(active_backend == IMU_SENSOR_TDK_ICM42688)
{
tdk_icm42688_irq_handler();
}
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
if(active_backend == IMU_SENSOR_ST_LSM6DSOX)
{
st_lsm6dsox_irq_handler();
}
#endif
}
void imu_sensor_service(void)
{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
if(active_backend == IMU_SENSOR_ST_LSM6DSOX)
{
st_lsm6dsox_service();
}
#endif
}
bool imu_sensor_requires_foreground_service(void)
{
return false;
}
bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out)
{
if(measurement_out == NULL)
{
return false;
}
bool have_measurement = false;
switch(active_backend)
{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
case IMU_SENSOR_TDK_ICM42688:
have_measurement =
tdk_icm42688_take_measurement(measurement_out);
break;
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
case IMU_SENSOR_ST_LSM6DSOX:
have_measurement =
st_lsm6dsox_take_measurement(measurement_out);
break;
#endif
default:
break;
}
if(!have_measurement)
{
return false;
}
if(!imu_calibration_apply(&active_calibration, measurement_out))
{
calibration_rejected_count++;
return false;
}
return true;
}
void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out)
{
if(diagnostics_out == NULL)
{
return;
}
memset(diagnostics_out, 0, sizeof(*diagnostics_out));
diagnostics_out->backend = active_backend;
diagnostics_out->configured_odr_hz = active_odr_hz;
diagnostics_out->foreground_polling =
imu_sensor_requires_foreground_service();
switch(active_backend)
{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
case IMU_SENSOR_TDK_ICM42688:
{
tdk_icm42688_diagnostics_t source;
tdk_icm42688_get_diagnostics(&source);
diagnostics_out->irq_count = source.irq_count;
diagnostics_out->sample_count = source.sample_count;
diagnostics_out->invalid_sample_count =
source.invalid_sample_count + calibration_rejected_count;
diagnostics_out->spurious_irq_count = source.spurious_irq_count;
diagnostics_out->queue_overflow_count = source.queue_overflow_count;
diagnostics_out->estimated_missed_sample_count =
source.estimated_missed_sample_count;
diagnostics_out->last_timestamp_ticks = source.last_timestamp_ticks;
diagnostics_out->who_am_i = source.who_am_i;
diagnostics_out->initialized = source.initialized;
diagnostics_out->running = source.running;
break;
}
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
case IMU_SENSOR_ST_LSM6DSOX:
{
st_lsm6dsox_diagnostics_t source;
st_lsm6dsox_get_diagnostics(&source);
diagnostics_out->irq_count = source.irq_count;
diagnostics_out->sample_count = source.sample_count;
diagnostics_out->invalid_sample_count =
source.invalid_sample_count + calibration_rejected_count;
diagnostics_out->spurious_irq_count = source.spurious_irq_count;
diagnostics_out->queue_overflow_count = source.queue_overflow_count;
diagnostics_out->estimated_missed_sample_count =
source.estimated_missed_sample_count;
diagnostics_out->last_timestamp_ticks = source.last_timestamp_ticks;
diagnostics_out->who_am_i = source.who_am_i;
diagnostics_out->initialized = source.initialized;
diagnostics_out->running = source.running;
break;
}
#endif
default:
diagnostics_out->invalid_sample_count = calibration_rejected_count;
break;
}
}
imu_sensor_backend_t imu_sensor_active_backend(void)
{
return active_backend;
}
bool imu_sensor_active_quality_profile(
fusion_imu_quality_profile_t* profile_out)
{
return imu_sensor_quality_profile_for_backend(active_backend, profile_out);
}
const char* imu_sensor_backend_name(imu_sensor_backend_t backend)
{
switch(backend)
{
case IMU_SENSOR_TDK_ICM42688: return "tdk-icm42688";
case IMU_SENSOR_ST_LSM6DSOX: return "st-lsm6dsox";
default: return "none";
}
}
soc/software/gps_system/src/imu_sensor_profile.c
+74
View File
@@ -0,0 +1,74 @@
#include "imu_sensor.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_sensor_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);
}
soc/software/gps_system/src/navigation_fusion.c
+9 -2
View File
@@ -2,7 +2,7 @@
#include "fusion_bus.h"
#include "fusion_gps_adapter.h"
#include "tdk_icm42688.h"
#include "imu_sensor.h"
#include <math.h>
#include <stddef.h>
@@ -23,6 +23,12 @@ bool navigation_fusion_init(uint64_t local_timebase_hz)
}
fusion_bus_init(&measurement_bus);
fusion_engine_default_config(&engine_config, local_timebase_hz);
fusion_imu_quality_profile_t imu_profile;
if(imu_sensor_active_quality_profile(&imu_profile) &&
!fusion_engine_apply_imu_quality(&engine_config, &imu_profile))
{
return false;
}
fusion_gps_adapter_default_config(&adapter_config);
if(!fusion_engine_init(&fusion_engine, &engine_config) ||
!fusion_gps_adapter_init(&gps_adapter, &adapter_config))
@@ -46,8 +52,9 @@ unsigned int navigation_fusion_service(void)
return 0u;
}
unsigned int processed = 0u;
imu_sensor_service();
fusion_imu_measurement_t imu;
while(tdk_icm42688_take_measurement(&imu))
while(imu_sensor_take_measurement(&imu))
{
(void)fusion_engine_push_imu(&fusion_engine, &imu);
processed++;
soc/software/gps_system/src/st_lsm6dsox.c
+503
View File
@@ -0,0 +1,503 @@
#include "st_lsm6dsox.h"
#include <bsp/mmio.h>
#include <bsp/plic.h>
#include <bsp/pmod_spi.h>
#include <bsp/soc.h>
#include <limits.h>
#include <math.h>
#include <stddef.h>
#include <string.h>
#define LSM6DSOX_WHO_AM_I 0x0fu
#define LSM6DSOX_COUNTER_BDR_REG1 0x0bu
#define LSM6DSOX_INT1_CTRL 0x0du
#define LSM6DSOX_INT2_CTRL 0x0eu
#define LSM6DSOX_CTRL1_XL 0x10u
#define LSM6DSOX_CTRL2_G 0x11u
#define LSM6DSOX_CTRL3_C 0x12u
#define LSM6DSOX_STATUS_REG 0x1eu
#define LSM6DSOX_TAP_CFG2 0x58u
#define LSM6DSOX_OUT_TEMP_L 0x20u
#define LSM6DSOX_OUTX_L_G 0x22u
#define LSM6DSOX_OUTX_L_A 0x28u
#define LSM6DSOX_SPI_READ 0x80u
#define LSM6DSOX_SPI_DUMMY 0xffu
#define LSM6DSOX_WHO_AM_I_VALUE 0x6cu
#define LSM6DSOX_CTRL3_SW_RESET 0x01u
#define LSM6DSOX_CTRL3_IF_INC 0x04u
#define LSM6DSOX_CTRL3_BDU 0x40u
#define LSM6DSOX_STATUS_XLDA 0x01u
#define LSM6DSOX_STATUS_GDA 0x02u
#define LSM6DSOX_INT1_DRDY_XL 0x01u
#define LSM6DSOX_DATAREADY_PULSED 0x80u
#define LSM6DSOX_INTERRUPTS_ENABLE 0x80u
#define LSM6DSOX_GYRO_READY_TIMEOUT_US 100u
#define ST_GRAVITY_MPS2 9.80665
#define ST_DEGREES_TO_RADIANS 0.017453292519943295769236907684886
#if (ST_IMU_RAW_QUEUE_CAPACITY < 2u)
#error "ST_IMU_RAW_QUEUE_CAPACITY must be at least 2"
#endif
static st_lsm6dsox_config_t active_config;
static st_lsm6dsox_raw_sample_t sample_queue[ST_IMU_RAW_QUEUE_CAPACITY];
static volatile unsigned int sample_head;
static volatile unsigned int sample_tail;
static volatile st_lsm6dsox_diagnostics_t diagnostics;
static uint64_t time_now(void)
{
return bsp_read64(BSP_CLINT_MTIME);
}
static void delay_ms(uint32_t milliseconds)
{
uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * milliseconds + 999u) /
1000u;
uint64_t start = time_now();
while((time_now() - start) < ticks)
{
}
}
static void write_register(uint8_t reg, uint8_t value)
{
bsp_pmod_spi_write_reg8(reg & (uint8_t)~LSM6DSOX_SPI_READ, value);
}
static uint8_t read_register(uint8_t reg)
{
return bsp_pmod_spi_read_reg8(reg | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY);
}
static int16_t little_endian_i16(const uint8_t* bytes)
{
return (int16_t)((uint16_t)bytes[0] | ((uint16_t)bytes[1] << 8));
}
static bool odr_register_value(uint32_t odr_hz, uint8_t* value)
{
if(value == NULL)
{
return false;
}
switch(odr_hz)
{
case 52u: *value = 0x30u; return true;
case 104u: *value = 0x40u; return true;
case 208u: *value = 0x50u; return true;
default: return false;
}
}
static bool accel_full_scale_bits(unsigned int full_scale_g, uint8_t* bits,
double* mg_per_lsb)
{
if((bits == NULL) || (mg_per_lsb == NULL))
{
return false;
}
switch(full_scale_g)
{
case 2u: *bits = 0x00u; *mg_per_lsb = 0.061; return true;
case 4u: *bits = 0x08u; *mg_per_lsb = 0.122; return true;
case 8u: *bits = 0x0cu; *mg_per_lsb = 0.244; return true;
case 16u: *bits = 0x04u; *mg_per_lsb = 0.488; return true;
default: return false;
}
}
static bool gyro_full_scale_bits(unsigned int full_scale_dps, uint8_t* bits,
double* mdps_per_lsb)
{
if((bits == NULL) || (mdps_per_lsb == NULL))
{
return false;
}
switch(full_scale_dps)
{
case 125u: *bits = 0x02u; *mdps_per_lsb = 4.375; return true;
case 250u: *bits = 0x00u; *mdps_per_lsb = 8.750; return true;
case 500u: *bits = 0x04u; *mdps_per_lsb = 17.50; return true;
case 1000u: *bits = 0x08u; *mdps_per_lsb = 35.00; return true;
case 2000u: *bits = 0x0cu; *mdps_per_lsb = 70.00; return true;
default: return false;
}
}
static unsigned int next_queue_index(unsigned int index)
{
index++;
return (index == ST_IMU_RAW_QUEUE_CAPACITY) ? 0u : index;
}
static bool raw_sample_valid(const st_lsm6dsox_raw_sample_t* sample)
{
if((sample == NULL) || (sample->temperature_raw == INT16_MIN))
{
return false;
}
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
if((sample->accel_raw[axis] == INT16_MIN) ||
(sample->gyro_raw[axis] == INT16_MIN))
{
return false;
}
}
return true;
}
static bool queue_push_isr(const st_lsm6dsox_raw_sample_t* sample)
{
unsigned int head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED);
unsigned int next = next_queue_index(head);
if(next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE))
{
__atomic_fetch_add(&diagnostics.queue_overflow_count, 1u,
__ATOMIC_RELAXED);
return false;
}
sample_queue[head] = *sample;
__atomic_store_n(&sample_head, next, __ATOMIC_RELEASE);
return true;
}
static bool queue_pop(st_lsm6dsox_raw_sample_t* sample)
{
unsigned int tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED);
if(tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE))
{
return false;
}
*sample = sample_queue[tail];
__atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE);
return true;
}
static void read_raw_outputs(st_lsm6dsox_raw_sample_t* sample,
uint64_t timestamp_ticks)
{
uint8_t bytes[14];
bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, bytes, 2u);
/* Temperature and gyro/accel are separate internal output blocks. */
bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, &bytes[2], 12u);
sample->timestamp_ticks = timestamp_ticks;
sample->temperature_raw = little_endian_i16(&bytes[0]);
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
sample->gyro_raw[axis] = little_endian_i16(&bytes[2u + 2u * axis]);
sample->accel_raw[axis] = little_endian_i16(&bytes[8u + 2u * axis]);
}
}
static void read_raw_accel(st_lsm6dsox_raw_sample_t* sample)
{
uint8_t bytes[6];
bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_A | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, bytes, sizeof(bytes));
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
sample->accel_raw[axis] = little_endian_i16(&bytes[2u * axis]);
}
}
static void read_raw_gyro_temperature(st_lsm6dsox_raw_sample_t* sample)
{
uint8_t bytes[8];
bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, bytes, 2u);
bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, &bytes[2], 6u);
sample->temperature_raw = little_endian_i16(&bytes[0]);
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
sample->gyro_raw[axis] = little_endian_i16(&bytes[2u + 2u * axis]);
}
}
void st_lsm6dsox_default_config(st_lsm6dsox_config_t* config)
{
if(config == NULL)
{
return;
}
config->odr_hz = 104u;
config->accel_full_scale_g = 2u;
config->gyro_full_scale_dps = 250u;
}
bool st_lsm6dsox_init(const st_lsm6dsox_config_t* config)
{
uint8_t odr_bits;
uint8_t accel_bits;
uint8_t gyro_bits;
double unused_accel_scale;
double unused_gyro_scale;
if((config == NULL) || !odr_register_value(config->odr_hz, &odr_bits) ||
!accel_full_scale_bits(config->accel_full_scale_g, &accel_bits,
&unused_accel_scale) ||
!gyro_full_scale_bits(config->gyro_full_scale_dps, &gyro_bits,
&unused_gyro_scale))
{
return false;
}
memset((void*)&diagnostics, 0, sizeof(diagnostics));
sample_head = 0u;
sample_tail = 0u;
active_config = *config;
bsp_pmod_spi_init();
bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE);
bsp_pmod_spi_set_clock_mode(BSP_PMOD_SPI_CLOCK_MODE0);
bsp_pmod_spi_set_timing(2u, 2u, 2u, 5u);
delay_ms(20u);
diagnostics.who_am_i = read_register(LSM6DSOX_WHO_AM_I);
if(diagnostics.who_am_i != LSM6DSOX_WHO_AM_I_VALUE)
{
return false;
}
write_register(LSM6DSOX_CTRL3_C, LSM6DSOX_CTRL3_SW_RESET);
uint64_t reset_start = time_now();
uint64_t reset_timeout =
((uint64_t)BSP_CLINT_TIMEBASE_HZ * 100u + 999u) / 1000u;
while((read_register(LSM6DSOX_CTRL3_C) & LSM6DSOX_CTRL3_SW_RESET) != 0u)
{
if((time_now() - reset_start) >= reset_timeout)
{
return false;
}
}
delay_ms(10u);
write_register(LSM6DSOX_CTRL3_C,
LSM6DSOX_CTRL3_BDU | LSM6DSOX_CTRL3_IF_INC);
/* Active-high push-pull is the CTRL3_C reset polarity/driver setting. */
write_register(LSM6DSOX_INT1_CTRL, 0u);
write_register(LSM6DSOX_INT2_CTRL, 0u);
write_register(LSM6DSOX_TAP_CFG2,
read_register(LSM6DSOX_TAP_CFG2) &
(uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
write_register(LSM6DSOX_COUNTER_BDR_REG1, 0u);
write_register(LSM6DSOX_CTRL1_XL, (uint8_t)(odr_bits | accel_bits));
write_register(LSM6DSOX_CTRL2_G, (uint8_t)(odr_bits | gyro_bits));
delay_ms(100u);
if((read_register(LSM6DSOX_CTRL3_C) !=
(LSM6DSOX_CTRL3_BDU | LSM6DSOX_CTRL3_IF_INC)) ||
(read_register(LSM6DSOX_CTRL1_XL) != (uint8_t)(odr_bits | accel_bits)) ||
(read_register(LSM6DSOX_CTRL2_G) != (uint8_t)(odr_bits | gyro_bits)) ||
(read_register(LSM6DSOX_INT1_CTRL) != 0u) ||
(read_register(LSM6DSOX_INT2_CTRL) != 0u) ||
((read_register(LSM6DSOX_TAP_CFG2) &
LSM6DSOX_INTERRUPTS_ENABLE) != 0u) ||
((read_register(LSM6DSOX_COUNTER_BDR_REG1) &
LSM6DSOX_DATAREADY_PULSED) != 0u))
{
st_lsm6dsox_stop();
return false;
}
diagnostics.initialized = true;
return true;
}
bool st_lsm6dsox_start(void)
{
if(!diagnostics.initialized || diagnostics.running)
{
return diagnostics.running;
}
bsp_plic_disable(BSP_PMOD_IRQ);
write_register(LSM6DSOX_INT1_CTRL, 0u);
write_register(LSM6DSOX_INT2_CTRL, 0u);
write_register(LSM6DSOX_TAP_CFG2,
read_register(LSM6DSOX_TAP_CFG2) &
(uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
write_register(LSM6DSOX_COUNTER_BDR_REG1, 0u);
/* Clear an old latched data-ready before exposing the PLIC source. */
if((read_register(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_XLDA) != 0u)
{
st_lsm6dsox_raw_sample_t stale;
read_raw_outputs(&stale, time_now());
}
/* Route accel DRDY to the wired sensor INT1 pad, then globally enable the
* physical interrupt pads as required by ST's reference driver. */
write_register(LSM6DSOX_INT1_CTRL, LSM6DSOX_INT1_DRDY_XL);
write_register(LSM6DSOX_TAP_CFG2,
read_register(LSM6DSOX_TAP_CFG2) |
LSM6DSOX_INTERRUPTS_ENABLE);
if((read_register(LSM6DSOX_INT1_CTRL) != LSM6DSOX_INT1_DRDY_XL) ||
(read_register(LSM6DSOX_INT2_CTRL) != 0u) ||
((read_register(LSM6DSOX_TAP_CFG2) &
LSM6DSOX_INTERRUPTS_ENABLE) == 0u) ||
((read_register(LSM6DSOX_COUNTER_BDR_REG1) &
LSM6DSOX_DATAREADY_PULSED) != 0u))
{
write_register(LSM6DSOX_INT1_CTRL, 0u);
write_register(LSM6DSOX_INT2_CTRL, 0u);
write_register(LSM6DSOX_TAP_CFG2,
read_register(LSM6DSOX_TAP_CFG2) &
(uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
return false;
}
bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
diagnostics.running = true;
bsp_plic_enable(BSP_PMOD_IRQ);
return true;
}
void st_lsm6dsox_stop(void)
{
bsp_plic_disable(BSP_PMOD_IRQ);
if(diagnostics.who_am_i == LSM6DSOX_WHO_AM_I_VALUE)
{
write_register(LSM6DSOX_INT1_CTRL, 0u);
write_register(LSM6DSOX_INT2_CTRL, 0u);
write_register(LSM6DSOX_TAP_CFG2,
read_register(LSM6DSOX_TAP_CFG2) &
(uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
write_register(LSM6DSOX_CTRL1_XL, 0u);
write_register(LSM6DSOX_CTRL2_G, 0u);
}
diagnostics.running = false;
}
void st_lsm6dsox_irq_handler(void)
{
const uint64_t timestamp = time_now();
__atomic_fetch_add(&diagnostics.irq_count, 1u, __ATOMIC_RELAXED);
if(!__atomic_load_n(&diagnostics.running, __ATOMIC_RELAXED))
{
__atomic_fetch_add(&diagnostics.spurious_irq_count, 1u,
__ATOMIC_RELAXED);
return;
}
uint8_t status = read_register(LSM6DSOX_STATUS_REG);
st_lsm6dsox_raw_sample_t sample;
sample.timestamp_ticks = timestamp;
/* Always clear the routed, latched accel source before returning. */
read_raw_accel(&sample);
if((status & LSM6DSOX_STATUS_XLDA) == 0u)
{
__atomic_fetch_add(&diagnostics.spurious_irq_count, 1u,
__ATOMIC_RELAXED);
return;
}
uint64_t wait_start = time_now();
uint64_t timeout_ticks =
((uint64_t)BSP_CLINT_TIMEBASE_HZ * LSM6DSOX_GYRO_READY_TIMEOUT_US +
999999u) / 1000000u;
while((read_register(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_GDA) == 0u)
{
if((time_now() - wait_start) >= timeout_ticks)
{
__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u,
__ATOMIC_RELAXED);
return;
}
}
read_raw_gyro_temperature(&sample);
if(!raw_sample_valid(&sample))
{
__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u,
__ATOMIC_RELAXED);
return;
}
sample.sequence = (uint32_t)(
__atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED) + 1u);
uint64_t previous_timestamp = __atomic_exchange_n(
&diagnostics.last_timestamp_ticks, timestamp, __ATOMIC_RELAXED);
if(previous_timestamp != 0u)
{
uint64_t expected_ticks = BSP_CLINT_TIMEBASE_HZ / active_config.odr_hz;
uint64_t elapsed = timestamp - previous_timestamp;
if((expected_ticks != 0u) &&
(elapsed > expected_ticks + expected_ticks / 2u))
{
uint64_t periods = (elapsed + expected_ticks / 2u) / expected_ticks;
if(periods > 1u)
{
__atomic_fetch_add(&diagnostics.estimated_missed_sample_count,
periods - 1u, __ATOMIC_RELAXED);
}
}
}
(void)queue_push_isr(&sample);
__atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED);
}
void st_lsm6dsox_service(void)
{
}
bool st_lsm6dsox_take_measurement(
fusion_imu_measurement_t* measurement_out)
{
if(measurement_out == NULL)
{
return false;
}
st_lsm6dsox_raw_sample_t raw;
if(!queue_pop(&raw))
{
return false;
}
double accel_mg_per_lsb;
double gyro_mdps_per_lsb;
uint8_t unused_bits;
if(!accel_full_scale_bits(active_config.accel_full_scale_g, &unused_bits,
&accel_mg_per_lsb) ||
!gyro_full_scale_bits(active_config.gyro_full_scale_dps, &unused_bits,
&gyro_mdps_per_lsb))
{
__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u,
__ATOMIC_RELAXED);
return false;
}
memset(measurement_out, 0, sizeof(*measurement_out));
measurement_out->timestamp_ticks = raw.timestamp_ticks;
for(unsigned int axis = 0 ; axis < 3u ; axis++)
{
measurement_out->accel_mps2[axis] =
(double)raw.accel_raw[axis] * accel_mg_per_lsb *
ST_GRAVITY_MPS2 / 1000.0;
measurement_out->gyro_rps[axis] =
(double)raw.gyro_raw[axis] * gyro_mdps_per_lsb *
ST_DEGREES_TO_RADIANS / 1000.0;
}
measurement_out->temperature_c =
25.0 + (double)raw.temperature_raw / 256.0;
measurement_out->sequence = raw.sequence;
return true;
}
void st_lsm6dsox_get_diagnostics(
st_lsm6dsox_diagnostics_t* diagnostics_out)
{
if(diagnostics_out != NULL)
{
diagnostics_out->irq_count =
__atomic_load_n(&diagnostics.irq_count, __ATOMIC_RELAXED);
diagnostics_out->sample_count =
__atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED);
diagnostics_out->invalid_sample_count =
__atomic_load_n(&diagnostics.invalid_sample_count, __ATOMIC_RELAXED);
diagnostics_out->spurious_irq_count =
__atomic_load_n(&diagnostics.spurious_irq_count, __ATOMIC_RELAXED);
diagnostics_out->queue_overflow_count =
__atomic_load_n(&diagnostics.queue_overflow_count, __ATOMIC_RELAXED);
diagnostics_out->estimated_missed_sample_count = __atomic_load_n(
&diagnostics.estimated_missed_sample_count, __ATOMIC_RELAXED);
diagnostics_out->last_timestamp_ticks =
__atomic_load_n(&diagnostics.last_timestamp_ticks, __ATOMIC_RELAXED);
diagnostics_out->who_am_i = diagnostics.who_am_i;
diagnostics_out->initialized = diagnostics.initialized;
diagnostics_out->running = diagnostics.running;
}
}