Tony YI
/
ESDC2014
123123123123123123123123123
Diff: compass.cpp
- Revision:
- 1:cbec1283a16a
- Parent:
- 0:3417ca0a36c0
- Child:
- 2:442902ec3aa1
--- a/compass.cpp Tue Jul 01 10:59:33 2014 +0000 +++ b/compass.cpp Thu Jul 03 03:56:29 2014 +0000 @@ -1,180 +1,58 @@ #include "compass.h" -HMC5883L::HMC5883L(PinName sda, PinName scl): i2c(sda, scl) +COMPASS::COMPASS(PinName tx, PinName rx) { - //100KHz, as specified by the datasheet. - char rx; - - - i2c.frequency(100000); - //Testar depois com 400KHz - //========================================================================================================== - // Read chip_id - //========================================================================================================== - rx = Read(HMC5883L_IDENT_A); - if (rx != 0x48)//ID do chip - printf("\ninvalid chip id %d\r\n", rx); - - //========================================================================================================== - // Let's set the Configuration Register A - //========================================================================================================== - // This register set's the number of samples averaged per measurement output, the rate at which data is written - // to all three data output registers and the measurement flow of the device. - // ------------------------------------------------------- - // |CRA7 CRA6 CRA5 CRA4 CRA3 CRA2 CRA1 CRA0 | - // |(1) MA1(1) MA0(1) DO2(1) DO1(0) DO0(0) MS1(0) MS0(0)| -> This is the default value - // ------------------------------------------------------- - // CRA7 -> we have to clear this bit for correct operation (0) - // CRA6 to CRA5 -> Let's select the maximum number of samples averaged per measurement output (11) - // CRA4 to CRA2 -> Also let's select the maximum data output rate (110) - // CRA1 to CRA0 -> The measurement flow is defined to normal (00) - // ------------------------------------------------------- - // |CRA7 CRA6 CRA5 CRA4 CRA3 CRA2 CRA1 CRA0 | - // |(0) MA1(1) MA0(1) DO2(1) DO1(1) DO0(0) MS1(0) MS0(0)| -> This is the new value, 0x78 in hex - // ------------------------------------------------------- - //Write(HMC5883L_CONFIG_A,0x78); - //Write(HMC5883L_CONFIG_A,0x70); - - //========================================================================================================== - // The Configuration Register B is set to 0010 0000 by default, this is a +/- 1.3 Ga sensor field range and - // the gain of LSB/gauss is 1090. This is the maximum value, so let's leave it like that. - //========================================================================================================== - //Datasheet page 13. I will explain later - //Write(HMC5883L_CONFIG_B,0x20); - //Write(HMC5883L_CONFIG_B,0xA0); - - //========================================================================================================== - // Let's set the Mode Register - //========================================================================================================== - // This register set's the operation mode, from continuous-measurements mode, single-measurement mode and idle mode. - // We will set to Continuouse-measurement mode, so the device continuously performs measurements and places the - // result in the data register - // --------------------------------------------- - // |MR7 MR6 MR5 MR4 MR3 MR2 MR1 MR0 | -> This is the new value, 0x78 in hex, we are going to change - // |(1) (0) (0) (0) (0) (0) MD1(0) MD0(1)| the MD1 and MD0 to 00 and clear the MR7 for correct operation. - // --------------------------------------------- The final value is 0000 0000 (0x00). - Write(HMC5883L_MODE,0x00); - wait(1); - setDeclination(DECLINATIONANGLE); + this->serial= new MySerial(tx,rx); + serial->baud(56000); + serial->format(8,SerialBase::None, 1); + init(); } -void HMC5883L::Write(char reg_address, char data) -{ - char tx[2]; - tx[0]=reg_address; - tx[1]=data; - - i2c.write(HMC5883L_I2C_WRITE,tx,2); -} - -char HMC5883L::Read(char data) +uint16_t COMPASS::Read() { - char tx = data; - char rx; - - i2c.write(HMC5883L_I2C_WRITE, &tx, 1); - i2c.read(HMC5883L_I2C_READ, &rx, 1); - return rx; -} - -void HMC5883L::MultiByteRead(char address, char* output, int size) -{ - i2c.write(HMC5883L_I2C_WRITE, &address, 1); //tell it where to read from - i2c.read(HMC5883L_I2C_READ, output, size); //tell it where to store the data read + resume(); + buffer[0]=serial->getc(); + buffer[1]=serial->getc(); + stop(); + degree=(uint8_t)(buffer[0])*256+(uint8_t)(buffer[1]); + return degree; } -float HMC5883L::getMx() +void COMPASS::init() { - //return (x * m_Scale); - char lsb_byte = 0; - signed short msb_byte; - - lsb_byte = Read(HMC5883L_X_MSB); - msb_byte = lsb_byte << 8; - msb_byte |= Read(HMC5883L_X_LSB); - return (float)msb_byte; - /* - char tx[1]; - char rx[2]; - - - tx[0]=HMC5883L_X_MSB; - i2c.write(HMC5883L_I2C_READ,tx,1); - i2c.read(HMC5883L_I2C_READ,rx,2); - return ((int)rx[0]<<8|(int)rx[1]); - */ - + run(); + stop(); } -float HMC5883L::getMy() +void COMPASS::run() { - //return (y * m_Scale); - - char lsb_byte = 0; - signed short msb_byte; - - lsb_byte = Read(HMC5883L_Y_MSB); - msb_byte = lsb_byte << 8; - msb_byte |= Read(HMC5883L_Y_LSB); - return (float)msb_byte; + write2Bytes(RUN_MSB,RUN_LSB); } -float HMC5883L::getMz() +void COMPASS::stop() { - //return (z * m_Scale); - - char lsb_byte = 0; - signed short msb_byte; + write2Bytes(STOP_MSB,STOP_LSB); +} - lsb_byte = Read(HMC5883L_Z_MSB); - msb_byte = lsb_byte << 8; - msb_byte |= Read(HMC5883L_Z_LSB); - return (float)msb_byte; - } - -void HMC5883L::setDeclination(float declinationAngle) -{ - this->declinationAngle=declinationAngle; - } - - void HMC5883L::setOffset(int offset) +void COMPASS::resume() { - this->offset=offset; - } - -unsigned short HMC5883L::get_degree() -{ - //return (z * m_Scale); - wait_ms(200); + write2Bytes(RESUME_MSB,RESUME_LSB); +} + - float x, y, z, heading; - x = getMx(); - y = getMy(); - z = getMz(); - heading += declinationAngle; - - heading = atan2(y, x); - if(heading < 0) - heading += 2*PI; - if(heading > 2*PI) - heading -= 2*PI; - - heading = heading * 180 / PI; - -// heading+=90; - - heading+=offset; - if(heading>=360) - heading-=360; - - if(heading<180) - heading+=180; - else - heading-=180; - return heading; - } +void COMPASS::reset() +{ + write2Bytes(RST_MSB,RST_LSB); +} + +void COMPASS::write2Bytes(char msb, char lsb) +{ + serial->putc(msb); + serial->putc(lsb); + +} + -