e2studio開發(fā)磁力計(jì)LIS2MDL(2)----電子羅盤

來源：嵌入式單片機(jī)MCU開發(fā)

概述

本文將介紹如何使用 LIS2MDL 傳感器來讀取數(shù)據(jù)來轉(zhuǎn)化為指南針。 地磁場(chǎng)強(qiáng)度范圍約為 23,000 至 66,000 nT ，并且可以建模為磁偶極子，其場(chǎng)線起源于地球地理南部附近的點(diǎn)，并終止于磁場(chǎng)附近的點(diǎn)。磁場(chǎng)具有七個(gè)分量，如圖 所示。x，y和z分別表示北分量，東分量和垂直分量的磁場(chǎng)強(qiáng)度。H代表總水平強(qiáng)度，F(xiàn)代表磁場(chǎng)的總強(qiáng)度，而D和I分別代表磁偏角和磁傾角。

使用硬件

環(huán)境磁場(chǎng)建模

盡管可以選擇七個(gè)不同的元素來處理給定點(diǎn)的磁場(chǎng)，但并非所有元素都是進(jìn)行定位的理想選擇。X，Y 和 Z 的三個(gè)元素是從磁力計(jì)獲得的基本值，而其他四個(gè)元素是使用這些元素來計(jì)算的。前三個(gè)元素隨移動(dòng)設(shè)備方位角的改變而偏離，因此，對(duì)于許多基于磁場(chǎng)的室內(nèi)定位系統(tǒng)通常假設(shè)以固定方位工作，行人可以更改方向，但不能更改設(shè)備方位。
在大地坐標(biāo)系的水平面上,假設(shè)磁北和x軸的夾角為a, x方向的磁分量是Mx,y方向的分量是My,則
ɑ=arctan(My/Mx)
這是電子羅盤定向的基本原理。實(shí)際應(yīng)用中,電子羅盤不能總是保持在水平面上，如下圖所示一樣存在俯仰角和橫滾角。將羅盤坐標(biāo)系下的 z軸向下, 3個(gè)軸的磁分量投影到水平面上可以得到Xh,Yh,相應(yīng)的磁感應(yīng)值:
Xh=Xcosф+Ysinфsinθ - Zsinфcosθ
Yh=Ycosθ + Zsinθ
相應(yīng)的
ɑ=arctan(Yh/Xh)

Angle_XY=atan2( (magnetic_mG[1]-Xoffset),(magnetic_mG[0]-Yoffset) ) * (180/3.14159265)+180;//計(jì)算角度 Angle_XZ=atan2( (magnetic_mG[2]-Zoffset),(magnetic_mG[0]-Yoffset) ) * (180/3.14159265)+180;//計(jì)算角度 Angle_YZ=atan2( (magnetic_mG[2]-Zoffset),(magnetic_mG[1]-Yoffset) ) * (180/3.14159265)+180;//計(jì)算角度

消除硬鐵誤差

電子羅盤有兩種工作模式，一種是正常工作模式，另一種是出廠設(shè)置模式，這種出場(chǎng)設(shè)置模式就是為了消除硬鐵干擾。硬鐵干擾產(chǎn)生于永久磁鐵，和被磁化的金屬，或羅盤平臺(tái)上的鋼。這些干擾會(huì)保持大小恒定，與羅盤的相對(duì)位置固定，而與羅盤指向無關(guān)。所以當(dāng)羅盤安裝好后，它周圍的硬鐵干擾就幾乎不會(huì)改變了，只要對(duì)羅盤做一次準(zhǔn)確的標(biāo)定，就能很輕松的消除這項(xiàng)干擾。
硬鐵干擾在羅盤輸出的每個(gè)軸向加了一個(gè)定值，輸出曲線圖的圓心被移動(dòng)了，對(duì)于航向的影響則是一個(gè)周期性的誤差，如下圖所示在理想狀態(tài)時(shí)，在 360 度范圍內(nèi)，傳感器輸出極值分別為 ymax ymin xmax xmin 坐標(biāo)原點(diǎn)為O，受到硬鐵干擾后，極值變?yōu)?y’max ,y’min , x’max , x’max , 坐標(biāo)原點(diǎn)變?yōu)镺’ 。要消除硬鐵干擾，可以將羅盤和平臺(tái)旋轉(zhuǎn)一周，得
到圓上的足夠的點(diǎn)再得到圓心偏移。

