Example #1
0
/**
	@brief	Init accelerometer.
*/
void initAccel()
{
	xmWriteByte(CTRL_REG0_XM, 0x00); // Disable fifo mode
	
	xmWriteByte(CTRL_REG1_XM, 0x57); // 100Hz data rate, x/y/z all enabled
	
	xmWriteByte(CTRL_REG2_XM, 0x00); // Set scale to 2g
	
	xmWriteByte(CTRL_REG3_XM, 0x00); 
}
Example #2
0
void LSM9DS0::initMag()
{	
	/* CTRL_REG5_XM enables temp sensor, sets mag resolution and data rate
	Bits (7-0): TEMP_EN M_RES1 M_RES0 M_ODR2 M_ODR1 M_ODR0 LIR2 LIR1
	TEMP_EN - Enable temperature sensor (0=disabled, 1=enabled)
	M_RES[1:0] - Magnetometer resolution select (0=low, 3=high)
	M_ODR[2:0] - Magnetometer data rate select
		000=3.125Hz, 001=6.25Hz, 010=12.5Hz, 011=25Hz, 100=50Hz, 101=100Hz
	LIR2 - Latch interrupt request on INT2_SRC (cleared by reading INT2_SRC)
		0=interrupt request not latched, 1=interrupt request latched
	LIR1 - Latch interrupt request on INT1_SRC (cleared by readging INT1_SRC)
		0=irq not latched, 1=irq latched 									 */
	xmWriteByte(CTRL_REG5_XM, 0x14); // Mag data rate - 100 Hz
	
	/* CTRL_REG6_XM sets the magnetometer full-scale
	Bits (7-0): 0 MFS1 MFS0 0 0 0 0 0
	MFS[1:0] - Magnetic full-scale selection
	00:+/-2Gauss, 01:+/-4Gs, 10:+/-8Gs, 11:+/-12Gs							 */
	xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS
	
	/* CTRL_REG7_XM sets magnetic sensor mode, low power mode, and filters
	AHPM1 AHPM0 AFDS 0 0 MLP MD1 MD0
	AHPM[1:0] - HPF mode selection
		00=normal (resets reference registers), 01=reference signal for filtering, 
		10=normal, 11=autoreset on interrupt event
	AFDS - Filtered acceleration data selection
		0=internal filter bypassed, 1=data from internal filter sent to FIFO
	MLP - Magnetic data low-power mode
		0=data rate is set by M_ODR bits in CTRL_REG5
		1=data rate is set to 3.125Hz
	MD[1:0] - Magnetic sensor mode selection (default 10)
		00=continuous-conversion, 01=single-conversion, 10 and 11=power-down */
	xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
	
	/* CTRL_REG4_XM is used to set interrupt generators on INT2_XM
	Bits (7-0): P2_TAP P2_INT1 P2_INT2 P2_INTM P2_DRDYA P2_DRDYM P2_Overrun P2_WTM
	*/
	xmWriteByte(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08)
	
	/* INT_CTRL_REG_M to set push-pull/open drain, and active-low/high
	Bits[7:0] - XMIEN YMIEN ZMIEN PP_OD IEA IEL 4D MIEN
	XMIEN, YMIEN, ZMIEN - Enable interrupt recognition on axis for mag data
	PP_OD - Push-pull/open-drain interrupt configuration (0=push-pull, 1=od)
	IEA - Interrupt polarity for accel and magneto
		0=active-low, 1=active-high
	IEL - Latch interrupt request for accel and magneto
		0=irq not latched, 1=irq latched
	4D - 4D enable. 4D detection is enabled when 6D bit in INT_GEN1_REG is set
	MIEN - Enable interrupt generation for magnetic data
		0=disable, 1=enable) */
	xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
}
Example #3
0
/**
	@brief	Init magnetometer.
*/
void initMag()

