I have the following code and everytime I re-read the new values the are always a little bit different. (not touching the HMC5883L!)
My plan was to use the HMC5883L to let my robot drive in a straight line, but since the value is always different, I don't see how this will work.
[code]
/*
* Using the HMC5883L (GY-273)
*
* Connection on a Raspberry Pi 3 model B v1.2 :
* - VCC -> 3V3 (1)
* - GND -> GND (6)
* - SCL -> GPIO3 - SCL1 I2C (5)
* - SDA -> GPIO2 - SDA1 I2C (3)
*
* Compiled in Netbeans IDE 8.1
* Also possible to compile with GCC :
* 'GCC main.cpp -o HMC5883L -lm'
*/
/*
* File: main.cpp
* Author: Steven Noppe
*
* Revision date : 13/11/2016
*/
/*
* Compiling with GCC, gives a fault because M_PI is not defined
* With an IDE like Netbeans, we do not have that problem
* Thats why we define it here
*/
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
/*
* Same problem here with GCC, it seems he doesn't know what 'true' is
*/
#ifndef true
#define true 1
#endif
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <linux/i2c-dev.h>
#include <math.h>
class HMC5883L
{
private:
short x ;
short y ;
short z ;
float angle ;
int fd ; // 'fd' stands for 'file descriptor'
unsigned char buf[16] ;
/*
* Find out which address the HMC5883L is on with the command :
* 'sudo i2cdetect -y 1' on a Raspberry Pi - model A, B rev 2 of B+
* 'sudo i2cdetect -y 0' on a Raspberry Pi - model B rev 1
* 1E => 0x1E
*/
const int HMC5883L_I2C_ADDR = 0x1E ;
void selectDevice(int fd, int addr, char *name)
{
/*
* With the ioctl() function call we initialize the HMC3882L as
* a i2c slave on the address that we found with
* 'sudo i2cdetect -y 1' and declared in the integer HMC5883L_I2C_ADDR
* see also : http://man7.org/linux/man-pages/man2/ioctl.2.html
*/
if (ioctl(fd, I2C_SLAVE, addr) < 0)
{
// if ioctl() returns < 0 then we had a problem
fprintf(stderr, "%s not present\n", name) ;
exit(1) ;
}
}
void writeToDevice(int fd, int reg, int val)
{
char buf[2] ;
buf[0]=reg ;
buf[1]=val ;
if (write(fd, buf, 2) != 2)
{
fprintf(stderr, "Can't write to ADXL345\n") ;
exit(1) ;
}
}
public:
short getX()
{
return x ;
}
short getY()
{
return y ;
}
short getZ()
{
return z ;
}
float getAngle()
{
return atan2(y, x) * 180 / M_PI ;
}
void initializeHMC5883L()
{
/*
* Open i2c-1 port for reading and writing with the open system call
* The open system call returns a number which you will need in future
* functions for reading and writing
* see also : https://en.wikipedia.org/wiki/Open_(system_call)
*/
if ((fd = open("/dev/i2c-1", O_RDWR)) < 0)
// on a Raspberry Pi - model B rev 1 use : '/dev/i2c-0'
{
// If it returns < 0 then something was wrong
fprintf(stderr, "Failed to open i2c bus\n") ;
// exit program with a 1 (failure)
exit(1) ;
}
else
{
// successfull !
printf("Succesfully openend i2c bus\n") ;
}
/*
* Initialise the HMC5883L with the integer 'fd' that was returned from the
* open system call
*/
selectDevice(fd, HMC5883L_I2C_ADDR, "HMC5883L") ;
/*
* Now write some values to the HMC5883L to initalize it
*/
/*
* First write the value 32 to the configuration register A
* 32 = Gain = 1090 Lsb/Gauss
* We send the following byte : 00100000
*/
writeToDevice(fd, 0x01, 32) ; // 0x01 = configuration register A
/*
* Second write the value 0 to the mode register
* 0 = Continuous-Measurement Mode
* We send the following byte : 0000000
*/
writeToDevice(fd, 0x02, 0) ; // 0x02 = Mode register
}
void updateData()
{
buf[0] = 0x03 ;
if ((write(fd, buf, 1)) != 1)
{
// Send the register to read from
fprintf(stderr, "Error writing to i2c slave\n") ;
}
else
{
//printf("Wrote to i2c slave.\n") ;
}
if (read(fd, buf, 6) != 6)
{
fprintf(stderr, "Unable to read from HMC5883L\n") ;
}
else
{
// Received the values
x = (buf[0] << 8) | buf[1] ;
z = (buf[2] << 8) | buf[3] ;
y = (buf[4] << 8) | buf[5] ;
//angle = atan2(y, x) * 180 / M_PI ;
float declDegree = 0 ;
float declMin = 35 ;
float decl = (declDegree+declMin/60) * (M_PI/180) ;
float headingRad = atan2(y, x) ;
headingRad += decl ;
if (headingRad < 0)
headingRad += 2*M_PI ;
if (headingRad > 2*M_PI)
headingRad -= 2*M_PI ;
float headingDeg = headingRad * 180/M_PI ;
float degrees = floor(headingDeg) ;
float minutes = round((headingDeg - degrees)*60) ;
printf("degrees : %0.2f, minutes : %0.2f \n", degrees, minutes) ;
// with GCC you need to declare b outside the for loop
// or compile with the option -std=c11
int b ;
for (b=0; b<6; ++b)
{
// printf("%02x ",buf[b]) ;
}
// printf("\n") ;
// printf("x=%d, y=%d, z=%d\n", x, y, z) ;
// printf("angle = %0.1f\n\n", angle) ;
}
}
};
// Main program
int main(int argc, char **argv)
{
HMC5883L compass ;
compass.initializeHMC5883L() ;
while (true)
{
compass.updateData() ;
printf("Angle : %f \n", compass.getAngle()) ;
fflush(stdout) ;
getchar() ;
}
return 0 ;
}
[/code]