具體操作過程如下：接通電源后，將羅盤勻速旋轉(zhuǎn)，使微控制器采集 360 °范圍內(nèi)的數(shù)據(jù)，通過數(shù)值比較，找出 x 、 y 方向的極值，得出偏移坐標(biāo) O’, 即電橋的偏置電壓，并將此電壓值保存，每次羅盤讀數(shù)時(shí)都會(huì)減去此偏移。實(shí)際上，本設(shè)計(jì)在方位角的計(jì)算過程就是此過程，所以在計(jì)算方位角的同時(shí)已經(jīng)消除了硬鐵干擾。這種方法也可以消除由于溫度漂移產(chǎn)生的誤差。

軟鐵干擾

軟鐵干擾來源于地球磁場(chǎng)和羅盤附近的任何磁性材料之間的相互作用，同硬鐵材料一樣，軟金屬也干擾地球的磁力線，不同點(diǎn)是，軟磁的干擾程度，與羅盤的方向有關(guān)。對(duì)軟鐵干擾的校正，比較復(fù)雜，下面討論采用霍尼韋爾公司的 Michal.J.Caruso 提出橢圓假設(shè)的誤差補(bǔ)償原理進(jìn)行誤差補(bǔ)償?shù)姆椒?。

根據(jù) Michal.J.Caruso 的研究，羅盤在理想的沒有任何干擾的磁場(chǎng)水平面里作圓形旋轉(zhuǎn)時(shí)，磁力計(jì)的顯示應(yīng)該呈現(xiàn)上圖的狀態(tài)，其中圓中心在 0,0 點(diǎn)處，每個(gè)計(jì)數(shù)代表 67微高斯，在 X 和 Y 平面中的地球磁場(chǎng)強(qiáng)度值讀到 2800 個(gè)計(jì)數(shù)，約為 190 毫高斯，根據(jù)下面公式可以對(duì)每個(gè)讀數(shù)確定一個(gè)方位角。

如果將磁力計(jì)安裝在有發(fā)動(dòng)機(jī)或者其他鐵磁材料的環(huán)境中，圓形旋轉(zhuǎn)時(shí)，磁力計(jì)的顯示應(yīng)該下圖的狀態(tài)。

這里的圖形不是一個(gè)圓 ( 有點(diǎn)橢圓 ) ，而它偏移 0,0 點(diǎn)為 -480 和 -795 個(gè)計(jì)數(shù)，這偏移和橢圓效應(yīng)是干擾磁場(chǎng)對(duì)地球磁場(chǎng)作用的結(jié)果。通過確定兩個(gè)定標(biāo)因數(shù) Xsf 和 Ysf 可以將橢圓改為圓。隨后計(jì)算偏移值 Xoff 和 Yoff, 將圓中心定在 0,0 原點(diǎn)，用下面公式來計(jì)算 Y,X 值。

X 值 =Xsf×X 讀數(shù) +Xoff
Y 值 =Ysf×Y 讀數(shù) +Yoff

這里的定標(biāo)因數(shù) Xsf 和 Ysf 可由下述方法獲得。
①將羅盤在水平面做旋轉(zhuǎn)運(yùn)動(dòng)
②找出 X 和 Y 讀數(shù)的最大值和最小值
③用這四個(gè)數(shù)值確定 X 和 Y 定標(biāo)因數(shù) (Xsf ， Ysf) ，以及零偏移值 (Xoff ， Yoff)

Xsf=1 或 (Y 最大 -Y 最小 )/2(X 最大 -Y 最小 )

以較大的數(shù)值為準(zhǔn)

Ysf=1 或 (X 最大 -Y 最小 )/2(Y 最大 -Y 最小 )

以較大的數(shù)值為準(zhǔn)

