Pileus Git - ~andy/csm213a-hw/blob - hw1/mag.cc

   1 #include "mbed.h"
   2 #include "mag.h"
   3
   4 /******************************************************************************
   5  * Constructors
   6  ******************************************************************************/
   7 MAG3110::MAG3110(PinName sda, PinName scl, int addr): _i2c(sda, scl),
   8     _i2c_address(addr), _pc(NULL), _debug(false)
   9 {
  10     begin();
  11 }
  12
  13 MAG3110::MAG3110(PinName sda, PinName scl, int addr, Serial *pc): _i2c(sda, scl),
  14    _i2c_address(addr), _pc(pc), _debug(true)
  15 {
  16     begin();
  17 }
  18
  19 void MAG3110::begin()
  20 {
  21     char cmd[2];
  22
  23     cmd[0] = MAG_CTRL_REG2;
  24     cmd[1] = 0x80;
  25     _i2c.write(_i2c_address, cmd, 2);
  26
  27     cmd[0] = MAG_CTRL_REG1;
  28     cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE;
  29     _i2c.write(_i2c_address, cmd, 2);
  30
  31     // No adjustment initially
  32     _avgX = 0;
  33     _avgY = 0;
  34 }
  35
  36 // Read a single byte form 8 bit register, return as int
  37 int MAG3110::readReg(char regAddr)
  38 {
  39     char cmd[1];
  40
  41     cmd[0] = regAddr;
  42     _i2c.write(_i2c_address, cmd, 1);
  43
  44     cmd[0] = 0x00;
  45     _i2c.read(_i2c_address, cmd, 1);
  46     return (int)( cmd[0]);
  47 }
  48
  49
  50 // read a register per, pass first reg value, reading 2 bytes increments register
  51 // Reads MSB first then LSB
  52 int MAG3110::readVal(char regAddr)
  53 {
  54     char cmd[2];
  55
  56     cmd[0] = regAddr;
  57     _i2c.write(_i2c_address, cmd, 1);
  58
  59     cmd[0] = 0x00;
  60     cmd[1] = 0x00;
  61     _i2c.read(_i2c_address, cmd, 2);
  62     return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB
  63 }
  64
  65
  66 float MAG3110::getHeading()
  67 {
  68     int xVal = readVal(MAG_OUT_X_MSB);
  69     int yVal = readVal(MAG_OUT_Y_MSB);
  70     return (atan2((double)(yVal - _avgY),(double)(xVal - _avgX)))*180/PI;
  71 }
  72
  73 void MAG3110::getValues(int *xVal, int *yVal, int *zVal)
  74 {
  75     *xVal = readVal(MAG_OUT_X_MSB);
  76     *yVal = readVal(MAG_OUT_Y_MSB);
  77     *zVal = readVal(MAG_OUT_Z_MSB);
  78 }
  79
  80 int16_t MAG3110::getMagX()
  81 {
  82     return readVal(MAG_OUT_X_MSB);
  83 }
  84
  85 int16_t MAG3110::getMagY()
  86 {
  87     return readVal(MAG_OUT_Y_MSB);
  88 }
  89
  90 int16_t MAG3110::getMagZ()
  91 {
  92     return readVal(MAG_OUT_Z_MSB);
  93 }
  94
  95 void MAG3110::setCalibration(int minX, int maxX, int minY, int maxY )
  96 {
  97     _avgX=(maxX+minX)/2;
  98     _avgY=(maxY+minY)/2;
  99 }
 100
 101
 102
 103
 104