概述

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

最近在弄ST和瑞薩RA的課程，需要樣片的可以加群申請：615061293 。

使用硬件

視頻教學

[https://www.bilibili.com/video/BV1zi4y1q7Sr/]

樣品申請

[https://www.wjx.top/vm/OhcKxJk.aspx#]

源碼下載

[https://download.csdn.net/download/qq_24312945/88722925]

環境磁場建模

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

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

消除硬鐵誤差

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

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

軟鐵干擾

軟鐵干擾來源于地球磁場和羅盤附近的任何磁性材料之間的相互作用，同硬鐵材料一樣，軟金屬也干擾地球的磁力線，不同點是，軟磁的干擾程度，與羅盤的方向有關。對軟鐵干擾的校正，比較復雜，下面討論采用霍尼韋爾公司的 Michal.J.Caruso 提出橢圓假設的誤差補償原理進行誤差補償的方法 。

根據 Michal.J.Caruso 的研究，羅盤在理想的沒有任何干擾的磁場水平面里作圓形旋轉時，磁力計的顯示應該呈現上圖的狀態，其中圓中心在 0,0 點處，每個計數代表 67微高斯，在 X 和 Y 平面中的地球磁場強度值讀到 2800 個計數，約為 190 毫高斯，根據下面公式可以對每個讀數確定一個方位角。

如果將磁力計安裝在有發動機或者其他鐵磁材料的環境中，圓形旋轉時，磁力計的顯示應該下圖的狀態。

這里的圖形不是一個圓 ( 有點橢圓 ) ，而它偏移 0,0 點為 -480 和 -795 個計數，這偏移和橢圓效應是干擾磁場對地球磁場作用的結果。通過確定兩個定標因數 Xsf 和 Ysf 可以將橢圓改為圓。隨后計算偏移值 Xoff 和 Yoff, 將圓中心定在 0,0 原點，用下面公式來計算 Y,X 值。

X 值 =Xsf×X 讀數 +Xoff
Y 值 =Ysf×Y 讀數 +Yoff

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

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

以較大的數值為準

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

以較大的數值為準

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

演示

主程序

在主程序中添加開機校準。

#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;//計算角度
                      printf("Angle_XY=%3.2fn",Angle_XY);
      //              Angle_XZ=atan2( (magnetic_mG[2]-Zoffset),(magnetic_mG[0]-Xoffset) ) * (180/3.14159265)+180;//計算角度
      //              Angle_YZ=atan2( (magnetic_mG[2]-Zoffset),(magnetic_mG[1]-Yoffset) ) * (180/3.14159265)+180;//計算角度
      //              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
}

審核編輯 黃宇