/*==============================================================================================*/
u8 MPU9150_Init(void)
{
	u8 i = 0;
	u8 ReadBuf[3] = {0};
	u8 MPU6050_Init_Data[6][2] = {
		{0x01, MPU6050_PWR_MGMT_1},   // Reset device
		{0x03, MPU6050_CONFIG},       //
		{0x18, MPU6050_GYRO_CONFIG},  // +-2000dps
		{0x08, MPU6050_ACCEL_CONFIG}, // +-4G
		{0x32, MPU6050_INT_PIN_CFG},  //
		{0x00, MPU6050_USER_CTRL}     //
	};
	u8 AK8975_Init_Data[3][2] = {
		{0x0F, AK8975_CNTL},          // Set Fuse ROM access mode
		{0x00, AK8975_CNTL},          // Set Power Down Mode
		{0x01, AK8975_CNTL}           // Set Single Measurement Mode
	};

	/* MPU6050 */
	I2C_DMA_ReadReg(MPU6050_I2C_ADDR, MPU6050_WHO_AM_I, ReadBuf, 1);

	if (ReadBuf[0] != MPU6050_Device_ID)
		return ERROR;

	Delay_1ms(10);

	for (i = 0; i < 6; i++) {
		I2C_DMA_WriteReg(MPU6050_I2C_ADDR, MPU6050_Init_Data[i][1], MPU6050_Init_Data[i], 1);
		Delay_1ms(10);
	}

	/* AK8975 */
	I2C_DMA_ReadReg(AK8975_I2C_ADDR, AK8975_WIA, ReadBuf, 1);

	if (ReadBuf[0] != AK8975_Device_ID)
		return ERROR;

	I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_Init_Data[0][1], AK8975_Init_Data[0], 1);
	Delay_1ms(10);

	I2C_DMA_ReadReg(AK8975_I2C_ADDR, AK8975_ASAX, ReadBuf, 3);
	Delay_1ms(10);

	Mag.AdjustX = ((ReadBuf[0] + 128) * 30) >> 8;
	Mag.AdjustY = ((ReadBuf[1] + 128) * 30) >> 8;
	Mag.AdjustZ = ((ReadBuf[2] + 128) * 30) >> 8;

	I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_Init_Data[1][1], AK8975_Init_Data[1], 1);
	Delay_1ms(10);
	I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_Init_Data[2][1], AK8975_Init_Data[2], 1);
	Delay_1ms(10);

	return SUCCESS;
}
/*==============================================================================================*/
void MPU9150_Read( u8* ReadBuf )
{
  u8 ReadData = 0x00;
  u8 WriteData = 0x01;

  I2C_DMA_ReadReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, ReadBuf, 14);

  I2C_DMA_ReadReg(AK8975_I2C_ADDR, AK8975_ST1, &ReadData, 1);       // Wait Prapare Data
  if(ReadData == 1) {
    I2C_DMA_ReadReg(AK8975_I2C_ADDR, AK8975_HXL, ReadBuf+14, 6);
    I2C_DMA_WriteReg(AK8975_I2C_ADDR, AK8975_CNTL, &WriteData, 1);  // Set Single Measurement Mode
  }
}
Example #3
0
/* 初始化所有板载传感器 */
unsigned int sensors_init(void)
{	
	unsigned char temp;
	
	//first of all we should initialize the i2c mutex
	i2c1_mutex_init();
	
	rt_thread_delay(100);
	
	//initialization of the ms5611
	i2c1_take(RT_WAITING_FOREVER);
	ms5611_parameter_get();
	i2c1_release();
	
	//take the i2c bus
	i2c1_take(RT_WAITING_FOREVER);
	
	//HMC5883L initialization. ID should be 0x48
	I2C_DMA_ReadReg(&HMC_ID, HMC5883_RD_ADDR, HMC_ID_REG_A, 1 );
	rt_thread_delay(5);
	temp = 0x78;
	I2C_DMA_WriteReg(&temp, HMC5883_WR_ADDR, CFG_A , 1); //75HZ out put rate
	temp = 0x20;
	I2C_DMA_WriteReg(&temp, HMC5883_WR_ADDR, CFG_B , 1); //1.3GASS
	temp = 0x00;
	I2C_DMA_WriteReg(&temp, HMC5883_WR_ADDR, MODE , 1); //continue measurment mode	

	//MPU6050 initialization. ID should be 0x68
	I2C_DMA_ReadReg(&MPU_ID, MPU6050_RD_ADDRESS, MPU6050_WHO_AM_I, 1 );
	rt_thread_delay(5);
	
	temp = 0x80;
	I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x6B, 1); //PWR_MGMT_1    -- DEVICE_RESET 1
	temp = 0x00;
    I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x19, 1); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
	temp = 0x00;
 	I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x1A, 1); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
	temp = 0x03;
	I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x6B, 1); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
	temp = 0x00;
	I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x1B, 1); //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 250 deg/sec	
	temp = 0x00;
	I2C_DMA_WriteReg(&temp, MPU6050_WR_ADDRESS, 0x1C, 1); //ACC_CONFIG   -- FS_SEL = 3: Full scale set to 2G
	
	//release the i2c bus
	i2c1_release();
	
	return 0;
}
Example #4
0
/*
 * Name										: read_imu_fast
 * Description						: ---
 * Author									: lynx@sia.
 *
 * History
 * ----------------------
 * Rev										: 0.00
 * Date										: 10/20/2013
 *
 * create.
 * ----------------------
 */
