关于陀螺仪的校准 (About gyroscope calibration)

[ou2356]

Subject of the issue

@PaulZC你好，首先非常感谢你贡献的代码，这边我想对前人提出的陀螺仪校准问题进行讨论。

Google Translate: Hello @PaulZC , first of all thank you very much for the code you contributed, here I want to discuss the problem of gyroscope calibration raised by the predecessors.

Your workbench

• What platform are you using?
• ESP32-S3
• What version of the device are you using? Is there a firmware version?
• V1.2.9
• How is the device wired to your platform?
• SPI，SCK = 4MHz，通过电平转换芯片通信
• How is everything being powered?
• 笔记本电脑USB供电

我在brawner的代码中看到了关于陀螺仪校准的内容，(https://github.com/isouriadakis/Arduino_ICM20948_DMP_Full-Function/blob/a13508d8145e003ea83190187047fb9dc247c6fc/ICM20948/Icm20948Setup.c#L625-L632) 他的代码中注释为满量程为2^20，且固定为2000dps，然后我对他进行了如下代码处理，效果还是不错的，可以实现一定的校准。也非常感谢@brawner的代码的辛勤付出。

Google Translate: I saw the content about gyroscope calibration in brawner 's code. His code is commented that the full scale is 2^20, and it is fixed at 2000dps, and then I processed him with the following code, the effect is still good, it can be achieved certain calibration. Also a big thanks to @brawner for the hard work on the code.

int16_t x, y, z;
int16_t gyrBias[3] = {0};
gyrBias[0] = data.Raw_Gyro.Data.BiasX >> (5 - dps2000 + icmSettings.gyr_fsr);
gyrBias[1] = data.Raw_Gyro.Data.BiasY >> (5 - dps2000 + icmSettings.gyr_fsr);
gyrBias[2] = data.Raw_Gyro.Data.BiasZ >> (5 - dps2000 + icmSettings.gyr_fsr);
x = data.Raw_Gyro.Data.X - gyrBias[0]; //校准后的数据
y = data.Raw_Gyro.Data.Y - gyrBias[1]; //校准后的数据
z = data.Raw_Gyro.Data.Z - gyrBias[2]; //校准后的数据

我想说的是data.Raw_Gyro.Data.BiasX，data.Raw_Gyro.Data.BiasY，data.Raw_Gyro.Data.BiasZ并不是僵尸数据，他们是真实有用的。

Google Translate: What I want to say is that data.Raw_Gyro.Data.BiasX, data.Raw_Gyro.Data.BiasY, data.Raw_Gyro.Data.BiasZ are not zombie data, they are real and useful.

我的配置代码如下：
// Enable any additional sensors / features
success &= (myICM.enableDMPSensor(INV_ICM20948_SENSOR_GYROSCOPE) == ICM_20948_Stat_Ok);

success &= (myICM.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum

加速度和陀螺仪的输出速率都设置成了225Hz。

由于我并没有使用Arduino IDE来使用您的库代码，我对你的例程代码加入了自己的一些代码实现，所以我这里就不把代码全部贴出来了，以免让你混淆，如果你需要的话请随时告诉我，我很乐意共享它。

Google Translate: The accelerometer and gyroscope output rates are both set to 225Hz.

Since I didn't use the Arduino IDE to use your library code, I added some of my own code implementation to your routine code, so I won't post all the code here, so as not to confuse you, if you need it Please feel free to let me know, I'd love to share it.

[ou2356]

顺便提一句，由于我想把ICM20948应用在无人机上面，于是就把DMP的加速度、陀螺仪数据输出速率设置成了1125Hz（由于我想用校准后的陀螺仪的值，所以选择用DMP来获得数据），但是实际只有700Hz的输出速率，而且当我的SPI通信速率为4210526Hz会有一个突然地出现一个很高或者很低的数值，这种现象无论是加速度计、陀螺仪和磁力计都会有。但是SPI刚好为4MHz时是不会有这个现象的，甚至SPI为3809523Hz时也会出现这种数据异常的现象，下图为一个陀螺仪的Z轴在静止状态下的数据图:

Google Translate: By the way, since I want to apply the ICM20948 to the drone, I set the DMP acceleration and gyroscope data output rate to 1125Hz (because I want to use the calibrated gyroscope value, so I choose to use DMP to Get data), but the actual output rate is only 700Hz, and when my SPI communication rate is 4210526Hz, there will be a sudden high or low value, this phenomenon will be accelerometer, gyroscope and magnetometer. Have. However, this phenomenon will not occur when the SPI is just 4MHz, and even when the SPI is 3809523Hz, this data abnormality phenomenon will occur. The following picture shows the data of the Z axis of a gyroscope in a static state:

数据图为MATLAB从串口中获得数据然后作出的图，下面附上MATLAB代码：

clear;

dispTime = 2; %单位(s)
fps = 60;
samp_freq = 1125; %单位(Hz)
lenght = samp_freq*dispTime;

y1 = zeros(1,lenght);
y2 = zeros(1,lenght);
device = serialport("COM6", 2000000, "DataBits", 8, "Parity", "none", "StopBits", 1);
x1 = 1/samp_freq:1/samp_freq:dispTime;
% write(device,sin(0:2pi/lenght:2pi),"double");
i=1;
fps = samp_freq/fps;

% 两组数据分开两个图画
% while true
% temp = read(device,2,"double");
% if size(temp) == 0
% break;
% end
%
% if i>lenght
% y1 = [y1(2:end) temp(1,1)];
% y2 = [y2(2:end) temp(1,2)];
% else
% y1(1,i) = temp(1,1);
% y2(1,i) = temp(1,2);
% end
%
% if rem(i,fps) == 0
% subplot(2,1,1);
% plot(x1,y1,'b-');
% subplot(2,1,2);
% plot(x1,y2,'r:');
% end
% i = i+1;
% end

% 两组数据画在一个图
% while true
% temp = read(device,2,"double");
% if size(temp) == 0
% break;
% end
%
% if i>lenght
% y1 = [y1(2:end) temp(1,1)];
% y2 = [y2(2:end) temp(1,2)];
% else
% y1(1,i) = temp(1,1);
% y2(1,i) = temp(1,2);
% end
%
% if rem(i,fps) == 0
% plot(x1,y1,'b-',x1,y2,'r:');
% end
% i = i+1;
% end

% 一组数据画在一个图
while true
temp = read(device,1,"double");
if size(temp) == 0
break;
end
if i>lenght
y1 = [y1(2:end) temp(1,1)];
else
y1(1,i) = temp(1,1);
end
if rem(i,fps) == 0
plot(x1,y1,'b-');
end
i = i+1;
end
clear;

[ou2356]

这里也给出校准后数据的图和没校准的数据图对比，同样是陀螺仪的Z轴在静止状态下的数据图：

校准后的图（数据集中在0）

Google Translate: Here is also a comparison between the calibrated data and the uncalibrated data, which is also the data map of the Z-axis of the gyroscope in a static state:

Plot after calibration (dataset at 0)

未校准的图（数据集中在-0.6）

Google Translate: Uncalibrated plot (dataset at -0.6)

[PaulZC]

Hello @ou2356 ,

This is fantastic - thank you for sharing.

Please do share your code if possible. Is it somewhere on GitHub?

Very best wishes,
Paul

Google Translate:

你好@ou2356，

这太棒了——谢谢你的分享。

如果可能，请分享您的代码。它在 GitHub 上的某个地方吗？

非常好的祝愿，
保罗

[PaulZC]

Adding a link to issue #60 - where Ilia Baranov ( @ibaranov-cp ) was investigating Gyro Calibrated Data and Bias

[ou2356]

@PaulZC 非常高兴看到你的回复，我打算直接在这里上传我的代码文件压缩包。
项目文件压缩包： test2.zip
这里给出我代码文件的一些简介，我知道读别人的代码是一件痛苦的事，我也承认我的代码注释较少，所以我会尽可能的描述我的项目文件结构来帮组你理解：
芯片：ESP32S3
开发环境：Eclipse CDT + 乐鑫IDF_v4.4插件
项目各组件介绍：
"test2\components\Delay"-延时函数等实现
"test2\components\ICM20948"-基于你的代码库改成的ICM20948驱动
"test2\components\KEY"-按键中断驱动的实现
"test2\components\mdrivers"-基础的SPI以及中断的驱动实现
"test2\components\QuickPrint"-为了缩短串口打印数据的速度，自己实现了浮点数的串口打印来代替printf函数（注意：为了及时响应中断，我的代码中设置了串口波特率为2M，如果你需要更改波特率，请修改"test2\components\QuickPrint\include\QuickPrint.h"中的"#define UART_BAUD_RATE (2000000UL)"）

我对你的代码库进行了轻微的改动，但是配置ICM20948的步骤以及参数我是严格按照你的例程的代码来的，在"test2\main\main.c"中你会发现有一个结构体，该结构体可以方便地配置ICM20948，该结构体的介绍如下：

ICM20948Settings icmSettings = {
.is_SPI = ICM_is_SPI, //选择SPI通信或者I2C通信，你可以在"test2\components\ICM20948\include\UserIcm20948.h"中找到该宏定义，（1:SPI, 0:I2C）
.sample_mode = ICM_20948_Sample_Mode_Continuous, //当没有启用DMP时设置ICM_20948_Sample_Mode_Continuous或ICM_20948_Sample_Mode_Cycled
.enable_gyroscope = true, //是否获取陀螺仪数据
.enable_accelerometer = true, //是否获取加速度计数据
.enable_magnetometer = true, //是否获取地磁数据
/DMP Function Start/
// .enable_gravity = false,
// .enable_linearAcceleration = false,
.enable_quaternion6 = false, //是否获取quaternion6数据，前提是要开启DMP功能
.enable_quaternion9 = false, //是否获取quaternion9数据，前提是要开启DMP功能
// .enable_har = false,
// .enable_steps = false,
/DMP Function End/
.gyroscope_frequency = frq_1125Hz, //设置陀螺仪ODR
.accelerometer_frequency = frq_1125Hz, //设置加速度计ODR
// .magnetometer_frequency = 68.75Hz,
// .gravity_frequency = frq_225Hz,
// .linearAcceleration_frequency = frq_225Hz,
// .quaternion6_frequency = frq_225Hz,
// .quaternion9_frequency = frq_225Hz,
// .har_frequency = frq_225Hz,
// .steps_frequency = frq_225Hz,
.acc_fsr = gpm4, //设置加速度计量程
.gyr_fsr = dps2000, //设置陀螺仪量程
.acc_dlp = acc_d5bw7_n8bw3, //设置加速度计低通滤波器带宽
.gyr_dlp = gyr_d361bw4_n376bw5, //设置陀螺仪低通滤波器带宽
.enable_acc_dlp = true, //是否开启加速度计低通滤波器
.enable_gyr_dlp = true, //是否开启陀螺仪低通滤波器
.enable_dataRDY_int = true, //是否开启data ready中断
.enable_dmp_int = false, //是否开启DMP中断
.enable_dmp = ICM_enable_dmp, //是否开启DMP功能，你可以在"test2\components\ICM20948\include\UserIcm20948.h"中找到该宏定义，（1:enable, 0:disable）
};

然后接下来就介绍一下如何获取数据的，我的工程通过中断触发来获取数据，中断触发时会进入"test2\components\ICM20948\UserIcm20948.c"的"icm20948_int1_task"函数，在该函数中获取数据的函数为"ICM20948_getAGMData"

void ICM20948_getAGMData(mSensorData_t * sensorData, bool isAccDmp, bool isGyrDmp, bool isMagDmp, bool isGyrBias)；

第一个参数为返回获取到的数据的指针。
"isAccDmp"选择获取加速计数据时是获取DMP得到的数据还是寄存器中的数据，（true：获取DMP中的数据，前提是已经开启DMP功能，false：获取寄存器中的数据）。
"isGyrDmp"选择获取陀螺仪数据时是获取DMP得到的数据还是寄存器中的数据，（true：获取DMP中的数据，前提是已经开启DMP功能，false：获取寄存器中的数据）。
"isMagDmp"选择获取地磁数据时是获取DMP得到的数据还是寄存器中的数据，（true：获取DMP中的数据，前提是已经开启DMP功能，false：获取寄存器中的数据）。
"isGyrBias"选择当获取寄存器陀螺仪数据时是否校准陀螺仪数据（前提是开启了DMP功能），如果获取DMP输出的陀螺仪数据择默认是校准的，（true：校准寄存器陀螺仪的数据，false：不校准寄存器陀螺仪的数据）。

"icm20948_int1_task"函数中的"KeySign"为按键按下信号，当按键按下时就会变为true，再按一下就会变回false。按下一次按键就会开始在串口中输出数据。

当你正常运行我的代码后，串口就会打印如下信息：

I (368) QuickPrint: UART BAUD = 2000000

I (369) ICM20948: SPI actual clock = 4000000Hz
Initialization of the sensor returned: All is well.
Device connected!
setSampleMode returned: All is well.
setSampleRate returned: All is well.
Enable DLPF for Accelerometer returned: All is well.
Enable DLPF for Gyroscope returned: All is well.
cfgIntLatch returned: All is well.
intEnable returned: All is well.

Configuration complete!
I (723) gpio: GPIO[9]| InputEn: 1| OutputEn: 0| OpenDrain: 0| Pullup: 1| Pulldown: 1| Intr:1

如果有什么任何疑问可以再问我，祝你愉快！

[PaulZC]

Google Translate:

@PaulZC Very happy to see your reply, I plan to upload my code file zip directly here.
Project file compressed package: test2.zip
Here is a brief introduction to my code files. I know that reading other people's code is a pain. I also admit that my code comments are few, so I will describe my project file structure as much as possible to help you understand. :
Chip: ESP32S3
Development environment: Eclipse CDT + Espressif IDF_v4.4 plug-in project components introduction:
"test2\components\Delay" - implementation of delay functions, etc.
"test2\components\ICM20948" - ICM20948 driver based on your codebase
"test2\components\KEY" - key interrupt driven implementation
"test2\components\mdrivers" - basic SPI and interrupt driver implementation
"test2\components\QuickPrint"-In order to shorten the speed of printing data on the serial port, I implemented the serial port printing of floating-point numbers instead of the printf function (Note: In order to respond to the interrupt in time, the serial port baud rate is set to 2M in my code, if You need to change the baud rate, please modify "#define UART_BAUD_RATE (2000000UL)" in "test2\components\QuickPrint\include\QuickPrint.h")

I have made slight changes to your code base, but the steps and parameters for configuring ICM20948 are strictly in accordance with the code of your routine. In "test2\main\main.c" you will find a structure , the structure can easily configure the ICM20948, the introduction of the structure is as follows:

ICM20948Settings icmSettings = {
.is_SPI = ICM_is_SPI, //Select SPI communication or I2C communication, you can find the macro definition in "test2\components\ICM20948\include\UserIcm20948.h", (1:SPI, 0:I2C)
.sample_mode = ICM_20948_Sample_Mode_Continuous, //Set ICM_20948_Sample_Mode_Continuous or ICM_20948_Sample_Mode_Cycled when DMP is not enabled
.enable_gyroscope = true, //whether to get gyroscope data
.enable_accelerometer = true, //whether to get accelerometer data
.enable_magnetometer = true, //whether to obtain geomagnetic data
/ DMP Function Start /
// .enable_gravity = false,
// .enable_linearAcceleration = false,
.enable_quaternion6 = false, //Whether to obtain quaternion6 data, the premise is to enable the DMP function
.enable_quaternion9 = false, //Whether to get quaternion9 data, the premise is to enable the DMP function
// .enable_har = false,
// .enable_steps = false,
/ DMP Function End /
.gyroscope_frequency = frq_1125Hz, //Set the gyroscope ODR
.accelerometer_frequency = frq_1125Hz, //Set the accelerometer ODR
// .magnetometer_frequency = 68.75Hz,
// .gravity_frequency = frq_225Hz,
// .linearAcceleration_frequency = frq_225Hz,
// .quaternion6_frequency = frq_225Hz,
// .quaternion9_frequency = frq_225Hz,
// .har_frequency = frq_225Hz,
// .steps_frequency = frq_225Hz,
.acc_fsr = gpm4, //Set the accelerometer range
.gyr_fsr = dps2000, //Set the gyroscope range
.acc_dlp = acc_d5bw7_n8bw3, //Set the accelerometer low-pass filter bandwidth
.gyr_dlp = gyr_d361bw4_n376bw5, //Set the gyroscope low-pass filter bandwidth
.enable_acc_dlp = true, //Whether to enable the accelerometer low-pass filter
.enable_gyr_dlp = true, //whether to enable the gyroscope low-pass filter
.enable_dataRDY_int = true, //whether to enable data ready interrupt
.enable_dmp_int = false, //Whether to enable DMP interrupt
.enable_dmp = ICM_enable_dmp, // Whether to enable the DMP function, you can find the macro definition in "test2\components\ICM20948\include\UserIcm20948.h", (1:enable, 0:disable)
};

Then I will introduce how to obtain data. My project obtains data through interrupt triggering. When the interrupt is triggered, it will enter the "icm20948_int1_task" function of "test2\components\ICM20948\UserIcm20948.c", in which the data is obtained. The function is "ICM20948_getAGMData"

void ICM20948_getAGMData(mSensorData_t * sensorData, bool isAccDmp, bool isGyrDmp, bool isMagDmp, bool isGyrBias);

The first parameter is a pointer to return the obtained data.
"isAccDmp" selects whether to obtain the data obtained by DMP or the data in the register when obtaining the accelerometer data, (true: obtain the data in the DMP, provided that the DMP function is enabled, false: obtain the data in the register).
"isGyrDmp" selects whether to obtain the data obtained by DMP or the data in the register when obtaining the gyroscope data, (true: obtain the data in the DMP, provided that the DMP function is enabled, false: obtain the data in the register).
"isMagDmp" selects whether to obtain the data obtained by DMP or the data in the register when obtaining geomagnetic data, (true: obtain the data in the DMP, provided that the DMP function is enabled, false: obtain the data in the register).
"isGyrBias" selects whether to calibrate the gyroscope data when obtaining the register gyroscope data (provided that the DMP function is enabled), if the gyroscope data output by the DMP is obtained, the default is to calibrate, (true: calibrate the register gyroscope data, false : do not calibrate the data of the register gyroscope).

The "KeySign" in the "icm20948_int1_task" function is the key press signal. When the key is pressed, it will become true, and it will return to false when the key is pressed again. Pressing a key will start outputting data in the serial port.

When you run my code normally, the serial port will print the following information:

I (368) QuickPrint: UART BAUD = 2000000

I (369) ICM20948: SPI actual clock = 4000000Hz
Initialization of the sensor returned: All is well.
Device connected!
setSampleMode returned: All is well.
setSampleRate returned: All is well.
Enable DLPF for Accelerometer returned: All is well.
Enable DLPF for Gyroscope returned: All is well.
cfgIntLatch returned: All is well.
intEnable returned: All is well.

Configuration complete!
I (723) gpio: GPIO[9]| InputEn: 1| OutputEn: 0| OpenDrain: 0| Pullup: 1| Pulldown: 1| Intr: 1

If you have any questions, you can ask me again, I wish you a good day!

[PaulZC]

Thank you @ou2356 !

Very best wishes,
Paul