陀螺仪LSM6DSOW开发.10--陀螺仪校准
概述
在现代电子设备中,陀螺仪传感器被广泛用于测量设备的旋转运动。然而,由于各种环境因素和制造工艺的影响,陀螺仪通常会存在零速率偏移(Zero-rate Offset),这会导致测量数据的不准确。为了提高测量的精度,需要对陀螺仪进行校准。
MotionGC库是STMicroelectronics提供的一个专门用于陀螺仪校准的中间件库。该库作为X-CUBE-MEMS1软件扩展包的一部分,运行在STM32微控制器上。它通过计算和补偿陀螺仪的角速度偏移,确保陀螺仪输出的角速度数据更加准确和可靠。
MotionGC库提供了多种API,允许用户在不同的采样频率下执行实时校准。库的核心功能包括检测设备的静止状态、计算陀螺仪的偏移量,并应用这些偏移量来校正传感器输出的数据。该库还允许用户根据特定应用需求调整校准参数,从而优化校准效果。
需要样片的可以加群申请:615061293 。
视频教学
样品申请
https://www.wjx.top/vm/OhcKxJk.aspx#
源码下载
硬件准备
首先需要准备一个开发板,这里我准备的是自己绘制的开发板,需要的可以进行申请。
主控为STM32H503CB,陀螺仪为LSM6DSOW,磁力计为LIS2MDL。
开启CRC
串口设置
设置串口速率为2000000。
开启X-CUBE-MEMS1
速率选择
陀螺仪数据可以设置25Hz-200Hz。
参考程序
这里参考 IKS01A3_MagnetometerCalibration 。
MX_MEMS_Init
MX_MEMS_Init() 函数是用于初始化MEMS(微机电系统)传感器组件的,特别是在这个上下文中,主要是针对陀螺仪的校准过程。下面是该函数在初始化过程中所执行的详细步骤:
调用 MX_GyroscopeCalibration_Init() 进行陀螺仪校准初始化
这是整个初始化过程的核心部分。该函数负责为陀螺仪校准准备必要的配置和设置,包括初始化MotionGC库。
MotionGC库初始化
在MX_GyroscopeCalibration_Init()中,首先调用了MotionGC_manager_init()函数,这个函数进一步调用了MotionGC_Initialize()函数,用于初始化MotionGC校准引擎。该引擎专门用于处理陀螺仪的偏置校准。
MotionGC_Initialize()接受两个参数:
mcu_type:指定所使用的MCU类型,在这里通常是MGC_MCU_STM32。
freq:设置采样频率,以确保校准算法按照正确的时间间隔运行。
获取并打印库版本信息
MotionGC_manager_get_version() 函数用于获取MotionGC库的版本信息,并将版本号打印出来。这在调试和版本控制时非常有用。
MotionGC_GetLibVersion() 会返回版本号的字符串长度,并将版本号保存到一个字符数组中。
获取并设置校准参数
MotionGC_GetKnobs() 函数用于获取当前的校准“旋钮”(Knobs)设置,这些设置包括了加速度计和陀螺仪的阈值、过滤常数等。
然后通过MotionGC_SetKnobs() 函数对这些参数进行调整。例如,设置加速度计的静止状态检测阈值为0.008g,陀螺仪的阈值为0.15dps。这些设置可以根据具体应用场景进行优化。
设置初始陀螺仪偏置
为了加快校准过程,可以通过MotionGC_manager_set_params() 设置初始的陀螺仪偏置值,主要执行MotionGC_SetCalParams。默认情况下,偏置值通常被设置为0.0f,表示没有初始偏置。
设置采样频率
通过MotionGC_manager_set_frequency() 函数设置采样频率,主要执行MotionGC_SetFrequency。采样频率是根据报告间隔计算得出的,确保陀螺仪校准算法在正确的频率下运行。
初始化定义
/* USER CODE BEGIN 2 */
printf("HELLO!\n");
HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET);
HAL_Delay(100);
stmdev_ctx_t dev_ctx;
/* Initialize mems driver interface */
dev_ctx.write_reg = platform_write;
dev_ctx.read_reg = platform_read;
dev_ctx.mdelay = platform_delay;
dev_ctx.handle = &SENSOR_BUS;
/* Init test platform */
// platform_init();
/* Wait sensor boot time */
platform_delay(BOOT_TIME);
/* Check device ID */
lsm6dso_device_id_get(&dev_ctx, &whoamI);
printf("LSM6DSO_ID=0x%x,whoamI=0x%x",LSM6DSO_ID,whoamI);
if (whoamI != LSM6DSO_ID)
while (1);
/* Restore default configuration */
lsm6dso_reset_set(&dev_ctx, PROPERTY_ENABLE);
do {
lsm6dso_reset_get(&dev_ctx, &rst);
} while (rst);
/* Disable I3C interface */
lsm6dso_i3c_disable_set(&dev_ctx, LSM6DSO_I3C_DISABLE);
/* Enable Block Data Update */
lsm6dso_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
/* Set Output Data Rate */
lsm6dso_xl_data_rate_set(&dev_ctx, LSM6DSO_XL_ODR_52Hz);
lsm6dso_gy_data_rate_set(&dev_ctx, LSM6DSO_GY_ODR_52Hz);
/* Set full scale */
lsm6dso_xl_full_scale_set(&dev_ctx, LSM6DSO_2g);
lsm6dso_gy_full_scale_set(&dev_ctx, LSM6DSO_2000dps);
/* Configure filtering chain(No aux interface)
* Accelerometer - LPF1 + LPF2 path
*/
// lsm6dso_xl_hp_path_on_out_set(&dev_ctx, LSM6DSO_LP_ODR_DIV_100);
// lsm6dso_xl_filter_lp2_set(&dev_ctx, PROPERTY_ENABLE);
MX_MEMS_Init();
uint8_t acc_flag=0,gy_flag=0;
/* USER CODE END 2 */
MX_MEMS_Process
MX_MEMS_Process() 函数在MEMS传感器应用中主要负责处理传感器数据并执行与校准相关的操作。在这个上下文中,MX_MEMS_Process() 函数具体执行以下任务:
MX_MEMS_Process() 主要调用了 MX_GyroscopeCalibration_Process() 函数。这是一个私有函数,专门用于处理陀螺仪校准数据的流和处理。
在 MX_GyroscopeCalibration_Process() 函数中,主要执行了 GC_Data_Handler(),它负责从传感器中读取数据,运行校准算法,并应用校准结果。
MotionGC_manager_update
GC_Data_Handler() 函数从加速度计和陀螺仪读取当前的传感器数据,具体包括:
● 将加速度计数据从毫重力单位(mg)转换为重力单位(g)。
● 将陀螺仪数据从毫度每秒(mdps)转换为度每秒(dps)。
然后调用 MotionGC_manager_update() 函数来运行陀螺仪校准算法,更新偏置值,并通过 MotionGC_manager_compensate() 函数对原始数据进行补偿,得到校准后的角速度数据,MotionGC_Update()。
● 当调用 MotionGC_Update() 函数后,如果陀螺仪的偏置(gyro bias)被更新了,那么 bias_update 所指向的整数值会被设置为 1,表示偏置已更新。
● 如果陀螺仪的偏置没有变化,则 bias_update 所指向的整数值将保持为 0,表示偏置没有更新。
这个参数的作用是让调用者知道在当前的调用中,校准算法是否对陀螺仪的偏置进行了调整。这对于动态监控校准过程和判断当前校准状态是否稳定非常有用。
MotionGC_manager_compensate
MotionGC_manager_compensate() 函数对原始数据进行补偿,得到校准后的角速度数据,主要执行MotionGC_GetCalParams。
● 这个函数用于获取当前陀螺仪的补偿参数,也就是当前的陀螺仪角速度零点偏置(angular zero-rate level)。
● 参数 *gyro_bias 是一个指向结构体的指针,这个结构体包含了陀螺仪在X、Y、Z轴上的零速率偏置值。调用这个函数后,gyro_bias 结构体将被填充当前的校准参数。
主程序执行流程
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
uint8_t reg;
/* Read output only if new xl value is available */
lsm6dso_xl_flag_data_ready_get(&dev_ctx, ®);
if (reg) {
/* Read acceleration field data */
memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
lsm6dso_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
acceleration_mg[0] =
lsm6dso_from_fs2_to_mg(data_raw_acceleration[0]);
acceleration_mg[1] =
lsm6dso_from_fs2_to_mg(data_raw_acceleration[1]);
acceleration_mg[2] =
lsm6dso_from_fs2_to_mg(data_raw_acceleration[2]);
// printf( "Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
// acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
acc_flag=1;
}
lsm6dso_gy_flag_data_ready_get(&dev_ctx, ®);
if (reg) {
/* Read angular rate field data */
memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
lsm6dso_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate);
angular_rate_mdps[0] =
lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[0]);
angular_rate_mdps[1] =
lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[1]);
angular_rate_mdps[2] =
lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate[2]);
// printf("Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
// angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
gy_flag=1;
}
if(acc_flag && gy_flag)
{
MX_MEMS_Process();
gy_flag=0;
acc_flag=0;
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
演示
陀螺仪的偏置未被更新为0。
陀螺仪的偏置被更新为1。
可以看到修正后静止状态夏更稳定。