陀螺儀LSM6DSOW開發(8)----磁力計校準
概述
磁力計校準是確保傳感器數據準確性和可靠性的關鍵步驟。磁力計用于測量地球磁場，并在導航、定位、姿態測量等應用中起到重要作用。然而，磁力計在使用過程中會受到環境磁場、硬件偏差、安裝誤差等因素的影響，從而導致測量數據出現偏差。因此，校準磁力計以消除這些影響，是獲得精確測量數據的必要步驟。
本文將介紹如何使用ST提供的MotionFX庫在嵌入式系統中實現磁力計校準， 通過本文的介紹，讀者將能夠理解磁力計校準的基本概念，掌握使用MotionFX庫進行校準的步驟和方法，并學會如何在實際項目中實現磁力計的校準，以獲得高精度的磁場測量數據。
需要樣片的可以加群申請：615061293 。
視頻教學
[https://www.bilibili.com/video/BV1Mi421a7QF/]
樣品申請
[https://www.wjx.top/vm/OhcKxJk.aspx#]
源碼下載
[https://download.csdn.net/download/qq_24312945/89614581](
硬件準備
首先需要準備一個開發板，這里我準備的是自己繪制的開發板，需要的可以進行申請。
主控為STM32H503CB，陀螺儀為LSM6DSOW，磁力計為LIS2MDL。
DataLogFusion
這里參考ST提供的DataLogFusion程序，DataLogFusion示例應用展示了如何使用STMicroelectronics開發的MotionFX中間件庫進行實時運動傳感器數據融合。
DataLogFusion的主要執行流程包括初始化硬件和傳感器、中間件庫（MotionFX）的配置與初始化、傳感器數據的采集、實時數據融合以及結果的輸出。
磁力計校準過程
MotionFX庫的磁力計校準庫用于補償硬鐵失真。磁力計校準可以以比傳感器融合輸出數據速率更慢的頻率進行（例如25 Hz）。
● 初始化磁力計校準庫：
● 調用 MotionFX_MagCal_init 或 MotionFX_CM0P_MagCal_init 函數。
● 定期調用校準函數：
● 調用 MotionFX_MagCal_run 或 MotionFX_CM0P_MagCal_run 函數，直到校準成功完成。
● 檢查校準是否成功：
● 調用 MotionFX_MagCal_getParams 或 MotionFX_CM0P_MagCal_getParams 函數。如果函數返回 mag_data_out.cal_quality = MFX_MAGCALGOOD 或 MFX_CM0P_CALQSTATUSBEST，則校準成功。
在app_mems.c中的Magneto_Sensor_Handler函數負責處理磁力計（磁傳感器）數據的獲取、校準和發送。該函數的主要功能是從磁力計傳感器獲取數據，進行必要的校準，然后將處理后的數據傳輸給其他部分使用。具體包括以下步驟：
- 檢查傳感器是否啟用：函數首先檢查是否啟用了磁力計傳感器。如果傳感器未啟用，則函數不會執行進一步操作。
- 獲取磁力計數據：
○ 如果使用離線數據（即在調試或仿真模式下），函數會從預定義的離線數據數組中讀取磁力計數據。
○ 如果使用實時數據，函數會通過調用BSP_SENSOR_MAG_GetAxes函數從實際的磁力計傳感器中讀取當前的磁力計數據。 - 執行磁力計校準：
○ 函數檢查磁力計是否已經校準。如果尚未校準，則會進行校準過程。
○ 將獲取的磁力計數據轉換為適當的單位，并準備校準輸入數據。
○ 調用MotionFX_manager_MagCal_run函數運行校準算法。
○ 如果校準結果良好（校準質量達到標準），函數會保存校準偏移值，并停止進一步的校準。 - 應用校準偏移：將校準偏移應用到磁力計讀數中，以補償硬件偏差和環境干擾。
- 序列化并發送數據：將校準后的磁力計數據序列化，并存儲到消息結構（Msg）中，以供系統其他部分使用。
MotionFX_manager_MagCal_run函數用于運行磁力計校準算法。該函數接受磁力計輸入數據，調用MotionFX庫中的校準算法對數據進行處理，并返回校準結果。
MotionFX_MagCal_run函數執行校準算法，對輸入數據進行處理，計算出校準所需的參數。
MotionFX_MagCal_getParams函數從校準算法中獲取校準后的參數，并存儲在輸出數據結構data_out中。這些參數包括磁力計的偏移和標度因子等校準信息。
MotionFX_MagCal_getParams
MotionFX_MagCal_getParams函數用于獲取磁力計的校準參數。這些參數包括校準后的硬鐵偏移量和校準質量指標。該函數通過傳入一個指向輸出數據結構的指針，返回校準結果的詳細信息。
調用MotionFX_MagCal_getParams函數后，可以通過檢查data_out結構體中的參數來評估校準結果的質量，并應用偏移量來調整磁力計數據。
cal_quality：校準質量因子，指示校準結果的準確性。具體值包括： MFX_MAGCALUNKNOWN = 0：校準參數的準確性未知。 MFX_MAGCALPOOR = 1：校準參數的準確性較差，不能被信任。 MFX_MAGCALOK = 2：校準參數的準確性尚可。 MFX_MAGCALGOOD = 3：校準參數的準確性良好。
校準移植
在main.c中添加變量定義。
/* USER CODE BEGIN 0 */
float MagOffset[3]={0.0f,0.0f,0.0f};//磁力計偏差
int Mag_TimeStamp,Mag_TimeStamp_1,Mag_TimeStamp_2;//磁力計時間戳
uint8_t Mag_flag=0;
/* USER CODE END 0 */
磁力計數據官方文檔推薦20/40HZ。
在mian.c中添加磁力計校準執行函數這里陀螺儀數據為416Hz，單次循環執行10次，所以讓磁力計在單次循環中只執行一次，頻率則為40Hz左右。
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
if(fifo_flag)// 如果 FIFO 中斷標志被設置
{
uint8_t acc_flag=0,gyr_flag=0;//加速度角速度標志位
uint8_t deltatime_flag=0;//時間標志位
// printf("fifo_num=%dn",fifo_num);
for(int i=0;i< fifo_num;i++)// 遍歷 FIFO 數據數組
{
// 獲取數據指針
datax = (int16_t *)&fifo_data[i][1];
datay = (int16_t *)&fifo_data[i][3];
dataz = (int16_t *)&fifo_data[i][5];
// 根據數據標簽處理不同類型的數據
switch (fifo_data[i][0]) {
case LSM6DSO_XL_NC_TAG:// 加速度數據
acc_flag=1;
acc_x=lsm6dso_from_fs2_to_mg(*datax);
acc_y=lsm6dso_from_fs2_to_mg(*datay);
acc_z=lsm6dso_from_fs2_to_mg(*dataz);
// printf("Acceleration [mg]:%4.2ft%4.2ft%4.2frn",
// acc_x, acc_y, acc_z);
break;
case LSM6DSO_GYRO_NC_TAG:// 角速度數據
gyr_flag=1;
gyr_x=lsm6dso_from_fs2000_to_mdps(*datax);
gyr_y=lsm6dso_from_fs2000_to_mdps(*datay);
gyr_z=lsm6dso_from_fs2000_to_mdps(*dataz);
// printf("Angular rate [mdps]:%4.2ft%4.2ft%4.2frn",
// gyr_x,gyr_y,gyr_z);
break;
case LSM6DSO_TIMESTAMP_TAG:// 時間戳數據
deltatime_flag=1;
/* 讀取時間戳數據 */
uint32_t timestamp=0;
timestamp+= fifo_data[i][1];
timestamp+= fifo_data[i][2]< 8;
timestamp+= fifo_data[i][3]< 16;
timestamp+= fifo_data[i][4]< 24;
if(deltatime_first==0)//第一次
{
deltatime_1=timestamp;
deltatime_2=deltatime_1;
deltatime_first=1;
Mag_TimeStamp_1=timestamp;
Mag_TimeStamp_2=timestamp;
}
else
{
deltatime_2=timestamp;
Mag_TimeStamp_2=timestamp;
}
// printf("timestamp=%drn",timestamp);
break;
default:
break;
}
if(i==0)
Mag_flag=1;
// 如果加速度、角速度和時間戳數據都已獲取
if(acc_flag&&gyr_flag&&deltatime_flag)
{
memset(data_raw_magnetic, 0x00, 3 * sizeof(int16_t));
lis2mdl_magnetic_raw_get(&lis2mdl_dev_ctx, data_raw_magnetic);
magnetic_mG[0] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[0]);
magnetic_mG[1] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[1]);
magnetic_mG[2] = lis2mdl_from_lsb_to_mgauss(data_raw_magnetic[2]);
if(Mag_flag)
{
Mag_TimeStamp=Mag_TimeStamp_2-Mag_TimeStamp_1;
if(Mag_TimeStamp_2 >Mag_TimeStamp_1)
Mag_TimeStamp=(int)((Mag_TimeStamp_2-Mag_TimeStamp_1)*25.0f/1000);
else if(Mag_TimeStamp_1 >Mag_TimeStamp_2)
Mag_TimeStamp=(int)((0xffffffff-Mag_TimeStamp_2+Mag_TimeStamp_1)*25.0f/1000);
else if(Mag_TimeStamp_1==Mag_TimeStamp_2)
Mag_TimeStamp=0;
Magneto_Sensor_Handler();
Mag_TimeStamp_1=Mag_TimeStamp_2;
}
magnetic_mG[0] = magnetic_mG[0]-MagOffset[0];
magnetic_mG[1] = magnetic_mG[1]-MagOffset[1];
magnetic_mG[2] = magnetic_mG[2]-MagOffset[2];
lsm6dso_motion_fx_determin();// 調用 MotionFX 處理函數
acc_flag=0;
gyr_flag=0;
deltatime_flag=0;
deltatime_1=deltatime_2; // 更新時間戳
Mag_flag=0;
}
}
// 清除 FIFO 標志和數據量
fifo_flag=0;
fifo_num=0;
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
在app.h中添加磁力計校準函數定義。
#define FROM_UT50_TO_MGAUSS 500.0f
void Magneto_Sensor_Handler(void);
在app.c中添加磁力計校準函數。
typedef struct
{
uint8_t hours;
uint8_t minutes;
uint8_t seconds;
uint8_t subsec;
float pressure;
float humidity;
float temperature;
int32_t acceleration_x_mg;
int32_t acceleration_y_mg;
int32_t acceleration_z_mg;
int32_t angular_rate_x_mdps;
int32_t angular_rate_y_mdps;
int32_t angular_rate_z_mdps;
int32_t magnetic_field_x_mgauss;
int32_t magnetic_field_y_mgauss;
int32_t magnetic_field_z_mgauss;
} offline_data_t;
#define OFFLINE_DATA_SIZE 8
uint8_t UseOfflineData = 0;
uint8_t MagCalStatus = 0;
offline_data_t OfflineData[OFFLINE_DATA_SIZE];
extern float MagOffset[3];
extern float magnetic_mG[3];
extern int Mag_TimeStamp;
/**
* @brief Handles the MAG axes data getting/sending
* @param Msg the MAG part of the stream
* @retval None
*/
void Magneto_Sensor_Handler(void)
{
float ans_float;
MFX_MagCal_input_t mag_data_in;
MFX_MagCal_output_t mag_data_out;
mag_data_in.mag[0] = (float)magnetic_mG[0] * FROM_MGAUSS_TO_UT50;
mag_data_in.mag[1] = (float)magnetic_mG[1] * FROM_MGAUSS_TO_UT50;
mag_data_in.mag[2] = (float)magnetic_mG[2] * FROM_MGAUSS_TO_UT50;
mag_data_in.time_stamp = (int)Mag_TimeStamp;
// Mag_TimeStamp += (uint32_t)ALGO_PERIOD;
MotionFX_MagCal_run(&mag_data_in);
MotionFX_MagCal_getParams(&mag_data_out);
printf("mag_data_out=%d,MFX_MAGCALGOOD=%dn",mag_data_out.cal_quality,MFX_MAGCALGOOD);
if (mag_data_out.cal_quality == MFX_MAGCALGOOD)
{
// MagCalStatus = 1;
ans_float = (mag_data_out.hi_bias[0] * FROM_UT50_TO_MGAUSS);
MagOffset[0] = (int32_t)ans_float;
ans_float = (mag_data_out.hi_bias[1] * FROM_UT50_TO_MGAUSS);
MagOffset[1] = (int32_t)ans_float;
ans_float = (mag_data_out.hi_bias[2] * FROM_UT50_TO_MGAUSS);
MagOffset[2] = (int32_t)ans_float;
// /* Disable magnetometer calibration */
// MotionFX_manager_MagCal_stop(ALGO_PERIOD);
}
}
演示
未校準成功時未0。
校準成功時為3。
審核編輯 黃宇
-
傳感器
+關注
關注
2541文章
49797瀏覽量
746748 -
陀螺儀
+關注
關注
44文章
768瀏覽量
98103 -
磁力計
+關注
關注
1文章
70瀏覽量
20732
發布評論請先 登錄
相關推薦
驅動LSM6DS3TR-C實現高效運動檢測與數據采集(10)----融合磁力計進行姿態解算
陀螺儀LSM6DSOW開發(1)----輪詢獲取陀螺儀數據
陀螺儀LSM6DSOW開發(3)----FIFO數據讀取與配置
陀螺儀LSM6DSOW開發(5)----MotionFX庫解析空間坐標
陀螺儀LSM6DSV16X與AI集成(11)----融合磁力計進行姿態解算
請問有和ADIS16488性能差不多，包含加速度計，陀螺儀，磁力計，壓力計，同時陀螺儀量程大于450dps的產品嗎？
LSM9DS0磁力計出現故障
LSM9DS0陀螺儀和磁力計有哪些？
請問通常使用的磁力計、加速度計、陀螺儀等傳感器在安裝之前為什么要先校準？
MicroStrain 3DM-GX5-25陀螺儀
使用LSM9DS1的應用程序，磁力計結果不穩定怎么解決？
lsm6dsox的陀螺儀數據有很多尖峰是什么原因？如何解決？
IMU中三軸加速計、陀螺儀和磁力計的工作原理
淺析IMU中加速度計、陀螺儀、磁力計原理
工商網監
評論