【ROHM传感器套件试用】十一、方位校准及获取

踏平幼稚园

说明：测试程序使用的开发板为nucleo-f411re，程序下载链接：【ROHM传感器套件试用】二、stm32代码移植(所有模块通过测试)

本贴使用模块：BM1422（地磁传感器）

一、增加校准及方位计算功能
    1、本次校准采用模块水平旋转，通过采集X，Y的最小最大值，然后将（最小值+最大值）/2作为偏移值。

    2、代码修改
        a、增加校准函数（offset[3]为x,y,z偏移值，offset_updated为偏移量校准完成标记，以上变量自己添加为BM1422类的私有变量）

1. void BM1422::calibrate(void)
   
2. {
   
3.   float x_min = 0, x_max = 0, y_min = 0, y_max = 0;
   
4.   uint8_t update = 0;
   
5.   uint32_t delay_3s = HAL_GetTick();
   
6.   
   
7.   offset_updated = 0;
   
8.   
   
9.   while(HAL_GetTick() - delay_3s < 5000) {
   
10.     get_val(offset);
    
11.     if(x_min > offset[0])
    
12.     {
    
13.       x_min = offset[0];
    
14.       update = 1;
    
15.     }   
    
16.     if(x_max < offset[0])
    
17.     {
    
18.       x_max = offset[0];
    
19.       update = 1;
    
20.     }
    
21.     if(y_min > offset[1])
    
22.     {
    
23.       y_min = offset[1];
    
24.       update = 1;
    
25.     }
    
26.     if(y_max < offset[1])
    
27.     {
    
28.       y_max = offset[1];
    
29.       update = 1;
    
30.     }
    
31.     if(update == 1)
    
32.     {
    
33.       update = 0;
    
34.       delay_3s = HAL_GetTick();
    
35.     }
    
36.   }
    
37.   
    
38.   offset_updated = 1;
    
39.   offset[0] = (x_min + x_max) / 2;
    
40.   offset[1] = (y_min + y_max) / 2;
    
41. }

复制代码

b、修改BM1422::get_val函数，加入偏移计算

1. uint8_t BM1422::get_val(float *data)
   
2. {
   
3.   uint8_t rc;
   
4.   unsigned char val[6];
   
5.   signed short mag[3];
   
6. 
   
7.   rc = get_rawval(val);
   
8.   if (rc != 0) {
   
9.     return (rc);
   
10.   }
    
11. 
    
12.   mag[0] = ((signed short)val[1] << 8) | (val[0]);
    
13.   mag[1] = ((signed short)val[3] << 8) | (val[2]);
    
14.   mag[2] = ((signed short)val[5] << 8) | (val[4]);
    
15. 
    
16.   convert_uT(mag, data);
    
17. 
    
18.   if(offset_updated == 1)
    
19.   {
    
20.     data[0] -= offset[0];
    
21.     data[1] -= offset[1];
    
22.   }
    
23.   
    
24.   return (rc);  
    
25. }

复制代码

c、main.c文件bm1422初始化部分添加偏移量计算函数

1. #ifdef BM1422_EN
   
2.   bm1422.init();
   
3.   bm1422.isr_func(2, bm1422_isr);
   
4.   bm1422.calibrate();
   
5. #endif

复制代码

d、main.c文件增加方位计算及方位打印

1. #ifdef BM1422_EN
   
2.     bm1422.get_val(mag);
   
3.     angle = atan2(mag[1], mag[0]) * (180 / 3.14159265) + 180;
   
4.     Serial.println("------ BM1422 [uT] ------");
   
5.     Serial.print("X = ");
   
6.     Serial.print(mag[0]);
   
7.     Serial.print(", Y = ");
   
8.     Serial.print(mag[1]);
   
9.     Serial.print(", Z = ");
   
10.     Serial.print(mag[2]);
    
11.     Serial.print(", Angle = ");
    
12.     Serial.print(angle);
    
13.     Serial.println();
    
14.     Serial.println();
    
15.   #endif

复制代码

e、添加数学库，添加方法自行百度，只用到atan2函数

二、运行程序并立即水平旋转模块，直到串口打印出结果
    1、模块水平放置，模块X正坐标分别指向东、南、西、北四个方位
        

    2、X正坐标指向东
        

    3、X正坐标指向南
        
  
    4、X正坐标指向西
        

    5、X正坐标指向北
        

小结：
    从结果看四个方位相差接近90度，x正坐标指向西时方位接近0,顺时针旋转方位角度增加