{	
	xmWriteByte(CTRL_REG5_XM, 0x14); // Mag data rate - 100 Hz
	
	xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS
	
	xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
	
	xmWriteByte(CTRL_REG4_XM, 0x00); // 
	
	xmWriteByte(INT_CTRL_REG_M, 0x00); // Disable interrupts for mag
}
Example #4
0
void LSM330D::initAccel()
{
	
	xmWriteByte(CTRL_REG1_A, 0x97); //0b10010111
    xmWriteByte(CTRL_REG2_A, 0x00);
    xmWriteByte(CTRL_REG3_A, 0x08); //0b00001000
    xmWriteByte(CTRL_REG4_A, 0x08);  // +/-2G (1mg/digt)
    xmWriteByte(CTRL_REG5_A, 0x40);
	
	/* CTRL_REG0_XM (0x1F) (Default value: 0x00)
	Bits (7-0): BOOT FIFO_EN WTM_EN 0 0 HP_CLICK HPIS1 HPIS2
	BOOT - Reboot memory content (0: normal, 1: reboot)
	FIFO_EN - Fifo enable (0: disable, 1: enable)
	WTM_EN - FIFO watermark enable (0: disable, 1: enable)
	HP_CLICK - HPF enabled for click (0: filter bypassed, 1: enabled)
	HPIS1 - HPF enabled for interrupt generator 1 (0: bypassed, 1: enabled)
	HPIS2 - HPF enabled for interrupt generator 2 (0: bypassed, 1 enabled)   */
	//xmWriteByte(CTRL_REG0_XM, 0x00);
	
	/* CTRL_REG1_XM (0x20) (Default value: 0x07)
	Bits (7-0): AODR3 AODR2 AODR1 AODR0 BDU AZEN AYEN AXEN
	AODR[3:0] - select the acceleration data rate:
		0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz, 
		0100=25Hz, 0101=50Hz, 0110=100Hz, 0111=200Hz, 1000=400Hz,
		1001=800Hz, 1010=1600Hz, (remaining combinations undefined).
	BDU - block data update for accel AND mag
		0: Continuous update
		1: Output registers aren't updated until MSB and LSB have been read.
	AZEN, AYEN, and AXEN - Acceleration x/y/z-axis enabled.
		0: Axis disabled, 1: Axis enabled									 */	
	//xmWriteByte(CTRL_REG1_XM, 0x57); // 100Hz data rate, x/y/z all enabled
	
	//Serial.println(xmReadByte(CTRL_REG1_XM));
	/* CTRL_REG2_XM (0x21) (Default value: 0x00)
	Bits (7-0): ABW1 ABW0 AFS2 AFS1 AFS0 AST1 AST0 SIM
	ABW[1:0] - Accelerometer anti-alias filter bandwidth
		00=773Hz, 01=194Hz, 10=362Hz, 11=50Hz
	AFS[2:0] - Accel full-scale selection
		000=+/-2g, 001=+/-4g, 010=+/-6g, 011=+/-8g, 100=+/-16g
	AST[1:0] - Accel self-test enable
		00=normal (no self-test), 01=positive st, 10=negative st, 11=not allowed
	SIM - SPI mode selection
		0=4-wire, 1=3-wire													 */
	//xmWriteByte(CTRL_REG2_XM, 0x00); // Set scale to 2g
	
	/* CTRL_REG3_XM is used to set interrupt generators on INT1_XM
	Bits (7-0): P1_BOOT P1_TAP P1_INT1 P1_INT2 P1_INTM P1_DRDYA P1_DRDYM P1_EMPTY
	*/
	// Accelerometer data ready on INT1_XM (0x04)
	//xmWriteByte(CTRL_REG3_XM, 0x04); 
}
Example #5
0
void LSM9DS0::setMagODR(mag_odr mRate)
{
	// We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
	uint8_t temp = xmReadByte(CTRL_REG5_XM);
	// Then mask out the mag ODR bits:
	temp &= 0xFF^(0x7 << 2);
	// Then shift in our new ODR bits:
	temp |= (mRate << 2);
	// And write the new register value back into CTRL_REG5_XM:
	xmWriteByte(CTRL_REG5_XM, temp);
}
Example #6
0
void LSM9DS0::setAccelABW(accel_abw abwRate)
{
	// We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
	uint8_t temp = xmReadByte(CTRL_REG2_XM);
	// Then mask out the accel ABW bits:
	temp &= 0xFF^(0x3 << 6);
	// Then shift in our new ODR bits:
	temp |= (abwRate << 6);
	// And write the new register value back into CTRL_REG2_XM:
	xmWriteByte(CTRL_REG2_XM, temp);
}
Example #7
0
void setAccelODR(LSM9DS0_t* lsm_t, accel_odr aRate)
{
	// We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
	uint8_t temp = xmReadByte(lsm_t,CTRL_REG1_XM);
	// Then mask out the accel ODR bits:
	temp &= 0xFF^(0xF << 4);
	// Then shift in our new ODR bits:
	temp |= (aRate << 4);
	// And write the new register value back into CTRL_REG1_XM:
	xmWriteByte(lsm_t,CTRL_REG1_XM, temp);
}
Example #8
0
void LSM330D::setAccelODR(accel_odr aRate)
{
	// We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
	uint8_t temp = xmReadByte(CTRL_REG1_A);
	// Then mask out the accel ODR bits:
	temp &= 0x0F;
	// Then shift in our new ODR bits:
	temp |= (aRate << 4);
	// And write the new register value back into CTRL_REG1_XM:
	xmWriteByte(CTRL_REG1_A, temp);
}
Example #9
0
void setMagODR(LSM9DS0_t* lsm_t, mag_odr mRate)
{
	// We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
	//uint8_t temp = xmReadByte(lsm_t,CTRL_REG5_XM);
	// Then mask out the mag ODR bits:
	lsm_t->xmCtrl[5] &= 0xFF^(0x7 << 2);
	// Then shift in our new ODR bits:
	lsm_t->xmCtrl[5] |= (mRate << 2);
	// And write the new register value back into CTRL_REG5_XM:
	if (lsm_t->update==UPDATE_ON_SET)
		xmWriteByte(lsm_t,CTRL_REG5_XM, lsm_t->xmCtrl+5);
}
Example #10
0
void setAccelABW(LSM9DS0_t* lsm_t, accel_abw abwRate)
{
	// We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
	//uint8_t temp = xmReadByte(lsm_t,CTRL_REG2_XM);
	// Then mask out the accel ABW bits:
	lsm_t->xmCtrl[2] &= 0xFF^(0x3 << 7);
	// Then shift in our new ODR bits:
	lsm_t->xmCtrl[2] |= (abwRate << 7);
	// And write the new register value back into CTRL_REG2_XM:
	if (lsm_t->update==UPDATE_ON_SET)
		xmWriteByte(lsm_t,CTRL_REG2_XM, lsm_t->xmCtrl+2);
}
Example #11
0
void LSM9DS0::setMagScale(mag_scale mScl)
{
	// We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
	uint8_t temp = xmReadByte(CTRL_REG6_XM);
	// Then mask out the mag scale bits:
	temp &= 0xFF^(0x3 << 5);
	// Then shift in our new scale bits:
	temp |= mScl << 5;
	// And write the new register value back into CTRL_REG6_XM:
	xmWriteByte(CTRL_REG6_XM, temp);
	
	// We've updated the sensor, but we also need to update our class variables
	// First update mScale:
	mScale = mScl;
	// Then calculate a new mRes, which relies on mScale being set correctly:
	calcmRes();
}
Example #12
0
/* ************************************************************************** */
void LSM9DS0_setAccelScale(stLSM9DS0_t * stThis, accel_scale aScl)
{
    // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
    uint8_t temp = xmReadByte(stThis, CTRL_REG2_XM);
    // Then mask out the accel scale bits:
    temp &= 0xFF^(0x3 << 3);
    // Then shift in our new scale bits:
    temp |= aScl << 3;
    // And write the new register value back into CTRL_REG2_XM:
    xmWriteByte(stThis, CTRL_REG2_XM, temp);

    // We've updated the sensor, but we also need to update our class variables
    // First update aScale:
    stThis->aScale = aScl;
    // Then calculate a new aRes, which relies on aScale being set correctly:
    calcaRes(stThis);
}
Example #13
0
void LSM330D::setAccelScale(accel_scale aScl)
{
	// We need to preserve the other bytes in CTRL_REG4_A. So, first read it:
	uint8_t temp = xmReadByte(CTRL_REG4_A);
	// Then mask out the accel scale bits:
	temp &= 0xCF;
	// Then shift in our new scale bits:
	temp |= aScl << 4;
	// And write the new register value back into CTRL_REG2_XM:
	xmWriteByte(CTRL_REG4_A, temp);
	
	// We've updated the sensor, but we also need to update our class variables
	// First update aScale:
	aScale = aScl;
	// Then calculate a new aRes, which relies on aScale being set correctly:
	calcaRes();
}
Example #14
0
void setAccelScale(LSM9DS0_t* lsm_t, accel_scale aScl)
{
	// We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
	//uint8_t temp = xmReadByte(lsm_t,CTRL_REG2_XM);
	// Then mask out the accel scale bits:
	lsm_t->xmCtrl[2] &= 0xFF^(0x3 << 3);
	// Then shift in our new scale bits:
	lsm_t->xmCtrl[2] |= aScl << 3;
	// And write the new register value back into CTRL_REG2_XM:
	if (lsm_t->update==UPDATE_ON_SET)
		xmWriteByte(lsm_t,CTRL_REG2_XM, lsm_t->xmCtrl+2);

	// We've updated the sensor, but we also need to update our class variables
	// First update aScale:
	lsm_t->aScale = aScl;

	// Then calculate a new aRes, which relies on aScale being set correctly:
	calcaRes(lsm_t);
}
Example #15
0
void setMagScale(LSM9DS0_t* lsm_t, mag_scale mScl)
{
	// We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
	//uint8_t temp = xmReadByte(lsm_t,CTRL_REG6_XM);
	// Then mask out the mag scale bits:
	lsm_t->xmCtrl[6] &= 0xFF^(0x3 << 5);
	// Then shift in our new scale bits:
	lsm_t->xmCtrl[6] |= mScl << 5;
	// And write the new register value back into CTRL_REG6_XM:
	if (lsm_t->update==UPDATE_ON_SET)
		xmWriteByte(lsm_t,CTRL_REG6_XM, lsm_t->xmCtrl+6);

	// We've updated the sensor, but we also need to update our class variables
	// First update mScale:
	lsm_t->mScale = mScl;

	// Then calculate a new mRes, which relies on mScale being set correctly:
	calcmRes(lsm_t);
}
Example #16
0
// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
// them, scales them to  gs and deg/s, respectively, and then passes the biases to the main sketch
// for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
// the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to
// subtract the biases ourselves. This results in a more accurate measurement in general and can
// remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
// is good practice.
void calLSM9DS0(LSM9DS0_t* lsm_t, float * gbias, float * abias)
{
  uint8_t data[6] 	= {0, 0, 0, 0, 0, 0};
  int16_t
  	gyro_bias[3] 	= {0, 0, 0},
  	accel_bias[3] 	= {0, 0, 0};
  int samples, ii;

  // First get gyro bias
  uint8_t c = gReadByte(lsm_t,CTRL_REG5_G);		//read modify write 
  gWriteByte(lsm_t, CTRL_REG5_G, c | 0x40);         	// Enable gyro FIFO
  delay(20);                                 	// Wait for change to take effect
  gWriteByte(lsm_t, FIFO_CTRL_REG_G, 0x20 | 0x1F);  	// Enable gyro FIFO stream mode and set watermark at 32 samples
  delay(1000);  								// delay 1000 milliseconds to collect FIFO samples

  samples = (gReadByte(lsm_t,FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples

  for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
    gReadBytes(lsm_t,OUT_X_L_G,  &data[0], 6);
    gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
    gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
    gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
  }

  gyro_bias[0] /= samples; 						// average the data
  gyro_bias[1] /= samples;
  gyro_bias[2] /= samples;

  gbias[0] = (float)gyro_bias[0]*lsm_t->gRes; 			 // Properly scale the data to get deg/s
  gbias[1] = (float)gyro_bias[1]*lsm_t->gRes;
  gbias[2] = (float)gyro_bias[2]*lsm_t->gRes;

  c = gReadByte(lsm_t,CTRL_REG5_G);
  gWriteByte(lsm_t, CTRL_REG5_G, c & ~0x40);  			// Disable gyro FIFO
  delay(20);
  gWriteByte(lsm_t, FIFO_CTRL_REG_G, 0x00);   			// Enable gyro bypass mode


  //  Now get the accelerometer biases
  c = xmReadByte(lsm_t,CTRL_REG0_XM);
  xmWriteByte(lsm_t,CTRL_REG0_XM, c | 0x40);      	// Enable accelerometer FIFO
  delay(20);                                	// Wait for change to take effect
  xmWriteByte(lsm_t,FIFO_CTRL_REG, 0x20 | 0x1F);  	// Enable accelerometer FIFO stream mode and set watermark at 32 samples
  delay(1000); 	 								// delay 1000 milliseconds to collect FIFO samples

  samples = (xmReadByte(lsm_t,FIFO_SRC_REG) & 0x1F); 	// Read number of stored accelerometer samples

   for(ii = 0; ii < samples ; ii++) {          	// Read the accelerometer data stored in the FIFO
    xmReadBytes(lsm_t,OUT_X_L_A, &data[0], 6);
    accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
    accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
    accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1.0f/lsm_t->aRes); // Assumes sensor facing up!
  }

  accel_bias[0] /= samples; // average the data
  accel_bias[1] /= samples;
  accel_bias[2] /= samples;

  abias[0] = (float)accel_bias[0]*lsm_t->aRes; // Properly scale data to get gs
  abias[1] = (float)accel_bias[1]*lsm_t->aRes;
  abias[2] = (float)accel_bias[2]*lsm_t->aRes;

  c = xmReadByte(lsm_t,CTRL_REG0_XM);
  xmWriteByte(lsm_t,CTRL_REG0_XM, c & ~0x40);    // Disable accelerometer FIFO
  delay(20);
  xmWriteByte(lsm_t,FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
}
Example #17
0
void LSM330D::calLSM330D(float * gbias, float * abias)
{  
  uint8_t data[6] = {0, 0, 0, 0, 0, 0};
  int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
  uint16_t samples, ii;
  
  // First get gyro bias
  byte c = gReadByte(CTRL_REG5_G);
  gWriteByte(CTRL_REG5_G, c | 0x40);         // Enable gyro FIFO  
  delay(20);                                 // Wait for change to take effect
  gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
  delay(1000);  // delay 1000 milliseconds to collect FIFO samples
  
  samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples

  for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
    int16_t gyro_temp[3] = {0, 0, 0};
    gReadBytes(OUT_X_L_G,  &data[0], 6);
    gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO
    gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
    gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);

    gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    gyro_bias[1] += (int32_t) gyro_temp[1]; 
    gyro_bias[2] += (int32_t) gyro_temp[2]; 
  }  

  gyro_bias[0] /= samples; // average the data
  gyro_bias[1] /= samples; 
  gyro_bias[2] /= samples; 
  
  gbias[0] = (float)gyro_bias[0]*gRes;  // Properly scale the data to get deg/s
  gbias[1] = (float)gyro_bias[1]*gRes;
  gbias[2] = (float)gyro_bias[2]*gRes;
  
  c = gReadByte(CTRL_REG5_G);
  gWriteByte(CTRL_REG5_G, c & ~0x40);  // Disable gyro FIFO  
  delay(20);
  gWriteByte(FIFO_CTRL_REG_G, 0x00);   // Enable gyro bypass mode
  

  //  Now get the accelerometer biases
  c = xmReadByte(CTRL_REG5_A);
  xmWriteByte(CTRL_REG5_A, c | 0x40);      // Enable accelerometer FIFO  
  delay(20);                                // Wait for change to take effect
  xmWriteByte(FIFO_CTRL_REG, 0x40 | 0x1F);  // Enable accelerometer FIFO stream mode and set watermark at 32 samples
  delay(1000);  // delay 1000 milliseconds to collect FIFO samples

  samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples

   for(ii = 0; ii < samples ; ii++) {          // Read the accelerometer data stored in the FIFO
    int16_t accel_temp[3] = {0, 0, 0};
    xmReadBytes(OUT_X_L_A, &data[0], 6);
    accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]);// Form signed 16-bit integer for each sample in FIFO
    accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]);
    accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]);  

    accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    accel_bias[1] += (int32_t) accel_temp[1]; 
    accel_bias[2] += (int32_t) accel_temp[2]; 
  }  

  accel_bias[0] /= samples; // average the data
  accel_bias[1] /= samples; 
  accel_bias[2] /= samples; 

  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) (1.0/aRes);}  // Remove gravity from the z-axis accelerometer bias calculation
  else {accel_bias[2] += (int32_t) (1.0/aRes);}
 
  
  abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
  abias[1] = (float)accel_bias[1]*aRes;
  abias[2] = (float)accel_bias[2]*aRes;

  c = xmReadByte(CTRL_REG5_A);
  xmWriteByte(CTRL_REG5_A, c & ~0x40);    // Disable accelerometer FIFO  
  delay(20);
  xmWriteByte(FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
}