static int read_imu_fast(void)
{
	u8 IMU_Buf[20] = {0};
	
	I2C_DMA_ReadReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, IMU_Buf,   14);
	// 	MS5611_ReadADC(&imu_sensor.Bar);
	// 	MS5611_Calculate(&imu_sensor.Bar);
	
	imu_sensor.Acc.X = (s16)((IMU_Buf[0]  << 8) | IMU_Buf[1]);
	imu_sensor.Acc.Y = (s16)((IMU_Buf[2]  << 8) | IMU_Buf[3]);
	imu_sensor.Acc.Z = (s16)((IMU_Buf[4]  << 8) | IMU_Buf[5]);
	//	Tmp   = (s16)((IMU_Buf[6]  << 8) | IMU_Buf[7]);
	imu_sensor.Gyr.X = (s16)((IMU_Buf[8]  << 8) | IMU_Buf[9]);
	imu_sensor.Gyr.Y = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
	imu_sensor.Gyr.Z = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
	
	//计算真值
	imu_sensor.Acc.TrueX = imu_sensor.Acc.X/imu_sensor.Acc.RawToTrue;
	imu_sensor.Acc.TrueY = imu_sensor.Acc.Y/imu_sensor.Acc.RawToTrue;
	imu_sensor.Acc.TrueZ = imu_sensor.Acc.Z/imu_sensor.Acc.RawToTrue;
	//计算真值
	imu_sensor.Gyr.TrueX = imu_sensor.Gyr.X/imu_sensor.Gyr.RawToTrue;
	imu_sensor.Gyr.TrueY = imu_sensor.Gyr.Y/imu_sensor.Gyr.RawToTrue;
	imu_sensor.Gyr.TrueZ = imu_sensor.Gyr.Z/imu_sensor.Gyr.RawToTrue;
	
	return 0;
}
Example #5
0
/*==============================================================================================*/
void MPU6050_Read( s16* IMU_Buf )
{
  u8 ReadBuf[14] = {0};

  I2C_DMA_ReadReg(MPU6050_I2C_ADDR, MPU6050_ACCEL_XOUT_H, ReadBuf, 14);

  IMU_Buf[0] = (s16)((ReadBuf[0]  << 8) | ReadBuf[1]);
  IMU_Buf[1] = (s16)((ReadBuf[2]  << 8) | ReadBuf[3]);
  IMU_Buf[2] = (s16)((ReadBuf[4]  << 8) | ReadBuf[5]);
  IMU_Buf[3] = (s16)((ReadBuf[6]  << 8) | ReadBuf[7]);
  IMU_Buf[4] = (s16)((ReadBuf[8]  << 8) | ReadBuf[9]);
  IMU_Buf[5] = (s16)((ReadBuf[10] << 8) | ReadBuf[11]);
  IMU_Buf[6] = (s16)((ReadBuf[12] << 8) | ReadBuf[13]);
}
/*=====================================================================================================*/
void SysTick_Handler( void )
{
 u8 IMU_Buf[16] = {0};

	/* 400Hz 取樣 */
  I2C_DMA_ReadReg(IMU_Buf, MPU6050_I2C_ADDR, 0x3B, 14);

	/* 資料合併 */
	Acc.X = (s16)((IMU_Buf[0]  << 8) | IMU_Buf[1]);
	Acc.Y = (s16)((IMU_Buf[2]  << 8) | IMU_Buf[3]);
	Acc.Z = (s16)((IMU_Buf[4]  << 8) | IMU_Buf[5]);
	Gyr.X = (s16)((IMU_Buf[8]  << 8) | IMU_Buf[9]);
	Gyr.Y = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
	Gyr.Z = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
// 	Meg.X = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
// 	Meg.Y = (s16)((IMU_Buf[14] << 8) | IMU_Buf[15]);
// 	Meg.Z = (s16)((IMU_Buf[16] << 8) | IMU_Buf[17]);
}
Example #7
0
/*==============================================================================================*/
void ADXL345_Read_Multiple( u8* ReadMultiple )
{
	I2C_DMA_ReadReg(ReadMultiple, ADXL345_I2C_ADDR, ADXL345_MULTIPLE, 6);
}