text: #include "Accelerometer_Compass_LSM303D.h"
  #include <Wire.h>
  #include <SPI.h>
  
  /* LSM303 Address definitions */
  #define LSM303D_ADDR  0x1E  // assuming SA0 grounded
  
  /* LSM303 Register definitions */
  #define TEMP_OUT_L		0x05
  #define TEMP_OUT_H		0x06
  #define	STATUS_REG_M	0x07
  #define OUT_X_L_M 		0x08
  #define OUT_X_H_M 		0x09
  #define OUT_Y_L_M 		0x0A
  #define OUT_Y_H_M 		0x0B
  #define OUT_Z_L_M 		0x0C
  #define OUT_Z_H_M 		0x0D
  #define	WHO_AM_I		0x0F
  #define	INT_CTRL_M		0x12
  #define	INT_SRC_M		0x13
  #define	INT_THS_L_M		0x14
  #define	INT_THS_H_M		0x15
  #define	OFFSET_X_L_M	0x16
  #define	OFFSET_X_H_M	0x17
  #define	OFFSET_Y_L_M	0x18
  #define	OFFSET_Y_H_M	0x19
  #define	OFFSET_Z_L_M	0x1A
  #define	OFFSET_Z_H_M	0x1B
  #define REFERENCE_X 	0x1C
  #define REFERENCE_Y 	0x1D
  #define REFERENCE_Z 	0x1E
  #define CTRL_REG0 		0x1F
  #define CTRL_REG1 		0x20
  #define CTRL_REG2		0x21
  #define CTRL_REG3 		0x22
  #define CTRL_REG4 		0x23
  #define CTRL_REG5 		0x24
  #define CTRL_REG6 		0x25
  #define CTRL_REG7 		0x26
  #define	STATUS_REG_A	0x27
  #define OUT_X_L_A 		0x28
  #define OUT_X_H_A 		0x29
  #define OUT_Y_L_A 		0x2A
  #define OUT_Y_H_A 		0x2B
  #define OUT_Z_L_A 		0x2C
  #define OUT_Z_H_A 		0x2D
  #define	FIFO_CTRL		0x2E
  #define	FIFO_SRC		0x2F
  #define	IG_CFG1			0x30
  #define	IG_SRC1			0x31
  #define	IG_THS1			0x32
  #define	IG_DUR1			0x33
  #define	IG_CFG2			0x34
  #define	IG_SRC2			0x35
  #define	IG_THS2			0x36
  #define	IG_DUR2			0x37
  #define	CLICK_CFG		0x38
  #define	CLICK_SRC		0x39
  #define	CLICK_THS		0x3A
  #define	TIME_LIMIT		0x3B
  #define	TIME_LATENCY	0x3C
  #define	TIME_WINDOW		0x3D
  #define	ACT_THS			0x3E
  #define	ACT_DUR			0x3F
  
  #define MAG_SCALE_2 	0x00//full-scale is +/-2Gauss
  #define MAG_SCALE_4 	0x20//+/-4Gauss
  #define MAG_SCALE_8 	0x40//+/-8Gauss
  #define MAG_SCALE_12 	0x60//+/-12Gauss
  
  byte Read  = 0B10000000;
  byte Write = 0B00000000;
  
  //I2C mode
  char LSM303D::initI2C()
  {
  	char rtn = -1;
      
      _mode = 0;//I2C mode
  	Wire.begin();  // Start up I2C, required for LSM303 communication
  	rtn = config();
      
  	return rtn;
  }
  
  //SPI mode
  char LSM303D::initSPI(char cspin)
  {
  	char rtn = -1;
      
      _mode = 1;//SPI mode
      _cs = cspin;
      pinMode(_cs, OUTPUT);//initialize the chip select pins;
      SPI.begin();//start the SPI library;
      rtn = config();
      
  	return rtn;
  }
  
  char LSM303D::config()
  {
      char rtn = -1;
      
      if(read(WHO_AM_I) != 0x49) return rtn; // return wrong if no LSM303D was found 
      
  	write(0x57, CTRL_REG1);  // 0x57 = ODR=50hz, all accel axes on
  	write((3<<6)|(0<<3), CTRL_REG2);  // set full-scale
  	write(0x00, CTRL_REG3);  // no interrupt
  	write(0x00, CTRL_REG4);  // no interrupt
  	write((4<<2), CTRL_REG5);  // 0x10 = mag 50Hz output rate
  	write(MAG_SCALE_2, CTRL_REG6); //magnetic scale = +/-1.3Gauss
  	write(0x00, CTRL_REG7);  // 0x00 = continouous conversion mode
  	rtn = 0;
      
  	return rtn;
  }
  
  unsigned char LSM303D::read(unsigned char address)
  {
  	char temp = 0x00;
      
      if(_mode == 0)//I2C mode
      {
          Wire.beginTransmission(LSM303D_ADDR);
          Wire.write(address);
          Wire.endTransmission();
          Wire.requestFrom(LSM303D_ADDR, 1);
          while(!Wire.available());
          temp = Wire.read();
          Wire.endTransmission();
      }
      else//SPI Mode
      {
          digitalWrite(_cs, LOW); 
          SPI.transfer(Read | address);
          temp = SPI.transfer(0x00);
          digitalWrite(_cs, HIGH);
      }
      
  	return temp;
  }
  
  void LSM303D::write(unsigned char data, unsigned char address)
  {
      if(_mode == 0)
      {
          Wire.beginTransmission(LSM303D_ADDR); 
          Wire.write(address);
          Wire.write(data);
          Wire.endTransmission();
      }
      else
      {
          digitalWrite(_cs, LOW);
          SPI.transfer(Write | address);
          SPI.transfer(data);
          digitalWrite(_cs, HIGH);
      }
  }
  
  char LSM303D::isMagReady()
  {
  	char temp;
      
  	temp = read(STATUS_REG_M) & 0x03;
      
  	return temp;
  }
  
  void LSM303D::getMag(int * rawValues)
  {
  	rawValues[X] = ((int)read(OUT_X_H_M) << 8) | (read(OUT_X_L_M));
  	rawValues[Y] = ((int)read(OUT_Y_H_M) << 8) | (read(OUT_Y_L_M));
  	rawValues[Z] = ((int)read(OUT_Z_H_M) << 8) | (read(OUT_Z_L_M));
  }
  
  void LSM303D::getAccel(int * rawValues)
  {
  	rawValues[X] = ((int)read(OUT_X_H_A) << 8) | (read(OUT_X_L_A));
  	rawValues[Y] = ((int)read(OUT_Y_H_A) << 8) | (read(OUT_Y_L_A));
  	rawValues[Z] = ((int)read(OUT_Z_H_A) << 8) | (read(OUT_Z_L_A));
  }
  
  float LSM303D::getHeading(int * magValue)
  {
  	// see section 1.2 in app note AN3192
  	float heading = 180*atan2(magValue[Y], magValue[X])/PI;  // assume pitch, roll are 0
  
  	if (heading <0)
  	heading += 360;
  
  	return heading;
  }
  
  float LSM303D::getTiltHeading(int * magValue, float * accelValue)
  {
  	// see appendix A in app note AN3192 
  	float pitch = asin(-accelValue[X]);
  	float roll = asin(accelValue[Y]/cos(pitch));
  
  	float xh = magValue[X] * cos(pitch) + magValue[Z] * sin(pitch);
  	float yh = magValue[X] * sin(roll) * sin(pitch) + magValue[Y] * cos(roll) - magValue[Z] * sin(roll) * cos(pitch);
  	float zh = -magValue[X] * cos(roll) * sin(pitch) + magValue[Y] * sin(roll) + magValue[Z] * cos(roll) * cos(pitch);
  	float heading = 180 * atan2(yh, xh)/PI;
  
  	if (yh >= 0)
  		return heading;
  	else
  		return (360 + heading);
  }
  
  void LSM303D::accelerometerLoop()
  {
      getAccel(this->accel);
      while(!isMagReady());// wait for the magnetometer readings to be ready
      getMag(this->mag);  // get the magnetometer values, store them in mag
      
      for (int i=0; i<3; i++)
      {
          this->realAccel[i] = this->accel[i] / pow(2, 15) * ACCELE_SCALE;  // calculate real acceleration values, in units of g
      }
      this->heading = getHeading(this->mag);
      this->titleHeading = getTiltHeading(this->mag, this->realAccel);
  }
  
  float LSM303D::getXAxisAcceleration()
  {
      getAccel(this->accel);
      this->realAccel[0] = this->accel[0] / pow(2, 15) * ACCELE_SCALE;
      return this->realAccel[0];
  }
  
  float LSM303D::getYAxisAcceleration()
  {
      getAccel(this->accel);
      this->realAccel[1] = this->accel[1] / pow(2, 15) * ACCELE_SCALE;
      return this->realAccel[1];
  }
  
  float LSM303D::getZAxisAcceleration()
  {
      getAccel(this->accel);
      this->realAccel[2] = this->accel[2] / pow(2, 15) * ACCELE_SCALE;
      return this->realAccel[2];
  }
  
  //The clockwise angle between the magnetic north and x-axis:
  float LSM303D::getAngleMagNorthANDXAxis()
  {
      getAccel(this->accel);
      while(!isMagReady());// wait for the magnetometer readings to be ready
      getMag(this->mag);  // get the magnetometer values, store them in mag
      
      for (int i=0; i<3; i++)
      {
          this->realAccel[i] = this->accel[i] / pow(2, 15) * ACCELE_SCALE;  // calculate real acceleration values, in units of g
      }
      this->heading = getHeading(this->mag);
      return this->heading;
  }
  
  //The clockwise angle between the magnetic north and the projection
  //of the positive x-axis in the horizontal plane:
  float LSM303D::getAngleMagNorthANDXAxisHorizPlane()
  {
      getAccel(this->accel);
      while(!isMagReady());// wait for the magnetometer readings to be ready
      getMag(this->mag);  // get the magnetometer values, store them in mag
      
      for (int i=0; i<3; i++)
      {
          this->realAccel[i] = this->accel[i] / pow(2, 15) * ACCELE_SCALE;  // calculate real acceleration values, in units of g
      }
      
      this->titleHeading = getTiltHeading(this->mag, this->realAccel);
      return this->titleHeading;
  }
  