Tony YI
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);
+
+}
+
-