hmc5883树莓派
目录
1 说明
2 QMC5883校准
3 QMC5883源码
4 HMC5883
1说明
国产qmc5883的地址是0x0d，hmc5883的地址是0x1e,注意区分，GY273模块标的是HMC5883实际上通过Linux下去读的是0X0D.
2 QMC5883校准
使用linux下i2c读取qmc5883的程序并将数据保存到文本中。
“./qmc > 1.csv”
使用excel打开，并画图
蓝色圈是x,y轴围城的图形，很显然坐标原点不在中间，所以需要校准。经过一番周折，发现校准是个体力活啊，经过多次校准，依然达不到理想的圆形。
环境有很大的干扰，明明转动90度，却出现120度的变化。每次校准获取的偏移量和比例数都不尽相同。
利用SMBUS的程序读取。
qmc5883.c如下
/*
===============================================================================================================
QMC5883LCompass.h
Library for using QMC5583L series chip boards as a compass.
Learn more at [https://github.com/mprograms/QMC5883LCompass]
Supports:
- Getting values of XYZ axis.
- Calculating Azimuth.
- Getting 16 point Azimuth bearing direction (0 - 15).
- Getting 16 point Azimuth bearing Names (N, NNE, NE, ENE, E, ESE, SE, SSE, S, SSW, SW, WSW, W, WNW, NW, NNW)
- Smoothing of XYZ readings via rolling averaging and min / max removal.
- Optional chipset modes (see below)
===============================================================================================================
v1.0 - June 13, 2019
Written by MRPrograms
Github: [https://github.com/mprograms/]
Release under the GNU General Public License v3
[https://www.gnu.org/licenses/gpl-3.0.en.html]
===============================================================================================================
FROM QST QMC5883L Datasheet [https://nettigo.pl/attachments/440]
-----------------------------------------------
MODE CONTROL (MODE)
Standby 0x00
Continuous 0x01
OUTPUT DATA RATE (ODR)
10Hz 0x00
50Hz 0x04
100Hz 0x08
200Hz 0x0C
FULL SCALE (RNG)
2G 0x00
8G 0x10
OVER SAMPLE RATIO (OSR)
512 0x00
256 0x40
128 0x80
64 0xC0
*/
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include<unistd.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <stdint.h>
#include <err.h>
#include <errno.h>
#include <linux/types.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <time.h>
#include <sys/time.h>
#define CHIP "/dev/i2c-3"
#define CHIP_ADDR 0x0d
#define MagnetcDeclination 7.43 //笔者所在地磁偏角，请根据情况自行百度
#define CalThreshold 0
int offsetX,offsetY,offsetZ;
int _vRaw[3] = {0,0,0};
int _vHistory[10][3];
int _vScan = 0;
long _vTotals[3] = {0,0,0};
int _vSmooth[3] = {0,0,0};
void _smoothing();
char _calibrationUse = 0;
int _vCalibration[3][2];
int _vCalibrated[3];
char _smoothUse = 0;
char _smoothSteps = 5;
char _smoothAdvanced = 0;
const char _bearings[16][3] = {
{' ', ' ', 'N'},
{'N', 'N', 'E'},
{' ', 'N', 'E'},
{'E', 'N', 'E'},
{' ', ' ', 'E'},
{'E', 'S', 'E'},
{' ', 'S', 'E'},
{'S', 'S', 'E'},
{' ', ' ', 'S'},
{'S', 'S', 'W'},
{' ', 'S', 'W'},
{'W', 'S', 'W'},
{' ', ' ', 'W'},
{'W', 'N', 'W'},
{' ', 'N', 'W'},
{'N', 'N', 'W'},
};
int i2c_fd;
static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command,
int size, union i2c_smbus_data *data)
{
struct i2c_smbus_ioctl_data args;
args.read_write = read_write;
args.command = command;
args.size = size;
args.data = data;
return ioctl(file,I2C_SMBUS,&args);
}
static inline __s32 i2c_smbus_write_quick(int file, __u8 value)
{
return i2c_smbus_access(file,value,0,I2C_SMBUS_QUICK,(union i2c_smbus_data *)0);
}
static inline __s32 i2c_smbus_write_byte(int file, __u8 value)
{
return i2c_smbus_access(file,I2C_SMBUS_WRITE,value,
I2C_SMBUS_BYTE,(union i2c_smbus_data *)0);
}
static inline __s32 i2c_smbus_write_byte_data(int file, __u8 command,
__u8 value)
{
union i2c_smbus_data data;
data.byte = value;
// printf("before write_byte: %#x\n", data.byte);
int ret = i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
I2C_SMBUS_BYTE_DATA, &data);
// printf("write_byte: %#x\n", data.byte);
return ret;
}
static inline __s32 i2c_smbus_read_word_data(int file, __u8 command)
{
union i2c_smbus_data data;
if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
I2C_SMBUS_WORD_DATA,&data))
return -1;
else
return 0x0FFFF & data.word;
}
static inline __s32 i2c_smbus_read_byte_data(int file, __u8 command)
{
union i2c_smbus_data data;
if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
I2C_SMBUS_BYTE_DATA,&data))
return -1;
else
return 0x0FF & data.byte;
}
void _writeReg(char r, char v){
i2c_smbus_write_byte_data(i2c_fd, r,v);
}
void setMode(char mode, char odr, char rng, char osr){
_writeReg(0x09,mode|odr|rng|osr);
}
/*******************************************************************************
* function name : Hmc5883l_Init
* description : 完成初始
* param[in] : none
* param[out] : none
* return : 0
*******************************************************************************/
void qmc5883l_Init()
{
//初始化串口和i2c
_writeReg(0x0B,0x01);
setMode(0x01,0x0C,0x10,0X00);
//calibrateMag();
}
// Reset the chip
void setReset(){
_writeReg(0x0A,0x80);
}
//从i2c读取数据
static int i2c_read_data(int file,int daddr,unsigned char *read_data)
{
int res;
res = i2c_smbus_read_byte_data(file, daddr);
if (res < 0) {
fprintf(stderr, "Error: Read failed\n");
return -1;
}
*read_data = res;
//printf("read data -- 0x%x\n",*read_data);
return 0;
}
static inline __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command,
__u8 length, __u8 *values)
{
union i2c_smbus_data data;
int i;
if (length > 32)
length = 32;
data.block[0] = length;
if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
length == 32 ? I2C_SMBUS_I2C_BLOCK_BROKEN :
I2C_SMBUS_I2C_BLOCK_DATA,&data))
return -1;
else {
for (i = 1; i <= data.block[0]; i++)
values[i-1] = data.block[i];
return data.block[0];
}
}
unsigned long millis()
{
struct timeval tv;
gettimeofday(&tv, 0);
unsigned long count = tv.tv_sec * 1000000 + tv.tv_usec;
return count / 1000;
}
// 1 = Basic 2 = Advanced
void setSmoothing(char steps, char adv){
_smoothUse = 1;
_smoothSteps = ( steps > 10) ? 10 : steps;
_smoothAdvanced = (adv == 1) ? 1 : 0;
}
/**
SMOOTH OUTPUT
This function smooths the output for the XYZ axis. Depending on the options set in
@see setSmoothing(), we can run multiple methods of smoothing the sensor readings.
First we store (n) samples of sensor readings for each axis and store them in a rolling array.
As each new sensor reading comes in we replace it with a new reading. Then we average the total
of all (n) readings.
Advanced Smoothing
If you turn advanced smoothing on, we will select the min and max values from our array
of (n) samples. We then subtract both the min and max from the total and average the total of all
(n - 2) readings.
NOTE: This function does several calculations and can cause your sketch to run slower.
@since v0.3;
**/
void _smoothing(){
char max = 0;
char min = 0;
if ( _vScan > _smoothSteps - 1 ) { _vScan = 0; }
for ( int i = 0; i < 3; i++ ) {
if ( _vTotals[i] != 0 ) {
_vTotals[i] = _vTotals[i] - _vHistory[_vScan][i];
}
_vHistory[_vScan][i] = ( _calibrationUse ) ? _vCalibrated[i] : _vRaw[i];
_vTotals[i] = _vTotals[i] + _vHistory[_vScan][i];
if ( _smoothAdvanced ) {
max = 0;
for (int j = 0; j < _smoothSteps - 1; j++) {
max = ( _vHistory[j][i] > _vHistory[max][i] ) ? j : max;
}
min = 0;
for (int k = 0; k < _smoothSteps - 1; k++) {
min = ( _vHistory[k][i] < _vHistory[min][i] ) ? k : min;
}
_vSmooth[i] = ( _vTotals[i] - (_vHistory[max][i] + _vHistory[min][i]) ) / (_smoothSteps - 2);
} else {
_vSmooth[i] = _vTotals[i] / _smoothSteps;
}
}
_vScan++;
}
/**
GET X AXIS
Read the X axis
@since v0.1;
@return int x axis
**/
int getX(){
return _get(0);
}
/**
GET Y AXIS
Read the Y axis
@since v0.1;
@return int y axis
**/
int getY(){
return _get(1);
}
/**
GET Z AXIS
Read the Z axis
@since v0.1;
@return int z axis
**/
int getZ(){
return _get(2);
}
/**
GET SENSOR AXIS READING
Get the smoothed, calibration, or raw data from a given sensor axis
@since v1.1.0
@return int sensor axis value
**/
int _get(int i){
if ( _smoothUse )
return _vSmooth[i];
if ( _calibrationUse )
return _vCalibrated[i];
return _vRaw[i];
}
/**
GET AZIMUTH
Calculate the azimuth (in degrees);
@since v0.1;
@return int azimuth
**/
int getAzimuth(){
int a = atan2( getY(), getX() ) * 180.0 / M_PI;
return a < 0 ? 360 + a : a;
}
/**
GET BEARING
Divide the 360 degree circle into 16 equal parts and then return the a value of 0-15
based on where the azimuth is currently pointing.
@since v1.0.1 - function now requires azimuth parameter.
@since v0.2.0 - initial creation
@return char direction of bearing
*/
char getBearing(int azimuth){
unsigned long a = azimuth / 22.5;
unsigned long r = a - (int)a;
char sexdec = 0;
sexdec = ( r >= .5 ) ? ceil(a) : floor(a);
return sexdec;
}
/**
This will take the location of the azimuth as calculated in getBearing() and then
produce an array of chars as a text representation of the direction.
NOTE: This function does not return anything since it is not possible to return an array.
Values must be passed by reference back to your sketch.
Example:
( if direction is in 1 / NNE)
char myArray[3];
compass.getDirection(myArray, azimuth);
Serial.print(myArray[0]); // N
Serial.print(myArray[1]); // N
Serial.print(myArray[2]); // E
@see getBearing();
@since v1.0.1 - function now requires azimuth parameter.
@since v0.2.0 - initial creation
*/
void getDirection(char* myArray, int azimuth){
int d = getBearing(azimuth);
myArray[0] = _bearings[d][0];
myArray[1] = _bearings[d][1];
myArray[2] = _bearings[d][2];
}
void read_datas(){
unsigned char rddata[15];
int t_value;
//printf("write address return: %d\n",write(i2c_fd, rdaddr, 2)); /* 读取之前首先设置读取的偏移量 */
{
i2c_smbus_read_i2c_block_data(i2c_fd, 0x00,6, rddata);
_vRaw[0] = (int)(int16_t)(rddata[0] | rddata[1] << 8);
_vRaw[1] = (int)(int16_t)(rddata[2] | rddata[3] << 8);
_vRaw[2] = (int)(int16_t)(rddata[4] | rddata[5] << 8);
//printf("%d,%d,%d\n", _vRaw[0],_vRaw[1],_vRaw[2]);
if ( _calibrationUse ) {
_applyCalibration();
}
if ( _smoothUse ) {
_smoothing();
}
}
}
/*******************************************************************************
* function name : getRawData
* description : 完成初始
* param[in] : none
* param[out] : none
* return : 0
*******************************************************************************/
void getRawData(int* x ,int* y,int* z)
{
*x = _vRaw[0] ;
*z = _vRaw[1] ;
*y = _vRaw[2];
}
/*******************************************************************************
* function name : calculateHeading
* description : 完成初始
* param[in] : none
* param[out] : none
* return : 0
*******************************************************************************/
int calculateHeading()
{
int a = atan2( getY(), getX() ) * 180.0 / M_PI;
int headingRadians =a - MagnetcDeclination;
return headingRadians < 0 ? 360 + headingRadians : headingRadians;
}
/**
SET CALIBRATION
Set calibration values for more accurate readings
@author Claus Näveke - TheNitek [https://github.com/TheNitek]
@since v1.1.0
**/
void setCalibration(int x_min, int x_max, int y_min, int y_max, int z_min, int z_max){
_calibrationUse = 1;
_vCalibration[0][0] = x_min;
_vCalibration[0][1] = x_max;
_vCalibration[1][0] = y_min;
_vCalibration[1][1] = y_max;
_vCalibration[2][0] = z_min;
_vCalibration[2][1] = z_max;
}
/**
APPLY CALIBRATION
This function uses the calibration data provided via @see setCalibration() to calculate more
accurate readings
@author Claus Näveke - TheNitek [https://github.com/TheNitek]
Based on this awesome article:
https://appelsiini.net/2018/calibrate-magnetometer/
@since v1.1.0
**/
void _applyCalibration(){
int x_offset = (_vCalibration[0][0] + _vCalibration[0][1])/2;
int y_offset = (_vCalibration[1][0] + _vCalibration[1][1])/2;
int z_offset = (_vCalibration[2][0] + _vCalibration[2][1])/2;
int x_avg_delta = (_vCalibration[0][1] - _vCalibration[0][0])/2;
int y_avg_delta = (_vCalibration[1][1] - _vCalibration[1][0])/2;
int z_avg_delta = (_vCalibration[2][1] - _vCalibration[2][0])/2;
int avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta) / 3;
float x_scale = (float)avg_delta / x_avg_delta;
float y_scale = (float)avg_delta / y_avg_delta;
float z_scale = (float)avg_delta / z_avg_delta;
_vCalibrated[0] = (_vRaw[0] - x_offset) * x_scale;
_vCalibrated[1] = (_vRaw[1] - y_offset) * y_scale;
_vCalibrated[2] = (_vRaw[2] - z_offset) * z_scale;
}
/*
具体的办法是：
1，水平匀速旋转，收集 XY轴的数据
2，转动器材90 度（此时 Z轴水平）匀速旋转以收集 Z轴数据
3，将读取到的各轴数据的最大值加上最小值除以2，就得到一个各轴的offset 值
Xoffset=（Xmax+Xmin）/2
Yoffset=（Ymax+Ymin）/2
Zoffset=（Zmax+Zmin）/2
4，然后将磁力计读取的各轴的裸值减去前面计算所得的 offset值，就可以得到用作角度计算的 Heading 值
XH=X裸-Xoffset
YH=Y裸-Yoffset
ZH=Z裸-Zoffset
如果只用作水平测量，则此时的方位角为
方位角=arctanYH/XH
*/
/*******************************************************************************
* function name : calibrateMag
* description : 完成初始
* param[in] : none
* param[out] : none
* return : 0
*******************************************************************************/
void calibrateMag()
{
int x,y,z; //三轴数据
int xMax, xMin, yMax, yMin, zMax, zMin;
//初始化
getRawData(&x,&y,&z);
xMax=xMin=x;
yMax=yMin=y;
zMax=zMin=z;
offsetX = offsetY = offsetZ = 0;
printf("Starting Calibration......");
printf("Please turn your device around in 20 seconds");
for(int i=0;i<10;i++)
{
getRawData(&x,&y,&z);
// 计算最大值与最小值
// 计算传感器绕X,Y,Z轴旋转时的磁场强度最大值和最小值
if (x > xMax)
xMax = x;
if (x < xMin )
xMin = x;
if(y > yMax )
yMax = y;
if(y < yMin )
yMin = y;
if(z > zMax )
zMax = z;
if(z < zMin )
zMin = z;
sleep(1);
if(i%10 == 0)
{
// printf(xMax);
// printf(" ");
//printf(xMin);
}
}
//计算修正量
if(abs(xMax - xMin) > CalThreshold )
offsetX = (xMax + xMin)/2;
if(abs(yMax - yMin) > CalThreshold )
offsetY = (yMax + yMin)/2;
if(abs(zMax - zMin) > CalThreshold )
offsetZ = (zMax +zMin)/2;
printf("offsetX: %d \n",offsetX);
printf(" offsetY:%d \n",offsetY);
printf(" offsetz:%d \n",offsetZ);
}
int calibrationData[3][2];
char changed = 0;
char done = 0;
int t = 0;
int c = 0;
void qmc5883L_calibration() {
int x, y, z;
// Read compass values
read_datas();
// Return XYZ readings
x = getX();
y = getY();
z = getZ();
changed = 0;
if(x < calibrationData[0][0]) {
calibrationData[0][0] = x;
changed = 1;
}
if(x > calibrationData[0][1]) {
calibrationData[0][1] = x;
changed = 1;
}
if(y < calibrationData[1][0]) {
calibrationData[1][0] = y;
changed = 1;
}
if(y > calibrationData[1][1]) {
calibrationData[1][1] = y;
changed = 1;
}
if(z < calibrationData[2][0]) {
calibrationData[2][0] = z;
changed = 1;
}
if(z > calibrationData[2][1]) {
calibrationData[2][1] = z;
changed = 1;
}
if (changed && !done) {
printf("CALIBRATING... Keep moving your sensor around.\n");
c = millis();
}
t = millis();
if ( (t - c > 5000) && !done) {
done = 1;
printf("DONE. Copy the line below and paste it into your projects sketch.);\n");
printf("compass.setCalibration(");
printf("%d,",calibrationData[0][0]);
printf("%d,",calibrationData[0][1]);
printf("%d,",calibrationData[1][0]);
printf("%d,",calibrationData[1][1]);
printf("%d,",calibrationData[2][0]);
printf("%d",calibrationData[2][1]);
printf(");\n");
}
}
/*******************************************************************************
* function name : main
* description : 完成初始
* param[in] : none
* param[out] : none
* return : 0
*******************************************************************************/
int main(int argc, char **argv)
{
printf("hello, this is i2c tester\n");
i2c_fd = open(CHIP, O_RDWR);
if (i2c_fd < 0)
{
printf("open "CHIP"failed\n");
return -1;
}
if (ioctl(i2c_fd, I2C_SLAVE, CHIP_ADDR) < 0)
{ /* 设置芯片地址 */
printf("oictl:set slave address failed\n");
return -1;
}
// system("i2cset -y 1 0x1e 0x02 0x00");
sleep(1);
qmc5883l_Init() ;
// calibrateMag();
//system("i2cset -y 1 0x1e 0x02 0x00");
// setCalibration(-1578,1046,-1403,1497,-579,1669);//1s
setCalibration(-1412,1239,-1013,1584,-1223,1024);//1d
// setCalibration(-1406,705,-323,1185,-1228,458);//1e
/*
* call setSmoothing(STEPS, ADVANCED);
*
* STEPS = int The number of steps to smooth the results by. Valid 1 to 10.
* Higher steps equals more smoothing but longer process time.
*
* ADVANCED = bool Turn advanced smmothing on or off. True will remove the max and min values from each step and then process as normal.
* Turning this feature on will results in even more smoothing but will take longer to process.
*
*/
//setSmoothing(10,1);
while(1)
{
//qmc5883L_calibration();
read_datas();
//int a =getAzimuth();
// Output here will be a value from 0 - 15 based on the direction of the bearing / azimuth.
//int b = getBearing(a);
int a =calculateHeading();
printf("degree:%d \n",a);
// printf("%d,%d,%d\n", _vCalibrated[0],_vCalibrated[1],_vCalibrated[2]);
// printf("B: ");
// printf("get:%d",b);
usleep(5000);
}
}
int main1(int argc, char **argv)
{
uint8_t data, addr = 0x76, reg = 0x00;
const char *path = argv[1];
int file, rc;
i2c_fd = open(CHIP, O_RDWR);
if (i2c_fd < 0)
err(errno, "Tried to open '%s'", CHIP);
rc = ioctl(i2c_fd, I2C_SLAVE, CHIP_ADDR);
if (rc < 0)
err(errno, "Tried to set device address '0x%02x'", CHIP_ADDR);
int error = i2c_smbus_write_byte_data(i2c_fd, 0x0b,0x01);
data = i2c_smbus_read_byte_data(i2c_fd, reg);
printf("%s: device 0x%02x at address 0x%02x: 0x%02x\n",
path, addr, reg, data);
}
获取北京的磁偏角。
https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml#declination
#include <wiringPi.h> //I2C 库
include <wiringPiI2C.h> //I2C 库
include <stdio.h>
include <math.h>
include <stdlib.h>
include<unistd.h>
include <sys/ioctl.h>
include <fcntl.h>
include <linux/i2c-dev.h>
include <linux/i2c.h>
define CHIP "/dev/i2c-1"
define CHIP_ADDR 0x1e
define address 0x1e//001 1110b(0x3C>>1), HMC5883的7位i2c地址
define MagnetcDeclination 4.43 //笔者所在地磁偏角，请根据情况自行百度
define CalThreshold 0
define HMC5883L_DATA_OUT_REG 0x03
define HMC5883L_STATUS_REG 0x09
int offsetX,offsetY,offsetZ;
int i2c_fd;
/***
- function name : Hmc5883l_Init
- description : 完成初始
- param[in] : none
- param[out] : none
- return : 0
***/
void Hmc5883l_Init()
{
//初始化串口和i2c
int i;
unsigned char rdaddr[2] = {0, 0}; / 将要读取的数据在芯片中的偏移量 /
unsigned char wrbuf[3] = {0, 0, 0x3c}; / 要写的数据，头两字节为偏移量 /
unsigned char rddata[15];
printf("Hmc5883l_Init \n");
// printf("write address return: %d\n",write(i2c_fd, rdaddr, 2)); / 读取之前首先设置读取的偏移量 /
write(i2c_fd, rdaddr, 2);
printf("read data return:%d\n", read(i2c_fd, rddata, 9)); //读取3个字节到buf中
for(i=0;i<3;i++)
printf(" %x ", rddata[i]);
printf("* \n");
if(rddata[0]!=0x70)
{
//设置HMC5883模式
wrbuf[1] = 0x00;
wrbuf[2] = 0x70;
write(i2c_fd, wrbuf,3);
}
if(rddata[2]!=0x00)
{
wrbuf[1] = 0x02;//选择模式寄存器
wrbuf[2] = 0x00;//连续测量模式:0x00,单一测量模式:0x01
write(i2c_fd, wrbuf,3);
}
//calibrateMag();
}
/*** - function name : getRawData
- description : 完成初始
- param[in] : none
- param[out] : none
- return : 0
***/
void getRawData(int x ,int y,int z)
{
//Wire.write(0x03); //从寄存器3开始读数据
//Wire.endTransmission();
//每轴的数据都是16位的
// Wire.requestFrom(address, 6);
int j,k,l;
unsigned char rddata[15];
unsigned int i;
unsigned char rdaddr[2] = {0,0}; / 将要读取的数据在芯片中的偏移量 /
unsigned char wrbuf[3] = {0, 0, 0x3c}; / 要写的数据，头两字节为偏移量 /
//printf("write address return: %d\n",write(i2c_fd, rdaddr, 2)); / 读取之前首先设置读取的偏移量 */
write(i2c_fd, rdaddr, 2);
printf("read data return:%d\n", read(i2c_fd, rddata, 9));
//for(i=0;i<9;i++)
//printf("%x ", rddata[i]);
// printf("***** \n");
j = rddata[2];
j =j<<8| rddata[3] ;
k = rddata[46];
k= k<< 8 | rddata[5] ;
l = rddata[6];
l=l << 8 | rddata[7] ;
memset(rddata,0x00,6);
/
j = wiringPiI2CReadReg8(fd, HMC5883L_DATA_OUT_REG) << 8;
j |= wiringPiI2CReadReg8(fd, HMC5883L_DATA_OUT_REG + 1);
k = wiringPiI2CReadReg8(fd, HMC5883L_DATA_OUT_REG + 2) << 8;
k |= wiringPiI2CReadReg8(fd, HMC5883L_DATA_OUT_REG + 3);
l = wiringPiI2CReadReg8(fd, HMC5883L_DATA_OUT_REG + 4) << 8;
l |= wiringPiI2CReadReg8(fd, HMC5883L_DATA_OUT_REG + 5);
/
if(j>0x7fff)j-=0xffff;
if(k>0x7fff)k-=0xffff;
if(l>0x7fff)l-=0xffff;
x = j;
z=k;
*y = l;
}
/*** - function name : calculateHeading
- description : 完成初始
- param[in] : none
- param[out] : none
- return : 0
/
int calculateHeading(int x ,int y,int z)
{
float headingRadians = atan2((double)((y)-offsetY),(double)((x)-offsetX)) (180/3.14159265)+180;
// float headingRadians = atan2((double)(y),(double)(x)) (180/3.14159265)+180;
// 保证数据在0-2PI之间
//if(headingRadians < 0)
// headingRadians += 2M_PI;
// int headingDegrees = headingRadians 180/M_PI;
// headingDegrees = MagnetcDeclination; //磁偏角
// headingRadians -= MagnetcDeclination;
// <span style="font-family: Arial, Helvetica, sans-serif;">保证数据在0-360之间</span>
// if(Degrees > 360)
// headingDegrees -= 360;
return headingRadians;
// return headingDegrees;
}
void loop_test()
{
int x,y,z; //三轴数据
int angle;
getRawData(&x,&y,&z);
//输出数据
printf(" x: %d \n",x);
printf(" y: %d \n",y);
printf(" z: %d \n",z);
angle = calculateHeading(&x,&y,&z);
printf(" angle(x,y): %d\n",calculateHeading(&x,&y,&z));
//输出x,y平面方向角
if((angle < 62.5) || (angle > 350 ))
printf("South");
if((angle > 62.5) && (angle < 135 ))
printf("South-West");
if((angle > 135) && (angle < 190 ))
printf("West");
if((angle > 190) && (angle < 235.5 ))
printf("North-West");
if((angle > 235.5) && (angle < 270 ))
printf("North");
if((angle > 270) && (angle < 285 ))
printf("NorthEast");
if((angle > 285) && (angle < 315 ))
printf("East");
if((angle > 315) && (angle < 350 ))
printf("SouthEast");
delay(250);
}
/
具体的办法是：
1，水平匀速旋转，收集 XY轴的数据
2，转动器材90 度（此时 Z轴水平）匀速旋转以收集 Z轴数据
3，将读取到的各轴数据的最大值加上最小值除以2，就得到一个各轴的offset 值
Xoffset=（Xmax+Xmin）/2
Yoffset=（Ymax+Ymin）/2
Zoffset=（Zmax+Zmin）/2
4，然后将磁力计读取的各轴的裸值减去前面计算所得的 offset值，就可以得到用作角度计算的 Heading 值
XH=X裸-Xoffset
YH=Y裸-Yoffset
ZH=Z裸-Zoffset
如果只用作水平测量，则此时的方位角为
方位角=arctanYH/XH
/
/ - function name : calibrateMag
- description : 完成初始
- param[in] : none
- param[out] : none
- return : 0
/
void calibrateMag()
{
int x,y,z; //三轴数据
int xMax, xMin, yMax, yMin, zMax, zMin;
//初始化
getRawData(&x,&y,&z);
xMax=xMin=x;
yMax=yMin=y;
zMax=zMin=z;
offsetX = offsetY = offsetZ = 0;
printf("Starting Calibration......");
printf("Please turn your device around in 20 seconds");
for(int i=0;i<10;i++)
{
getRawData(&x,&y,&z);
// 计算最大值与最小值
// 计算传感器绕X,Y,Z轴旋转时的磁场强度最大值和最小值
if (x > xMax)
xMax = x;
if (x < xMin )
xMin = x;
if(y > yMax )
yMax = y;
if(y < yMin )
yMin = y;
if(z > zMax )
zMax = z;
if(z < zMin )
zMin = z;
sleep(1);
if(i%10 == 0)
{
// printf(xMax);
// printf(" ");
//printf(xMin);
}
}
//计算修正量
if(abs(xMax - xMin) > CalThreshold )
offsetX = (xMax + xMin)/2;
if(abs(yMax - yMin) > CalThreshold )
offsetY = (yMax + yMin)/2;
if(abs(zMax - zMin) > CalThreshold )
offsetZ = (zMax +zMin)/2;
printf("offsetX: %d \n",offsetX);
printf(" offsetY:%d \n",offsetY);
printf(" offsetz:%d \n",offsetZ);
delay(5000);
}
/ - function name : main
- description : 完成初始
- param[in] : none
- param[out] : none
- return : 0
***/
int main()
{
printf("hello, this is i2c tester\n");
i2c_fd = open(CHIP, O_RDWR);
if (i2c_fd < 0)
{
printf("open "CHIP"failed\n");
return -1;
}
if (ioctl(i2c_fd, I2C_SLAVE_FORCE, CHIP_ADDR) < 0)
{ / 设置芯片地址 /
printf("oictl:set slave address failed\n");
return -1;
}
// system("i2cset -y 1 0x1e 0x02 0x00");
sleep(1);
Hmc5883l_Init() ;
calibrateMag();
//system("i2cset -y 1 0x1e 0x02 0x00");
while(1)
{
loop_test();
sleep(1);
}
}
最新评论
参考这里进行self test 获取比例因子和偏移量