极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 98660|回复: 57

向电子罗盘进发[未完]

  [复制链接]
发表于 2013-7-27 07:27:44 | 显示全部楼层 |阅读模式
本帖最后由 arduino_ykk 于 2013-7-27 21:13 编辑

【声明】 电子罗盘对我们这种东西南北都分不清楚的小白来说,真是吸引力无穷啊~ 特别是现在手机导航仪里面都有这么个东西,心痒难耐,也想自己来弄下。 但是网络上光有公式,没有推导。于是很闲的我们,就把羞答答的她,推倒了。。 以下部分公式来自网络,部分来自推导;图片大部分来自网络,少部分自己绘制。


【材料】:
arduino 板一块
HMC5883 - 三轴方位传感器
MPU6050 - 六轴加速度传感器
面包板,线材若干


(一) 什么是磁偏角和方位公式



磁偏角,即现实中指北针指向的磁极(地磁极)和我们地图上标注的传统意义的磁极是不重合的,有一个夹角。 你看地球仪上的地球也不是歪脖子转动么,就是这个意思。

下面是几何时间,如何定义方位。这里采用了反正切角的表达方式。





为了做这个电子罗盘,反正切角(Atan2)这种不知哪个星球来的东西都学习了。
π是180度,2π是360度,1/2π是90度... 其实这个反正切角就是射线(x,y)和x轴正向间的夹角。没什么好神秘的。如果y<0,射线在x轴下方,那么角度从x轴正向顺时针旋转,而且是负值;如果y>0,角度从x轴正向逆时针旋转。大量图示和公式来自基维百科。

