Tony YI
/
ESDC2014
123123123123123123123123123
compass.cpp@0:3417ca0a36c0, 2014-07-01 (annotated)
- Committer:
- TonyYI
- Date:
- Tue Jul 01 10:59:33 2014 +0000
- Revision:
- 0:3417ca0a36c0
- Child:
- 1:cbec1283a16a
2014
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TonyYI | 0:3417ca0a36c0 | 1 | #include "compass.h" |
TonyYI | 0:3417ca0a36c0 | 2 | |
TonyYI | 0:3417ca0a36c0 | 3 | HMC5883L::HMC5883L(PinName sda, PinName scl): i2c(sda, scl) |
TonyYI | 0:3417ca0a36c0 | 4 | { |
TonyYI | 0:3417ca0a36c0 | 5 | //100KHz, as specified by the datasheet. |
TonyYI | 0:3417ca0a36c0 | 6 | char rx; |
TonyYI | 0:3417ca0a36c0 | 7 | |
TonyYI | 0:3417ca0a36c0 | 8 | |
TonyYI | 0:3417ca0a36c0 | 9 | i2c.frequency(100000); |
TonyYI | 0:3417ca0a36c0 | 10 | //Testar depois com 400KHz |
TonyYI | 0:3417ca0a36c0 | 11 | //========================================================================================================== |
TonyYI | 0:3417ca0a36c0 | 12 | // Read chip_id |
TonyYI | 0:3417ca0a36c0 | 13 | //========================================================================================================== |
TonyYI | 0:3417ca0a36c0 | 14 | rx = Read(HMC5883L_IDENT_A); |
TonyYI | 0:3417ca0a36c0 | 15 | if (rx != 0x48)//ID do chip |
TonyYI | 0:3417ca0a36c0 | 16 | printf("\ninvalid chip id %d\r\n", rx); |
TonyYI | 0:3417ca0a36c0 | 17 | |
TonyYI | 0:3417ca0a36c0 | 18 | //========================================================================================================== |
TonyYI | 0:3417ca0a36c0 | 19 | // Let's set the Configuration Register A |
TonyYI | 0:3417ca0a36c0 | 20 | //========================================================================================================== |
TonyYI | 0:3417ca0a36c0 | 21 | // This register set's the number of samples averaged per measurement output, the rate at which data is written |
TonyYI | 0:3417ca0a36c0 | 22 | // to all three data output registers and the measurement flow of the device. |
TonyYI | 0:3417ca0a36c0 | 23 | // ------------------------------------------------------- |
TonyYI | 0:3417ca0a36c0 | 24 | // |CRA7 CRA6 CRA5 CRA4 CRA3 CRA2 CRA1 CRA0 | |
TonyYI | 0:3417ca0a36c0 | 25 | // |(1) MA1(1) MA0(1) DO2(1) DO1(0) DO0(0) MS1(0) MS0(0)| -> This is the default value |
TonyYI | 0:3417ca0a36c0 | 26 | // ------------------------------------------------------- |
TonyYI | 0:3417ca0a36c0 | 27 | // CRA7 -> we have to clear this bit for correct operation (0) |
TonyYI | 0:3417ca0a36c0 | 28 | // CRA6 to CRA5 -> Let's select the maximum number of samples averaged per measurement output (11) |
TonyYI | 0:3417ca0a36c0 | 29 | // CRA4 to CRA2 -> Also let's select the maximum data output rate (110) |
TonyYI | 0:3417ca0a36c0 | 30 | // CRA1 to CRA0 -> The measurement flow is defined to normal (00) |
TonyYI | 0:3417ca0a36c0 | 31 | // ------------------------------------------------------- |
TonyYI | 0:3417ca0a36c0 | 32 | // |CRA7 CRA6 CRA5 CRA4 CRA3 CRA2 CRA1 CRA0 | |
TonyYI | 0:3417ca0a36c0 | 33 | // |(0) MA1(1) MA0(1) DO2(1) DO1(1) DO0(0) MS1(0) MS0(0)| -> This is the new value, 0x78 in hex |
TonyYI | 0:3417ca0a36c0 | 34 | // ------------------------------------------------------- |
TonyYI | 0:3417ca0a36c0 | 35 | //Write(HMC5883L_CONFIG_A,0x78); |
TonyYI | 0:3417ca0a36c0 | 36 | //Write(HMC5883L_CONFIG_A,0x70); |
TonyYI | 0:3417ca0a36c0 | 37 | |
TonyYI | 0:3417ca0a36c0 | 38 | //========================================================================================================== |
TonyYI | 0:3417ca0a36c0 | 39 | // The Configuration Register B is set to 0010 0000 by default, this is a +/- 1.3 Ga sensor field range and |
TonyYI | 0:3417ca0a36c0 | 40 | // the gain of LSB/gauss is 1090. This is the maximum value, so let's leave it like that. |
TonyYI | 0:3417ca0a36c0 | 41 | //========================================================================================================== |
TonyYI | 0:3417ca0a36c0 | 42 | //Datasheet page 13. I will explain later |
TonyYI | 0:3417ca0a36c0 | 43 | //Write(HMC5883L_CONFIG_B,0x20); |
TonyYI | 0:3417ca0a36c0 | 44 | //Write(HMC5883L_CONFIG_B,0xA0); |
TonyYI | 0:3417ca0a36c0 | 45 | |
TonyYI | 0:3417ca0a36c0 | 46 | //========================================================================================================== |
TonyYI | 0:3417ca0a36c0 | 47 | // Let's set the Mode Register |
TonyYI | 0:3417ca0a36c0 | 48 | //========================================================================================================== |
TonyYI | 0:3417ca0a36c0 | 49 | // This register set's the operation mode, from continuous-measurements mode, single-measurement mode and idle mode. |
TonyYI | 0:3417ca0a36c0 | 50 | // We will set to Continuouse-measurement mode, so the device continuously performs measurements and places the |
TonyYI | 0:3417ca0a36c0 | 51 | // result in the data register |
TonyYI | 0:3417ca0a36c0 | 52 | // --------------------------------------------- |
TonyYI | 0:3417ca0a36c0 | 53 | // |MR7 MR6 MR5 MR4 MR3 MR2 MR1 MR0 | -> This is the new value, 0x78 in hex, we are going to change |
TonyYI | 0:3417ca0a36c0 | 54 | // |(1) (0) (0) (0) (0) (0) MD1(0) MD0(1)| the MD1 and MD0 to 00 and clear the MR7 for correct operation. |
TonyYI | 0:3417ca0a36c0 | 55 | // --------------------------------------------- The final value is 0000 0000 (0x00). |
TonyYI | 0:3417ca0a36c0 | 56 | Write(HMC5883L_MODE,0x00); |
TonyYI | 0:3417ca0a36c0 | 57 | wait(1); |
TonyYI | 0:3417ca0a36c0 | 58 | setDeclination(DECLINATIONANGLE); |
TonyYI | 0:3417ca0a36c0 | 59 | } |
TonyYI | 0:3417ca0a36c0 | 60 | |
TonyYI | 0:3417ca0a36c0 | 61 | |
TonyYI | 0:3417ca0a36c0 | 62 | void HMC5883L::Write(char reg_address, char data) |
TonyYI | 0:3417ca0a36c0 | 63 | { |
TonyYI | 0:3417ca0a36c0 | 64 | char tx[2]; |
TonyYI | 0:3417ca0a36c0 | 65 | tx[0]=reg_address; |
TonyYI | 0:3417ca0a36c0 | 66 | tx[1]=data; |
TonyYI | 0:3417ca0a36c0 | 67 | |
TonyYI | 0:3417ca0a36c0 | 68 | i2c.write(HMC5883L_I2C_WRITE,tx,2); |
TonyYI | 0:3417ca0a36c0 | 69 | } |
TonyYI | 0:3417ca0a36c0 | 70 | |
TonyYI | 0:3417ca0a36c0 | 71 | char HMC5883L::Read(char data) |
TonyYI | 0:3417ca0a36c0 | 72 | { |
TonyYI | 0:3417ca0a36c0 | 73 | char tx = data; |
TonyYI | 0:3417ca0a36c0 | 74 | char rx; |
TonyYI | 0:3417ca0a36c0 | 75 | |
TonyYI | 0:3417ca0a36c0 | 76 | i2c.write(HMC5883L_I2C_WRITE, &tx, 1); |
TonyYI | 0:3417ca0a36c0 | 77 | i2c.read(HMC5883L_I2C_READ, &rx, 1); |
TonyYI | 0:3417ca0a36c0 | 78 | return rx; |
TonyYI | 0:3417ca0a36c0 | 79 | } |
TonyYI | 0:3417ca0a36c0 | 80 | |
TonyYI | 0:3417ca0a36c0 | 81 | void HMC5883L::MultiByteRead(char address, char* output, int size) |
TonyYI | 0:3417ca0a36c0 | 82 | { |
TonyYI | 0:3417ca0a36c0 | 83 | i2c.write(HMC5883L_I2C_WRITE, &address, 1); //tell it where to read from |
TonyYI | 0:3417ca0a36c0 | 84 | i2c.read(HMC5883L_I2C_READ, output, size); //tell it where to store the data read |
TonyYI | 0:3417ca0a36c0 | 85 | } |
TonyYI | 0:3417ca0a36c0 | 86 | |
TonyYI | 0:3417ca0a36c0 | 87 | float HMC5883L::getMx() |
TonyYI | 0:3417ca0a36c0 | 88 | { |
TonyYI | 0:3417ca0a36c0 | 89 | //return (x * m_Scale); |
TonyYI | 0:3417ca0a36c0 | 90 | char lsb_byte = 0; |
TonyYI | 0:3417ca0a36c0 | 91 | signed short msb_byte; |
TonyYI | 0:3417ca0a36c0 | 92 | |
TonyYI | 0:3417ca0a36c0 | 93 | lsb_byte = Read(HMC5883L_X_MSB); |
TonyYI | 0:3417ca0a36c0 | 94 | msb_byte = lsb_byte << 8; |
TonyYI | 0:3417ca0a36c0 | 95 | msb_byte |= Read(HMC5883L_X_LSB); |
TonyYI | 0:3417ca0a36c0 | 96 | return (float)msb_byte; |
TonyYI | 0:3417ca0a36c0 | 97 | /* |
TonyYI | 0:3417ca0a36c0 | 98 | char tx[1]; |
TonyYI | 0:3417ca0a36c0 | 99 | char rx[2]; |
TonyYI | 0:3417ca0a36c0 | 100 | |
TonyYI | 0:3417ca0a36c0 | 101 | |
TonyYI | 0:3417ca0a36c0 | 102 | tx[0]=HMC5883L_X_MSB; |
TonyYI | 0:3417ca0a36c0 | 103 | i2c.write(HMC5883L_I2C_READ,tx,1); |
TonyYI | 0:3417ca0a36c0 | 104 | i2c.read(HMC5883L_I2C_READ,rx,2); |
TonyYI | 0:3417ca0a36c0 | 105 | return ((int)rx[0]<<8|(int)rx[1]); |
TonyYI | 0:3417ca0a36c0 | 106 | */ |
TonyYI | 0:3417ca0a36c0 | 107 | |
TonyYI | 0:3417ca0a36c0 | 108 | } |
TonyYI | 0:3417ca0a36c0 | 109 | |
TonyYI | 0:3417ca0a36c0 | 110 | float HMC5883L::getMy() |
TonyYI | 0:3417ca0a36c0 | 111 | { |
TonyYI | 0:3417ca0a36c0 | 112 | //return (y * m_Scale); |
TonyYI | 0:3417ca0a36c0 | 113 | |
TonyYI | 0:3417ca0a36c0 | 114 | char lsb_byte = 0; |
TonyYI | 0:3417ca0a36c0 | 115 | signed short msb_byte; |
TonyYI | 0:3417ca0a36c0 | 116 | |
TonyYI | 0:3417ca0a36c0 | 117 | lsb_byte = Read(HMC5883L_Y_MSB); |
TonyYI | 0:3417ca0a36c0 | 118 | msb_byte = lsb_byte << 8; |
TonyYI | 0:3417ca0a36c0 | 119 | msb_byte |= Read(HMC5883L_Y_LSB); |
TonyYI | 0:3417ca0a36c0 | 120 | return (float)msb_byte; |
TonyYI | 0:3417ca0a36c0 | 121 | } |
TonyYI | 0:3417ca0a36c0 | 122 | |
TonyYI | 0:3417ca0a36c0 | 123 | |
TonyYI | 0:3417ca0a36c0 | 124 | float HMC5883L::getMz() |
TonyYI | 0:3417ca0a36c0 | 125 | { |
TonyYI | 0:3417ca0a36c0 | 126 | //return (z * m_Scale); |
TonyYI | 0:3417ca0a36c0 | 127 | |
TonyYI | 0:3417ca0a36c0 | 128 | char lsb_byte = 0; |
TonyYI | 0:3417ca0a36c0 | 129 | signed short msb_byte; |
TonyYI | 0:3417ca0a36c0 | 130 | |
TonyYI | 0:3417ca0a36c0 | 131 | lsb_byte = Read(HMC5883L_Z_MSB); |
TonyYI | 0:3417ca0a36c0 | 132 | msb_byte = lsb_byte << 8; |
TonyYI | 0:3417ca0a36c0 | 133 | msb_byte |= Read(HMC5883L_Z_LSB); |
TonyYI | 0:3417ca0a36c0 | 134 | return (float)msb_byte; |
TonyYI | 0:3417ca0a36c0 | 135 | } |
TonyYI | 0:3417ca0a36c0 | 136 | |
TonyYI | 0:3417ca0a36c0 | 137 | |
TonyYI | 0:3417ca0a36c0 | 138 | void HMC5883L::setDeclination(float declinationAngle) |
TonyYI | 0:3417ca0a36c0 | 139 | { |
TonyYI | 0:3417ca0a36c0 | 140 | this->declinationAngle=declinationAngle; |
TonyYI | 0:3417ca0a36c0 | 141 | } |
TonyYI | 0:3417ca0a36c0 | 142 | |
TonyYI | 0:3417ca0a36c0 | 143 | void HMC5883L::setOffset(int offset) |
TonyYI | 0:3417ca0a36c0 | 144 | { |
TonyYI | 0:3417ca0a36c0 | 145 | this->offset=offset; |
TonyYI | 0:3417ca0a36c0 | 146 | } |
TonyYI | 0:3417ca0a36c0 | 147 | |
TonyYI | 0:3417ca0a36c0 | 148 | unsigned short HMC5883L::get_degree() |
TonyYI | 0:3417ca0a36c0 | 149 | { |
TonyYI | 0:3417ca0a36c0 | 150 | //return (z * m_Scale); |
TonyYI | 0:3417ca0a36c0 | 151 | wait_ms(200); |
TonyYI | 0:3417ca0a36c0 | 152 | |
TonyYI | 0:3417ca0a36c0 | 153 | float x, y, z, heading; |
TonyYI | 0:3417ca0a36c0 | 154 | x = getMx(); |
TonyYI | 0:3417ca0a36c0 | 155 | y = getMy(); |
TonyYI | 0:3417ca0a36c0 | 156 | z = getMz(); |
TonyYI | 0:3417ca0a36c0 | 157 | heading += declinationAngle; |
TonyYI | 0:3417ca0a36c0 | 158 | |
TonyYI | 0:3417ca0a36c0 | 159 | heading = atan2(y, x); |
TonyYI | 0:3417ca0a36c0 | 160 | if(heading < 0) |
TonyYI | 0:3417ca0a36c0 | 161 | heading += 2*PI; |
TonyYI | 0:3417ca0a36c0 | 162 | if(heading > 2*PI) |
TonyYI | 0:3417ca0a36c0 | 163 | heading -= 2*PI; |
TonyYI | 0:3417ca0a36c0 | 164 | |
TonyYI | 0:3417ca0a36c0 | 165 | heading = heading * 180 / PI; |
TonyYI | 0:3417ca0a36c0 | 166 | |
TonyYI | 0:3417ca0a36c0 | 167 | // heading+=90; |
TonyYI | 0:3417ca0a36c0 | 168 | |
TonyYI | 0:3417ca0a36c0 | 169 | heading+=offset; |
TonyYI | 0:3417ca0a36c0 | 170 | if(heading>=360) |
TonyYI | 0:3417ca0a36c0 | 171 | heading-=360; |
TonyYI | 0:3417ca0a36c0 | 172 | |
TonyYI | 0:3417ca0a36c0 | 173 | if(heading<180) |
TonyYI | 0:3417ca0a36c0 | 174 | heading+=180; |
TonyYI | 0:3417ca0a36c0 | 175 | else |
TonyYI | 0:3417ca0a36c0 | 176 | heading-=180; |
TonyYI | 0:3417ca0a36c0 | 177 | return heading; |
TonyYI | 0:3417ca0a36c0 | 178 | } |
TonyYI | 0:3417ca0a36c0 | 179 | |
TonyYI | 0:3417ca0a36c0 | 180 |