向电子罗盘进发[未完]
本帖最后由 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，
(−1, 0)对于复平面的夹角为π，
(0, −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 (东北)位置，正确。 本帖最后由 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.
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
之后再判断角度。这里不赘述。
下面是代码.
// megnatometer
#include <Wire.h>
// define address of register B, mode register, and the value
#define HMC5883_WriteAddress 0x1E
#define HMC5883_RegisterB 0x01
#define HMC5883_ModeRegister 0x02
#define HMC5883_ContinusMode 0x00
#define HMC5883_DataOutputAddress 0x03
#define HMC5883_MeasureScale 0x40
int x, y, z;
double angle;
void setup()
{
Wire.begin();// initialize Wire library
// setup measurement scale
Wire.beginTransmission( HMC5883_WriteAddress );
Wire.write( HMC5883_RegisterB);
Wire.write( HMC5883_MeasureScale );
Wire.endTransmission();
// setup measure mode
Wire.beginTransmission( HMC5883_WriteAddress );
Wire.write( HMC5883_ModeRegister );
Wire.write( HMC5883_ContinusMode );
Wire.endTransmission();
Serial.begin( 9600 );
}
// begin measurement
void loop()
{
Wire.requestFrom( HMC5883_WriteAddress, 6 ); // get 6 bytes(xyz register)
while( 6 <= Wire.available() )
{
x = Wire.read() << 8 ; // get axis X msb
x |= Wire.read();// get axis X lsb
z = Wire.read() << 8; // get axis Z msb
z |= Wire.read(); // get axis Z lsb
y = Wire.read(); // get axis Y msb
y |= Wire.read(); // get axis Y lsb
}
// get its angle, axis Z is not in use, as right now we dont' have direction Z compensation.
angle = atan2( (double)y, (double)x ) * ( 180 / 3.1415926 ) + 180;
// get its direction.
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( "North-East" );
if ( (angle > 247.5) && (angle < 292.5) )
Serial.print( "East" );
if ( (angle > 292.5) && (angle < 337.5) )
Serial.print( "South-East" );
Serial.print(": Angle between X-axis and the South direction ");
if ( angle > 180 )
angle=360-angle;
Serial.print(angle,2);
Serial.println(" Deg");
// to return the pointer to xyz register
Wire.beginTransmission( HMC5883_WriteAddress );
Wire.write( HMC5883_DataOutputAddress );
Wire.endTransmission();
delay(1000);
}
=================================================
下课前提问： 为啥 z轴的值没有用到公式里？
回答：现在这样测量的方位，必须是传感器水平放置才会准确。如果传感器有个倾角（现实生活中很常见），那么测量的值就会有偏差。z轴（垂直轴）的值需要结合三轴加速度传感器来用，做一个倾斜补偿。至于三轴加速度传感器。。。还在研究中。 本帖最后由 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主板已经等不及要接上传感器了，官人我要
电工无力者路过。 实在是对焊接排插无能。焊的丑也没办法了。
六轴加速度传感器的插头，焊接的稍微好看点，其中有一个脚居然脱焊。。 = = 算了，焊接也只是起一个固定作用。以后还是要弄下来的。
未完。。。。。。占坑。。。。 本帖最后由 wing 于 2013-7-27 10:51 编辑
这些数学问题我一直不清晰，这些很好的资料，要慢慢研究
如果是原创的话绝对的精帖
另外请尽快补完,后面的内容很吸引 我也期待后面的6050部分 楼主好牛，电子罗盘我还没有研究过，下午有空仔细看看... 焊接的很霸气 数学。。。楼主威武 分析计算 很牛X
焊接就不行了。。。。。
烙铁烧热顶在引脚和焊盘之间焊锡丝往上一送一个圆润的焊点就出来了 ro0t 发表于 2013-7-27 20:42 static/image/common/back.gif
分析计算 很牛X
焊接就不行了。。。。。
谢谢。。。实在是怕烙铁烧坏电路板。。很久不捉刀，捉着电烙铁手就打颤。 楼主:5883使用是需要经过校正的,我更想知道楼主的校正方法...... 楼主数学强人啊，小弟佩服，也在此谢了。 坐等更新 arduino_ykk 发表于 2013-7-27 21:17 static/image/common/back.gif
谢谢。。。实在是怕烙铁烧坏电路板。。很久不捉刀，捉着电烙铁手就打颤。
手打颤 最好治了
白酒 喝上个三四两 基本都能好
酒量大的可以多喝一点