几个特殊点的取值。
(0,0)夹角为0
(0, 1)对于的复平面夹角为π/2,
(&#8722;1, 0)对于复平面的夹角为π,
(0, &#8722;1)对于复平面的夹角为3π/2,

下面看下atan2和我们一般atan的区别。三维图,找到x,y轴就省事了。其实只是个计量方法的不同,不晓得为什么要弄得这么复杂的样子。





****
看完了这些基础知识,我们来分析honeywell的HMC5883三轴数位罗盘对角度的计算公式。

angle= atan2(y,x) * (180 / 3.14159265) + 180;

其实就是这样的:
角度 = atan2(y,x) * (180 / π) + 180,这里角度用0~360°表示

因为atan2(y,x) 算出来的会是多少π,所以后面用了个 180/π来换算成度数(0~360°)。但是,但是后面那个 +180是干嘛?

如果是60°的角,+180就变成了240°,从A点变成了B点变成了下面这个图。



仔细想想公式,因为这种反正切角的范围是从-π到π (-180°~180°), 而我们通常读出的指南针角度是0°~360°,所以需要把这个范围进行平移180°,到指南针的角度。但是这样一来,原来60°的角度变成了240°,也就是,整个象限沿着虚线被"镜像"了一下。


下图x轴要变成"南“的位置,好吧,这样就正常了。



于是我们得到了下面这个方位判断:

    if((angle < 22.5) || (angle > 337.5 ))
        Serial.print("South");
    if((angle > 22.5) && (angle < 67.5 ))
        Serial.print("South-West");
    if((angle > 67.5) && (angle < 112.5 ))
        Serial.print("West");
    if((angle > 112.5) && (angle < 157.5 ))
        Serial.print("North-West");
    if((angle > 157.5) && (angle < 202.5 ))
        Serial.print("North");
    if((angle > 202.5) && (angle < 247.5 ))
        Serial.print("NorthEast");
    if((angle > 247.5) && (angle < 292.5 ))
        Serial.print("East");
    if((angle > 292.5) && (angle < 337.5 ))
        Serial.print("SouthEast");

假设我们是60°角,也就是指北针正北顺时针转60度位置,应该在NE(东北)位置,套用上面的公式,60 + 180 = 240°,然后套用上面的条件判断:angle > 202.5° 并且angle < 247.5°,处于NorthEast (东北)位置,正确。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

 楼主| 发表于 2013-7-27 07:37:44 | 显示全部楼层
本帖最后由 arduino_ykk 于 2013-7-27 08:08 编辑

(二)
第二集来看看HMC5883怎么读取x,y轴的角度(也就是磁极的方位)。
< 插播广告 > 推荐读物:honeywell HMC5883文档,看寄存器页




首先是三轴方位传感器连接arduino板的图,可以看出非常简单。除了接Vcc=3.3V,接地外,连接数据SDA到模拟A4口,时间同步SLC到模拟A5口。好了,很简单。我的还有个DRDY(reset)接口,不连接了。



HMC5883是如何驱动的呢? 它采用了I2C的接口标准,里面内置了几个寄存器,用来存放数据和运行模式,所以调用上稍微麻烦一点点。



要调用HMC5883,就要记住几个寄存器(也就是临时小内存,黄色标注)- 图中寄存器B(0x01, 存放测量量程), 模式寄存器(0x02, 测量是单次,还是连续),以及数据寄存器X~Z(0x03~0x08)。使用时规定好量程(多多少大小的高斯,因为可以测量强或弱磁场),规定好连续测量,然后读取数据就是了。

如何规定量程和测量模式呢,如下。
写入寄存器B : send 0x3C 0x01 0x40  表示:0x3c - 写入  0x01 - 寄存器b的地址  0x40 - 数值,也就是B0100 0000.

[arduino代码]
    Wire.beginTransmission(0x1E); - 数据存放地址
    Wire.write(0x01);
    Wire.write(0x40);
    Wire.endTransmission();

0x40这个数值,可以从寄存器B的定义值看出。0x40就是二进制 0100 0000



0100套入二进制高位,查表可知,量程为+/- 1.9Ga



好了,要连续测量,也照猫画虎写代码,对模式寄存器:
Wire.beginTransmission(0x1E);
Wire.write(0x02);
Wire.write(0x00);  - 连续测量
Wire.endTransmission();

接下来要读取x,y,z三个轴上面的数据了。x,y,z轴的寄存器每个有2个字节可用。一个高字节,一个低字节。获取后要自己做合并操作。

因为其实它的指针,每读取一个寄存器,会自动指向下一个寄存器。所以,如果之前读取了模式寄存器,指针下移,下一个就会是xyz轴的数据寄存器了。所以可以直接读取内容。

Wire.requestFrom(0x1E, 6) -> 读取接下来6个字节(刚好就是xyz轴的寄存器,每个寄存器2个字节)。0x1E是这个设备的指针(HMC5883)。

把读到的数据存放到我们设的x,y,z变量里面去。
x= Wire.read() <<8; // x轴的高位寄存器 X msb
x |= Wire.read();  // x轴的低位寄存器 X lsb
z...
y...

x,y,z都读出来,就要请出我们注明的角度公式了:
angle = atan2( y, x ) * 180/3.1415926 + 180

之后再判断角度。这里不赘述。

下面是代码.
[arduino 1.0.5编译通过]


  1. // megnatometer

  2. #include <Wire.h>

  3. // define address of register B, mode register, and the value
  4. #define HMC5883_WriteAddress 0x1E
  5. #define HMC5883_RegisterB 0x01
  6. #define HMC5883_ModeRegister 0x02
  7. #define HMC5883_ContinusMode 0x00
  8. #define HMC5883_DataOutputAddress 0x03
  9. #define HMC5883_MeasureScale 0x40

  10. int x, y, z;
  11. double angle;

  12. void setup()
  13. {
  14.   Wire.begin();  // initialize Wire library
  15.   
  16.   // setup measurement scale
  17.   Wire.beginTransmission( HMC5883_WriteAddress );
  18.   Wire.write( HMC5883_RegisterB);
  19.   Wire.write( HMC5883_MeasureScale );
  20.   Wire.endTransmission();
  21.   
  22.   // setup measure mode
  23.   Wire.beginTransmission( HMC5883_WriteAddress );
  24.   Wire.write( HMC5883_ModeRegister );
  25.   Wire.write( HMC5883_ContinusMode );
  26.   Wire.endTransmission();
  27.   
  28.   Serial.begin( 9600 );
  29. }

  30. // begin measurement
  31. void loop()
  32. {
  33.   Wire.requestFrom( HMC5883_WriteAddress, 6 ); // get 6 bytes(xyz register)
  34.   while( 6 <= Wire.available() )
  35.   {
  36.     x = Wire.read() << 8 ; // get axis X msb
  37.     x |= Wire.read();  // get axis X lsb
  38.     z = Wire.read() << 8; // get axis Z msb
  39.     z |= Wire.read();   // get axis Z lsb
  40.     y = Wire.read();    // get axis Y msb
  41.     y |= Wire.read();   // get axis Y lsb
  42.   }
  43.   
  44.   // get its angle, axis Z is not in use, as right now we dont' have direction Z compensation.
  45.   angle = atan2( (double)y, (double)x ) * ( 180 / 3.1415926 ) + 180;
  46.   
  47.   // get its direction.
  48.   if ( (angle < 22.5) || (angle > 337.5) )
  49.     Serial.print( "South" );
  50.   if ( (angle > 22.5) && (angle < 67.5) )
  51.     Serial.print( "South-West" );
  52.   if ( (angle > 67.5) && (angle < 112.5) )
  53.     Serial.print( "West" );
  54.   if ( (angle > 112.5) && (angle < 157.5) )
  55.     Serial.print( "North-West" );
  56.   if ( (angle > 157.5) && (angle < 202.5) )
  57.     Serial.print( "North" );
  58.   if ( (angle > 202.5) && (angle < 247.5) )
  59.     Serial.print( "North-East" );
  60.   if ( (angle > 247.5) && (angle < 292.5) )
  61.     Serial.print( "East" );
  62.   if ( (angle > 292.5) && (angle < 337.5) )
  63.     Serial.print( "South-East" );
  64.   
  65.   Serial.print(": Angle between X-axis and the South direction ");
  66.    if ( angle > 180 )
  67.         angle=360-angle;
  68.    
  69.   Serial.print(angle,2);
  70.   Serial.println(" Deg");
  71.   
  72.   // to return the pointer to xyz register
  73.   Wire.beginTransmission( HMC5883_WriteAddress );
  74.   Wire.write( HMC5883_DataOutputAddress );
  75.   Wire.endTransmission();
  76.   
  77.   
  78.   delay(1000);
  79. }
复制代码

=================================================

下课前提问: 为啥 z轴的值没有用到公式里?
回答:现在这样测量的方位,必须是传感器水平放置才会准确。如果传感器有个倾角(现实生活中很常见),那么测量的值就会有偏差。z轴(垂直轴)的值需要结合三轴加速度传感器来用,做一个倾斜补偿。至于三轴加速度传感器。。。还在研究中。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-7-27 07:47:53 | 显示全部楼层
本帖最后由 arduino_ykk 于 2013-7-27 08:10 编辑

(三)

倾斜补偿。mpu6050六轴加速度传感器

想要让电子罗盘在任何角度都能准确测量方位,倾斜补偿是过不去的坎。通过三轴加速度传感器感知的xyz三轴上的倾角和转角,来对电子罗盘测量磁场的准确度进行修正。

下面就是让我大脑空白的倾斜补偿公式。x轴上的倾斜补偿想了一整天,终于弄明白意思。y轴上,到现在还no idea... 不得不说几何学这门学的最差的学科,60分不是白拿的。

Xh = XM * cos(Pitch) + ZM * sin(Pitch)
Yh = XM * sin(Roll) * sin(Pitch) + YM * cos(Roll) - ZM * sin(Roll) * cos(Pitch)

其中,XM,YM,ZM是电子罗盘在一定俯仰角( pitch angle )和转动角( roll angle )后x,y,z轴上测得的数据。Xh, Yh是补偿后在水平面上X轴,Y轴的数据,就好像电子罗盘在水平时测得的数据一样。



<06/20>
今天干活无聊的时候,在纸上写写画画,把补偿公式推导出来了。



对于Y轴,
Zm, Ym平面围绕Xm轴旋转,Zm在Y轴上始终无分量。(Xm处于XZ面内,与Y轴垂直)
所以:Yh = Ym * cos Roll + Zm * Sin Roll

对于x轴,其受到Xm, Ym, Zm三力共同作用。
Xh = Xm * cos Pitch + (- Zm * cos Roll * sin Pitch) + Ym * sin Roll * sin Pitch
   = Xm * cos Pitch - Zm * cos Roll * sin Pitch + Ym * sin Roll * sin Pitch

Zm, Ym都要先投影到XZ截面上再做分量处理。所以Zm在ZX轴上投影:Zm * cos Roll; Ym在ZX轴上投影:Ym * sin Roll.

推导出来的公式,和网站上提供的公式不同?! 不管了, 实际上去测试一下再说. 时间长了电路怎么接都忘了.

那么角度 angle = arctan(Yh / Xh)

于是我要开始焊电路了。买的传感器板居然没有焊插头。
摆个造型



两个传感器套头的样子



arduino主板已经等不及要接上传感器了,官人我要



电工无力者路过。 实在是对焊接排插无能。焊的丑也没办法了。



六轴加速度传感器的插头,焊接的稍微好看点,其中有一个脚居然脱焊。。 = = 算了,焊接也只是起一个固定作用。以后还是要弄下来的。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-7-27 07:51:48 | 显示全部楼层
未完。。。。。。占坑。。。。
回复 支持 反对

使用道具 举报

发表于 2013-7-27 10:49:51 | 显示全部楼层
本帖最后由 wing 于 2013-7-27 10:51 编辑

这些数学问题我一直不清晰,这些很好的资料,要慢慢研究
如果是原创的话绝对的精帖
另外请尽快补完,后面的内容很吸引
回复 支持 反对

使用道具 举报

发表于 2013-7-27 11:54:26 | 显示全部楼层
我也期待后面的6050部分
回复 支持 反对

使用道具 举报

发表于 2013-7-27 12:47:11 | 显示全部楼层
楼主好牛,电子罗盘我还没有研究过,下午有空仔细看看...
回复 支持 反对

使用道具 举报

发表于 2013-7-27 16:36:30 | 显示全部楼层
焊接的很霸气
回复 支持 反对

使用道具 举报

发表于 2013-7-27 16:38:07 | 显示全部楼层
数学。。。楼主威武
回复 支持 反对

使用道具 举报

发表于 2013-7-27 20:42:52 | 显示全部楼层
分析计算 很牛X

焊接就不行了。。。。。

烙铁烧热  顶在引脚和焊盘之间  焊锡丝往上一送  一个圆润的焊点就出来了
回复 支持 反对

使用道具 举报

 楼主| 发表于 2013-7-27 21:17:10 | 显示全部楼层
ro0t 发表于 2013-7-27 20:42
分析计算 很牛X

焊接就不行了。。。。。

谢谢。。。实在是怕烙铁烧坏电路板。。很久不捉刀,捉着电烙铁手就打颤。
回复 支持 反对

使用道具 举报

发表于 2013-7-28 12:04:35 | 显示全部楼层
楼主:5883使用是需要经过校正的,我更想知道楼主的校正方法......
回复 支持 反对

使用道具 举报

发表于 2013-7-28 12:54:18 | 显示全部楼层
楼主数学强人啊,小弟佩服,也在此谢了。
回复 支持 反对

使用道具 举报

发表于 2013-7-28 23:26:09 | 显示全部楼层
坐等更新                 
回复 支持 反对

使用道具 举报

发表于 2013-7-29 07:57:24 | 显示全部楼层
arduino_ykk 发表于 2013-7-27 21:17
谢谢。。。实在是怕烙铁烧坏电路板。。很久不捉刀,捉着电烙铁手就打颤。

手打颤 最好治了

白酒 喝上个三四两 基本都能好

酒量大的可以多喝一点
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 20:28 , Processed in 0.047740 second(s), 27 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表