Xoff=[(X 最大 -X 最小 ) /2-X 最大 ]×Xsf
Yoff=[(Y 最大 -Y 最小 ) /2-Y 最大 ]×Ysf

演示

主程序

在主程序中添加開機(jī)校準(zhǔn)。

#include "hal_data.h" #include < stdio.h > #include "lis2mdl_reg.h" fsp_err_t err = FSP_SUCCESS; volatile bool uart_send_complete_flag = false; void user_uart_callback (uart_callback_args_t * p_args) { if(p_args- >event == UART_EVENT_TX_COMPLETE) { uart_send_complete_flag = true; } } /* Callback function */ i2c_master_event_t i2c_event = I2C_MASTER_EVENT_ABORTED; uint32_t timeout_ms = 100000; void sci_i2c_master_callback(i2c_master_callback_args_t *p_args) { i2c_event = I2C_MASTER_EVENT_ABORTED; if (NULL != p_args) { /* capture callback event for validating the i2c transfer event*/ i2c_event = p_args- >event; } } #ifdef __GNUC__ //串口重定向 #define PUTCHAR_PROTOTYPE int __io_putchar(int ch) #else #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f) #endif PUTCHAR_PROTOTYPE { err = R_SCI_UART_Write(&g_uart9_ctrl, (uint8_t *)&ch, 1); if(FSP_SUCCESS != err) __BKPT(); while(uart_send_complete_flag == false){} uart_send_complete_flag = false; return ch; } int _write(int fd,char *pBuffer,int size) { for(int i=0;i< size;i++) { __io_putchar(*pBuffer++); } return size; } FSP_CPP_HEADER void R_BSP_WarmStart(bsp_warm_start_event_t event); FSP_CPP_FOOTER #define SENSOR_BUS g_i2c_master0_ctrl /* Private macro -------------------------------------------------------------*/ #define BOOT_TIME 20 //ms /* Private variables ---------------------------------------------------------*/ static int16_t data_raw_magnetic[3]; static int16_t data_raw_temperature; static float magnetic_mG[3]; static float temperature_degC; static uint8_t whoamI, rst; static uint8_t tx_buffer[1000]; /* Extern variables ----------------------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ /* * WARNING: * Functions declare in this section are defined at the end of this file * and are strictly related to the hardware platform used. * */ static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len); static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len); static void tx_com(uint8_t *tx_buffer, uint16_t len); static void platform_delay(uint32_t ms); static void platform_init(void); /*******************************************************************************************************************//** * main() is generated by the RA Configuration editor and is used to generate threads if an RTOS is used. This function * is called by main() when no RTOS is used. **********************************************************************************************************************/ void hal_entry(void) { /* TODO: add your own code here */ /* Open the transfer instance with initial configuration. */ err = R_SCI_UART_Open(&g_uart9_ctrl, &g_uart9_cfg); assert(FSP_SUCCESS == err); printf("hello world!n"); R_IOPORT_PinWrite(&g_ioport_ctrl, BSP_IO_PORT_00_PIN_14, BSP_IO_LEVEL_HIGH); /* Initialize the I2C module */ err = R_IIC_MASTER_Open(&g_i2c_master0_ctrl, &g_i2c_master0_cfg); /* Handle any errors. This function should be defined by the user. */ assert(FSP_SUCCESS == err); /* Initialize mems driver interface */ stmdev_ctx_t dev_ctx; dev_ctx.write_reg = platform_write; dev_ctx.read_reg = platform_read; dev_ctx.handle = &SENSOR_BUS; /* Wait sensor boot time */ platform_delay(BOOT_TIME); /* Check device ID */ lis2mdl_device_id_get(&dev_ctx, &whoamI); printf("LIS2MDL_ID=0x%x,whoamI=0x%xn",LIS2MDL_ID,whoamI); if (whoamI != LIS2MDL_ID) while (1) { /* manage here device not found */ } /* Restore default configuration */ lis2mdl_reset_set(&dev_ctx, PROPERTY_ENABLE); do { lis2mdl_reset_get(&dev_ctx, &rst); } while (rst); /* Enable Block Data Update */ lis2mdl_block_data_update_set(&dev_ctx, PROPERTY_ENABLE); /* Set Output Data Rate */ lis2mdl_data_rate_set(&dev_ctx, LIS2MDL_ODR_10Hz); /* Set / Reset sensor mode */ lis2mdl_set_rst_mode_set(&dev_ctx, LIS2MDL_SENS_OFF_CANC_EVERY_ODR); /* Enable temperature compensation */ lis2mdl_offset_temp_comp_set(&dev_ctx, PROPERTY_ENABLE); /* Set device in continuous mode */ lis2mdl_operating_mode_set(&dev_ctx, LIS2MDL_CONTINUOUS_MODE); int Xmax=0,Xmin=0; int Ymax=0,Ymin=0; int Zmax=0,Zmin=0; int i=0; int Xoffset,Yoffset,Zoffset; float angle; static float Angle_XY; static float Angle_XZ; static float Angle_YZ; static float Xsf; static float Ysf; static float Zsf; while (1) { uint8_t reg; /* Read output only if new value is available */ lis2mdl_mag_data_ready_get(&dev_ctx, ?); if (reg) { /* Read magnetic field data */ memset(data_raw_magnetic, 0x00, 3 * sizeof(int16_t)); lis2mdl_magnetic_raw_get(&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]); // printf("Magnetic field [mG]:%4.2ft%4.2ft%4.2frn",magnetic_mG[0], magnetic_mG[1], magnetic_mG[2]); // /* Read temperature data */ // memset(&data_raw_temperature, 0x00, sizeof(int16_t)); // lis2mdl_temperature_raw_get(&dev_ctx, &data_raw_temperature); // temperature_degC = lis2mdl_from_lsb_to_celsius(data_raw_temperature); // printf("Temperature [degC]:%6.2frn",temperature_degC); printf("i=%d,Magnetic field [mG]:%4.2ft%4.2ft%4.2frn",i,magnetic_mG[0], magnetic_mG[1], magnetic_mG[2]); if(i< 500) { i++; if(magnetic_mG[0]< Xmin) Xmin=magnetic_mG[0]; else if(magnetic_mG[0] >Xmax) Xmax=magnetic_mG[0]; if(magnetic_mG[1]< Ymin) Ymin=magnetic_mG[1]; else if(magnetic_mG[1] >Ymax) Ymax=magnetic_mG[1]; if(magnetic_mG[2]< Zmin) Zmin=magnetic_mG[2]; else if(magnetic_mG[2] >Zmax) Zmax=magnetic_mG[2]; } else if(i==500) { i++; Xsf = (Ymax - Ymin) / (Xmax - Xmin); Ysf = (Xmax - Xmin) / (Ymax - Ymin); if (Xsf < 1) Xsf = 1; if (Ysf < 1) Ysf = 1; Xoffset=( (Xmax-Xmin)/2 - Xmax) *Xsf; Yoffset=( (Ymax-Ymin)/2 - Ymax) *Ysf; // Zoffset=( (Zmax-Zmin)/2 - Zmax) *Xsf; } else { Angle_XY=atan2( (magnetic_mG[1]-Yoffset),(magnetic_mG[0]-Xoffset) ) * (180/3.14159265)+180;//計(jì)算角度 printf("Angle_XY=%3.2fn",Angle_XY); // Angle_XZ=atan2( (magnetic_mG[2]-Zoffset),(magnetic_mG[0]-Xoffset) ) * (180/3.14159265)+180;//計(jì)算角度 // Angle_YZ=atan2( (magnetic_mG[2]-Zoffset),(magnetic_mG[1]-Yoffset) ) * (180/3.14159265)+180;//計(jì)算角度 // printf("Angle_XY=%3.2f,Angle_XZ=%3.2f,Angle_YZ=%3.2fn",Angle_XY,Angle_XZ,Angle_YZ); } } R_BSP_SoftwareDelay(10, BSP_DELAY_UNITS_MILLISECONDS); } #if BSP_TZ_SECURE_BUILD /* Enter non-secure code */ R_BSP_NonSecureEnter(); #endif }