Tony YI / Mbed 2 deprecated ESDC2014

123123123123123123123123123

Dependencies:   mbed

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?

UserRevisionLine numberNew 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