MAG3110.cpp

00001 
00002 #include "MAG3110.h"
00003 #include "mbed.h"
00004 #include "MotionSensor.h"
00005 
00006 /******************************************************************************
00007  * Constructors
00008  ******************************************************************************/
00009 MAG3110::MAG3110(PinName sda, PinName scl): m_i2c(sda, scl), 
00010     m_addr(0x1d)
00011 {
00012     char cmd[2];
00013 
00014     cmd[0] = MAG_CTRL_REG2;
00015     cmd[1] = 0x80;
00016     m_i2c.write(m_addr, cmd, 2);
00017 
00018 }
00019 
00020 void MAG3110::enable(void) {
00021     uint8_t data[2];
00022     readRegs( MAG_CTRL_REG1, &data[1], 1);
00023     data[1] |= 0x01;
00024     data[0] = MAG_CTRL_REG1;
00025     writeRegs(data, 2);
00026 }
00027 
00028 
00029 void MAG3110::disable(void) {
00030     uint8_t data[2];
00031     readRegs( MAG_CTRL_REG1, &data[1], 1);
00032     data[1] &= 0xFE;
00033     data[0] = MAG_CTRL_REG1;
00034     writeRegs(data, 2);
00035 }
00036 
00037 
00038 void MAG3110::readRegs(int addr, uint8_t * data, int len)
00039 {
00040     char cmd[1];
00041 
00042     cmd[0] = addr;
00043     m_i2c.write( m_addr, cmd, 1, true);
00044     m_i2c.read( m_addr, (char *) data, len);
00045     return;
00046 }
00047 
00048 
00049 void MAG3110::writeRegs(uint8_t * data, int len) {
00050     m_i2c.write(m_addr, (char *)data, len);
00051 }
00052 
00053 
00054 uint32_t MAG3110::whoAmI() {
00055     uint8_t who_am_i = 0;
00056     readRegs(MAG_WHOAMI, &who_am_i, 1);
00057     return (uint32_t) who_am_i;
00058 }
00059 
00060 uint32_t MAG3110::dataReady(void) {
00061     uint8_t stat = 0;
00062     readRegs(MAG_DR_STATUS, &stat, 1);
00063     return (uint32_t) stat;
00064 }
00065 
00066 uint32_t MAG3110::sampleRate(uint32_t f) {
00067     return(50); // for now sample rate is fixed at 50Hz
00068 }
00069 
00070 
00071 void MAG3110::getX(float * x) {
00072     *x = (float(getMagAxis(MAG_OUT_X_MSB)) * 0.1f);
00073 }
00074 
00075 void MAG3110::getY(float * y) {
00076     *y = (float(getMagAxis(MAG_OUT_Y_MSB)) * 0.1f);
00077 }
00078 
00079 void MAG3110::getZ(float * z) {
00080     *z = (float(getMagAxis(MAG_OUT_Z_MSB)) * 0.1f);
00081 }
00082 
00083 void MAG3110::getX(int16_t * d) {
00084     *d = getMagAxis(MAG_OUT_X_MSB);
00085 }
00086 
00087 void MAG3110::getY(int16_t * d) {
00088     *d = getMagAxis(MAG_OUT_Y_MSB);
00089 }
00090 
00091 void MAG3110::getZ(int16_t * d) {
00092     *d = getMagAxis(MAG_OUT_Z_MSB);
00093 }
00094 
00095 int16_t MAG3110::getMagAxis(uint8_t addr) {
00096     int16_t acc;
00097     uint8_t res[2];
00098     readRegs(addr, res, 2);
00099 
00100     acc = (res[0] << 8) | res[1];
00101 
00102     return acc;
00103 }
00104 
00105 
00106 void MAG3110::getAxis(MotionSensorDataUnits &data) {
00107     int16_t t[3];
00108     uint8_t res[6];
00109 
00110     readRegs(MAG_OUT_X_MSB, res, 6);
00111     t[0] = (res[0] << 8) | res[1];
00112     t[1] = (res[2] << 8) | res[3];
00113     t[2] = (res[4] << 8) | res[5];
00114     data.x = ((float) t[0]) * 0.1f;
00115     data.y = ((float) t[1]) * 0.1f;
00116     data.z = ((float) t[2]) * 0.1f;
00117 }
00118 
00119 
00120 void MAG3110::getAxis(MotionSensorDataCounts &data) {
00121 
00122     uint8_t res[6];
00123     readRegs(MAG_OUT_X_MSB, res, 6);
00124 
00125     data.x = (res[0] << 8) | res[1];
00126     data.y = (res[2] << 8) | res[3];
00127     data.z = (res[4] << 8) | res[5];
00128 }
00129 /*
00130 
00131 // read a register per, pass first reg value, reading 2 bytes increments register
00132 // Reads MSB first then LSB
00133 int MAG3110::readVal(char regAddr)
00134 {
00135     char cmd[2];
00136     int16_t t;
00137     cmd[0] = regAddr;
00138     if(_i2c.write(m_addr, cmd, 1)) {
00139         printf("MAG3110 write error\r\n");
00140         _i2c.stop();
00141         _i2c.start();
00142         }
00143 
00144     cmd[0] = 0x00;
00145     cmd[1] = 0x00;
00146     _i2c.read(m_addr, cmd, 2);
00147     t = (cmd[0] * 256) + (unsigned short) cmd[1];
00148     return ((int) t); //concatenate the MSB and LSB
00149 }
00150 
00151 
00152 float MAG3110::getHeading()
00153 {
00154     int xVal = readVal(MAG_OUT_X_MSB);
00155     int yVal = readVal(MAG_OUT_Y_MSB);
00156     return (atan2((double)(yVal - _avgY),(double)(xVal - _avgX)))*180/PI;
00157 }
00158 
00159 void MAG3110::getValues(int *xVal, int *yVal, int *zVal)
00160 {
00161     *xVal = readVal(MAG_OUT_X_MSB);
00162     *yVal = readVal(MAG_OUT_Y_MSB);
00163     *zVal = readVal(MAG_OUT_Z_MSB);
00164 }
00165 
00166 void MAG3110::ReadXYZ(float * mag)
00167 {
00168     int x, y, z;
00169     x = readVal(MAG_OUT_X_MSB);
00170     y = readVal(MAG_OUT_Y_MSB);
00171     z = readVal(MAG_OUT_Z_MSB);
00172     mag[0] = (float) x / 10.0;
00173     mag[1] = (float) y / 10.0;
00174     mag[2] = (float) z / 10.0;
00175     
00176 }
00177 
00178 void MAG3110::ReadXYZraw(int16_t * mag_raw)
00179 {
00180     mag_raw[0] = readVal(MAG_OUT_X_MSB);
00181     mag_raw[1] = readVal(MAG_OUT_Y_MSB);
00182     mag_raw[2] = readVal(MAG_OUT_Z_MSB);
00183 }
00184 
00185 void MAG3110::setCalibration(int minX, int maxX, int minY, int maxY )
00186 {
00187     _avgX=(maxX+minX)/2;
00188     _avgY=(maxY+minY)/2;
00189 }
00190 */
00191 
00192 
00193 
00194