Пример #1
0
void LSM9DS0_readGyro(LSM9DS0_t* lsm_t)
{
	uint8_t temp[6]; // We'll read six bytes from the gyro into temp
	gReadBytes(lsm_t,OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
	lsm_t->gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
	lsm_t->gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
	lsm_t->gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
}
Пример #2
0
void LSM330D::readGyro()
{
	uint8_t temp[6]; // We'll read six bytes from the gyro into temp
	gReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
	gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
	gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
	gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
}
Пример #3
0
bool LSM9DS0::readGyro()
{
//	uint8_t temp[6]; // We'll read six bytes from the gyro into temp
//	gReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
	uint8_t temp[7]; // We'll read six bytes from the gyro into temp
	gReadBytes(STATUS_REG_G, temp, 7); // Read 6 bytes, beginning at OUT_X_L_G
	if ( temp[0] & BIT_ZYXxDA ) {
		gx = (temp[2] << 8) | temp[1]; // Store x-axis values into gx
		gy = (temp[4] << 8) | temp[3]; // Store y-axis values into gy
		gz = (temp[6] << 8) | temp[5]; // Store z-axis values into gz
		return true;
	}
	return false;
}
Пример #4
0
void LSM9DS0_readGyro(LSM9DS0_t* lsm_t)
{
	uint8_t temp[6];
#if(DEBUG_R_G==1)
	temp[0]=gReadByte(lsm_t,OUT_X_L_G);
	temp[1]=gReadByte(lsm_t,OUT_X_H_G);
	temp[2]=gReadByte(lsm_t,OUT_Y_L_G);
	temp[3]=gReadByte(lsm_t,OUT_Y_H_G);
	temp[4]=gReadByte(lsm_t,OUT_Z_L_G);
	temp[5]=gReadByte(lsm_t,OUT_Z_H_G);
#else
	//uint8_t temp[6]; // We'll read six bytes from the gyro into temp
	gReadBytes(lsm_t,OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
#endif
	lsm_t->gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
	lsm_t->gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
	lsm_t->gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
}
Пример #5
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
}
Пример #6
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
}
Пример #7
0
void initGyro( LSM9DS0_t* lsm_t )
{
#if (DEBUG0==1)
		uint8_t test[5],test1[5],i;
#endif
	/* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables
	Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen
	DR[1:0] - Output data rate selection
		00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz
	BW[1:0] - Bandwidth selection (sets cutoff frequency)
		 Value depends on ODR. See datasheet table 21.
	PD - Power down enable (0=power down mode, 1=normal or sleep mode)
	Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled)	*/
	//gWriteByte(lsm_t, CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
	lsm_t->gCtrl[0]=0b01111111;
#if (DEBUG0==1)
		gWriteByte(lsm_t, CTRL_REG1_G, lsm_t->gCtrl[0]);
		test[0] = gReadByte(lsm_t,CTRL_REG1_G);
		assert(test[0]==lsm_t->gCtrl[0]);
#endif
	/* CTRL_REG2_G sets up the HPF
	Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0
	HPM[1:0] - High pass filter mode selection
		00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering,
		10=normal, 11=autoreset on interrupt
	HPCF[3:0] - High pass filter cutoff frequency
		Value depends on data rate. See datasheet table 26.
	*/
	//gWriteByte(lsm_t, CTRL_REG2_G, 0x25); // Normal mode, high cutoff frequency //0x00
	lsm_t->gCtrl[1]=0b00100101;
#if (DEBUG0==1)
		gWriteByte(lsm_t, CTRL_REG2_G, lsm_t->gCtrl[1]);
		test[1] = gReadByte(lsm_t,CTRL_REG2_G);
		assert(test[1]==lsm_t->gCtrl[1]);
#endif
	/* CTRL_REG3_G sets up interrupt and DRDY_G pins
	Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY
	I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable)
	I1_BOOT - Boot status available on INT_G (0=disable, 1=enable)
	H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low)
	PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain)
	I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable)
	I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable)
	I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable)
	I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */
	// Int1 enabled (pp, active low), data read on DRDY_G:
	//gWriteByte(lsm_t, CTRL_REG3_G, 0x00); //88
	lsm_t->gCtrl[2]=0b00000000;
#if (DEBUG0==1)
		gWriteByte(lsm_t, CTRL_REG3_G, lsm_t->gCtrl[2]);
		test[2] = gReadByte(lsm_t,CTRL_REG3_G);
		assert(test[2]==lsm_t->gCtrl[2]);
#endif
	/* CTRL_REG4_G sets the scale, update mode
	Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM
	BDU - Block data update (0=continuous, 1=output not updated until read
	BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add)
	FS[1:0] - Full-scale selection
		00=245dps, 01=500dps, 10=2000dps, 11=2000dps
	ST[1:0] - Self-test enable
		00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+)
	SIM - SPI serial interface mode select
		0=4 wire, 1=3 wire */
	//gWriteByte(lsm_t, CTRL_REG4_G, 0x90); // Set scale to 245 dps//00
	lsm_t->gCtrl[3]=0b10010000;
#if (DEBUG0==1)
		gWriteByte(lsm_t, CTRL_REG4_G, lsm_t->gCtrl[3]);
		test[3] = gReadByte(lsm_t,CTRL_REG4_G);
		assert(test[3]==lsm_t->gCtrl[3]);
#endif
	/* CTRL_REG5_G sets up the FIFO, HPF, and INT1
	Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
	BOOT - Reboot memory content (0=normal, 1=reboot)
	FIFO_EN - FIFO enable (0=disable, 1=enable)
	HPen - HPF enable (0=disable, 1=enable)
	INT1_Sel[1:0] - Int 1 selection configuration
	Out_Sel[1:0] - Out selection configuration */

	//gWriteByte(lsm_t, CTRL_REG5_G, 0x00);
	lsm_t->gCtrl[4]=0b00000000;
#if (DEBUG0==1)
		gWriteByte(lsm_t, CTRL_REG5_G, lsm_t->gCtrl[4]);
		test[4] = gReadByte(lsm_t,CTRL_REG5_G);
		assert(test[4]==lsm_t->gCtrl[4]);
#endif
	if (lsm_t->update==UPDATE_ON_CALL)
	{
		gWriteBytes(lsm_t,CTRL_REG1_G, lsm_t->gCtrl,5);
	}

#if (DEBUG0==1)
	gReadBytes(lsm_t, CTRL_REG1_G,test1,5);
	for (i=0;i<5;i++)
	{
		assert(test[i]==test1[i]);
	}
#endif

}