Seeed / grove_compass

grove_compass

grove_compass.cpp@0:609518794a5d, 2015-06-09 (annotated)

Committer:
JackyZhangFromSeeed
Date:
Tue Jun 09 10:17:32 2015 +0000
Revision:
0:609518794a5d
grove_compass

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JackyZhangFromSeeed 0:609518794a5d 1
JackyZhangFromSeeed 0:609518794a5d 2
JackyZhangFromSeeed 0:609518794a5d 3 #include "suli2.h"
JackyZhangFromSeeed 0:609518794a5d 4 #include "grove_compass.h"
JackyZhangFromSeeed 0:609518794a5d 5
JackyZhangFromSeeed 0:609518794a5d 6 //local functions
JackyZhangFromSeeed 0:609518794a5d 7 static void grove_compass_getxyz_raw(I2C_T *i2c, int16_t *x, int16_t *y, int16_t *z);
JackyZhangFromSeeed 0:609518794a5d 8 static int setScale(I2C_T *i2c, float gauss);
JackyZhangFromSeeed 0:609518794a5d 9 static void setMeasurementMode(I2C_T *i2c, uint8_t mode);
JackyZhangFromSeeed 0:609518794a5d 10
JackyZhangFromSeeed 0:609518794a5d 11 //local variables
JackyZhangFromSeeed 0:609518794a5d 12 static float m_Scale;
JackyZhangFromSeeed 0:609518794a5d 13 static unsigned char cmdbuf[2];
JackyZhangFromSeeed 0:609518794a5d 14 static unsigned char databuf[6];
JackyZhangFromSeeed 0:609518794a5d 15
JackyZhangFromSeeed 0:609518794a5d 16 void grove_compass_init(I2C_T *i2c, int pinsda, int pinscl)
JackyZhangFromSeeed 0:609518794a5d 17 {
JackyZhangFromSeeed 0:609518794a5d 18 suli_i2c_init(i2c, pinsda, pinscl);
JackyZhangFromSeeed 0:609518794a5d 19 }
JackyZhangFromSeeed 0:609518794a5d 20
JackyZhangFromSeeed 0:609518794a5d 21 bool grove_compass_write_setup(I2C_T *i2c)
JackyZhangFromSeeed 0:609518794a5d 22 {
JackyZhangFromSeeed 0:609518794a5d 23 uint8_t regValue = 0x00;
JackyZhangFromSeeed 0:609518794a5d 24
JackyZhangFromSeeed 0:609518794a5d 25 //setScale(i2c, (float)1.3); // Set the scale of the compass.
JackyZhangFromSeeed 0:609518794a5d 26 regValue = 0x01;
JackyZhangFromSeeed 0:609518794a5d 27 m_Scale = 0.92;
JackyZhangFromSeeed 0:609518794a5d 28 // Setting is in the top 3 bits of the register.
JackyZhangFromSeeed 0:609518794a5d 29 regValue = regValue << 5;
JackyZhangFromSeeed 0:609518794a5d 30 //write(CONFIGURATION_REGISTERB, regValue);
JackyZhangFromSeeed 0:609518794a5d 31 cmdbuf[0] = CONFIGURATION_REGISTERB;
JackyZhangFromSeeed 0:609518794a5d 32 cmdbuf[1] = regValue;
JackyZhangFromSeeed 0:609518794a5d 33 suli_i2c_write(i2c, HMC5883L_ADDRESS, cmdbuf, 2);
JackyZhangFromSeeed 0:609518794a5d 34 setMeasurementMode(i2c, MEASUREMENT_CONTINUOUS); // Set the measurement mode to Continuous
JackyZhangFromSeeed 0:609518794a5d 35
JackyZhangFromSeeed 0:609518794a5d 36 return true;
JackyZhangFromSeeed 0:609518794a5d 37 }
JackyZhangFromSeeed 0:609518794a5d 38
JackyZhangFromSeeed 0:609518794a5d 39 static void grove_compass_getxyz_raw(I2C_T *i2c, int16_t *x, int16_t *y, int16_t *z)
JackyZhangFromSeeed 0:609518794a5d 40 {
JackyZhangFromSeeed 0:609518794a5d 41 cmdbuf[0] = DATA_REGISTER_BEGIN;
JackyZhangFromSeeed 0:609518794a5d 42 suli_i2c_write(i2c, HMC5883L_ADDRESS, &cmdbuf[0], 1);
JackyZhangFromSeeed 0:609518794a5d 43 suli_i2c_read(i2c, HMC5883L_ADDRESS, databuf, 6);
JackyZhangFromSeeed 0:609518794a5d 44 *x = (databuf[0] << 8) | databuf[1];
JackyZhangFromSeeed 0:609518794a5d 45 *y = (databuf[2] << 8) | databuf[3];
JackyZhangFromSeeed 0:609518794a5d 46 *z = (databuf[4] << 8) | databuf[5];
JackyZhangFromSeeed 0:609518794a5d 47 }
JackyZhangFromSeeed 0:609518794a5d 48
JackyZhangFromSeeed 0:609518794a5d 49 bool grove_compass_getxyz_scaled(I2C_T *i2c, float *cx, float *cy, float *cz)
JackyZhangFromSeeed 0:609518794a5d 50 {
JackyZhangFromSeeed 0:609518794a5d 51 int16_t x,y,z;
JackyZhangFromSeeed 0:609518794a5d 52
JackyZhangFromSeeed 0:609518794a5d 53 grove_compass_getxyz_raw(i2c, &x,&y,&z);
JackyZhangFromSeeed 0:609518794a5d 54 *cx = (float)x * m_Scale;
JackyZhangFromSeeed 0:609518794a5d 55 *cy = (float)y * m_Scale;
JackyZhangFromSeeed 0:609518794a5d 56 *cz = (float)z * m_Scale;
JackyZhangFromSeeed 0:609518794a5d 57
JackyZhangFromSeeed 0:609518794a5d 58 return true;
JackyZhangFromSeeed 0:609518794a5d 59 }
JackyZhangFromSeeed 0:609518794a5d 60
JackyZhangFromSeeed 0:609518794a5d 61 bool grove_compass_getheading(I2C_T *i2c, float *heading)
JackyZhangFromSeeed 0:609518794a5d 62 {
JackyZhangFromSeeed 0:609518794a5d 63 float cx, cy, cz;
JackyZhangFromSeeed 0:609518794a5d 64
JackyZhangFromSeeed 0:609518794a5d 65 grove_compass_getxyz_scaled(i2c, &cx, &cy, &cz);
JackyZhangFromSeeed 0:609518794a5d 66
JackyZhangFromSeeed 0:609518794a5d 67 float head = atan2(cy, cx) - 0.0457;
JackyZhangFromSeeed 0:609518794a5d 68
JackyZhangFromSeeed 0:609518794a5d 69 // Correct for when signs are reversed.
JackyZhangFromSeeed 0:609518794a5d 70 if(heading < 0)
JackyZhangFromSeeed 0:609518794a5d 71 head += 2*PI;
JackyZhangFromSeeed 0:609518794a5d 72
JackyZhangFromSeeed 0:609518794a5d 73 // Check for wrap due to addition of declination.
JackyZhangFromSeeed 0:609518794a5d 74 if(head > 2*PI)
JackyZhangFromSeeed 0:609518794a5d 75 head -= 2*PI;
JackyZhangFromSeeed 0:609518794a5d 76
JackyZhangFromSeeed 0:609518794a5d 77 // Convert radians to degrees for readability.
JackyZhangFromSeeed 0:609518794a5d 78 *heading = head * 180 / PI;
JackyZhangFromSeeed 0:609518794a5d 79
JackyZhangFromSeeed 0:609518794a5d 80 return true;
JackyZhangFromSeeed 0:609518794a5d 81 }
JackyZhangFromSeeed 0:609518794a5d 82
JackyZhangFromSeeed 0:609518794a5d 83
JackyZhangFromSeeed 0:609518794a5d 84 static int setScale(I2C_T *i2c, float gauss)
JackyZhangFromSeeed 0:609518794a5d 85 {
JackyZhangFromSeeed 0:609518794a5d 86 uint8_t regValue = 0x00;
JackyZhangFromSeeed 0:609518794a5d 87 if(gauss == 0.88)
JackyZhangFromSeeed 0:609518794a5d 88 {
JackyZhangFromSeeed 0:609518794a5d 89 regValue = 0x00;
JackyZhangFromSeeed 0:609518794a5d 90 m_Scale = 0.73;
JackyZhangFromSeeed 0:609518794a5d 91 }
JackyZhangFromSeeed 0:609518794a5d 92 else if(gauss == 1.3)
JackyZhangFromSeeed 0:609518794a5d 93 {
JackyZhangFromSeeed 0:609518794a5d 94 regValue = 0x01;
JackyZhangFromSeeed 0:609518794a5d 95 m_Scale = 0.92;
JackyZhangFromSeeed 0:609518794a5d 96 }
JackyZhangFromSeeed 0:609518794a5d 97 else if(gauss == 1.9)
JackyZhangFromSeeed 0:609518794a5d 98 {
JackyZhangFromSeeed 0:609518794a5d 99 regValue = 0x02;
JackyZhangFromSeeed 0:609518794a5d 100 m_Scale = 1.22;
JackyZhangFromSeeed 0:609518794a5d 101 }
JackyZhangFromSeeed 0:609518794a5d 102 else if(gauss == 2.5)
JackyZhangFromSeeed 0:609518794a5d 103 {
JackyZhangFromSeeed 0:609518794a5d 104 regValue = 0x03;
JackyZhangFromSeeed 0:609518794a5d 105 m_Scale = 1.52;
JackyZhangFromSeeed 0:609518794a5d 106 }
JackyZhangFromSeeed 0:609518794a5d 107 else if(gauss == 4.0)
JackyZhangFromSeeed 0:609518794a5d 108 {
JackyZhangFromSeeed 0:609518794a5d 109 regValue = 0x04;
JackyZhangFromSeeed 0:609518794a5d 110 m_Scale = 2.27;
JackyZhangFromSeeed 0:609518794a5d 111 }
JackyZhangFromSeeed 0:609518794a5d 112 else if(gauss == 4.7)
JackyZhangFromSeeed 0:609518794a5d 113 {
JackyZhangFromSeeed 0:609518794a5d 114 regValue = 0x05;
JackyZhangFromSeeed 0:609518794a5d 115 m_Scale = 2.56;
JackyZhangFromSeeed 0:609518794a5d 116 }
JackyZhangFromSeeed 0:609518794a5d 117 else if(gauss == 5.6)
JackyZhangFromSeeed 0:609518794a5d 118 {
JackyZhangFromSeeed 0:609518794a5d 119 regValue = 0x06;
JackyZhangFromSeeed 0:609518794a5d 120 m_Scale = 3.03;
JackyZhangFromSeeed 0:609518794a5d 121 }
JackyZhangFromSeeed 0:609518794a5d 122 else if(gauss == 8.1)
JackyZhangFromSeeed 0:609518794a5d 123 {
JackyZhangFromSeeed 0:609518794a5d 124 regValue = 0x07;
JackyZhangFromSeeed 0:609518794a5d 125 m_Scale = 4.35;
JackyZhangFromSeeed 0:609518794a5d 126 }
JackyZhangFromSeeed 0:609518794a5d 127 else
JackyZhangFromSeeed 0:609518794a5d 128 return -1;
JackyZhangFromSeeed 0:609518794a5d 129
JackyZhangFromSeeed 0:609518794a5d 130 // Setting is in the top 3 bits of the register.
JackyZhangFromSeeed 0:609518794a5d 131 regValue = regValue << 5;
JackyZhangFromSeeed 0:609518794a5d 132 //write(CONFIGURATION_REGISTERB, regValue);
JackyZhangFromSeeed 0:609518794a5d 133 cmdbuf[0] = CONFIGURATION_REGISTERB;
JackyZhangFromSeeed 0:609518794a5d 134 cmdbuf[1] = regValue;
JackyZhangFromSeeed 0:609518794a5d 135 suli_i2c_write(i2c, HMC5883L_ADDRESS, &cmdbuf[0], 2);
JackyZhangFromSeeed 0:609518794a5d 136
JackyZhangFromSeeed 0:609518794a5d 137 return true;
JackyZhangFromSeeed 0:609518794a5d 138 }
JackyZhangFromSeeed 0:609518794a5d 139
JackyZhangFromSeeed 0:609518794a5d 140 static void setMeasurementMode(I2C_T *i2c, uint8_t mode)
JackyZhangFromSeeed 0:609518794a5d 141 {
JackyZhangFromSeeed 0:609518794a5d 142 //write(MODE_REGISTER, mode);
JackyZhangFromSeeed 0:609518794a5d 143 cmdbuf[0] = MODE_REGISTER;
JackyZhangFromSeeed 0:609518794a5d 144 cmdbuf[1] = mode;
JackyZhangFromSeeed 0:609518794a5d 145 suli_i2c_write(i2c, HMC5883L_ADDRESS, cmdbuf, 2);
JackyZhangFromSeeed 0:609518794a5d 146 }