bool MPU9150::setup(){
uint8_t data[6];
// Reset device
data[0] = 1<<MPU9150_PWR1_DEVICE_RESET_BIT;
if(!i2c.write_register8(MPU9150_ADDRESS, MPU9150_RA_PWR_MGMT_1, 1, data))
return -1;
Time.delay_ms(100);
// Power up device (this should happen no matter what, but just to be sure)
data[0] = 0;
if(!i2c.write_register8(MPU9150_ADDRESS, MPU9150_RA_PWR_MGMT_1, 1, data))
return -1;
// Set Gyroscope FSR to 2000 degree per second
if(set_gyro_fsr(2000))
return -1;
// Set Accel FSR to 8g
if(set_accel_fsr(8))
return -1;
// Set digital lowpass filter
if(set_lpf(42))
return -1;
// Set gyro/acc sampling rate to 1000 (no divider)
if(set_sample_rate(1000))
return -1;
// Disable interrupt
data[0] = 0;
if(!i2c.write_register8(MPU9150_ADDRESS, MPU9150_RA_INT_ENABLE, 1, data))
return -1;
// Setup Compass
if(setup_compass())
return -1;
if(set_compass_sample_rate(100))
return -1;
// Enable sensors
if(set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS))
return -1;
return 0;
}
uint8_t mpu_init(){
uint8_t data[6];
uint8_t datai;
/* Reset device. */
data[0] = BIT_RESET;
if (mpu9250_writes(MPU6500_PWR_MGNT_1, 1, data))
return -1;
delay_ms(100);
/* Wake up chip. */
data[0] = 0x00;
if (mpu9250_writes(MPU6500_PWR_MGNT_1, 1, data))
return -1;
// とりあえず
//data[0] = 0x00;
//mpu9250_writes(MPU6500_USER_CTRL, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_ACCEL_CFG2, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_INT_PIN_CFG, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_LPF, 1, data);
//delay_ms(10);
data[0] = 0x02;
mpu9250_writes(MPU6500_INT_PIN_CFG, 1, data);
delay_ms(10);
//datai = 0;
//datai |= BIT_STBY_XYZA;
data[0] = 0x00;
mpu9250_writes(MPU6500_PWR_MGNT_2, 1, data[0]);
delay_ms(10);
//data[0] = 0x01;
//mpu9250_writes(MPU6500_INT_ENABLE, 1, data);
//delay_ms(10);
#if 0
/* user control */
data[0] = MPU9250_I2C_IF_DIS | MPU9250_I2C_MST_EN;
if (mpu9250_writes(MPU6500_USER_CTRL, 1, data))
return -1;
if (mpu9250_writes(MPU6500_USER_CTRL, 1, data))
return -1;
delay_ms(10);
mpu_set_bypass(0);
delay_ms(10);
mpu9250_st.accel_half = 0;
/* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
* first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
*/
data[0] = BIT_FIFO_SIZE_1024 | 0x8;
if (mpu9250_writes(MPU6500_ACCEL_CFG2, 1, data))
return -1;
/* Set to invalid values to ensure no I2C writes are skipped. */
//mpu9250_st.sensors = 0xFF;
//mpu9250_st.gyro_fsr = 0xFF;
//mpu9250_st.accel_fsr = 0xFF;
//mpu9250_st.lpf = 0xFF;
//mpu9250_st.sample_rate = 0xFFFF;
//mpu9250_st.fifo_enable = 0xFF;
//mpu9250_st.bypass_mode = 0xFF;
//mpu9250_st.compass_sample_rate = 0xFFFF;
/* mpu_set_sensors always preserves this setting. */
mpu9250_st.clk_src = INV_CLK_PLL;
/* Handled in next call to mpu_set_bypass. */
mpu9250_st.active_low_int = 1;
mpu9250_st.latched_int = 0;
mpu9250_st.int_motion_only = 0;
mpu9250_st.lp_accel_mode = 0;
//とりあえずマスク
//memset(&mpu9250_st.cache, 0, sizeof(mpu9250_st.cache));
mpu9250_st.dmp_on = 0;
mpu9250_st.dmp_loaded = 0;
mpu9250_st.dmp_sample_rate = 0;
// ジャイロフルスケール設定
if (mpu_set_gyro_fsr(2000))
return -1;
// 加速度フルスケール設定
if (mpu_set_accel_fsr(2))
return -1;
// LPF設定
if (mpu_set_lpf(42))
return -1;
// サンプルレート設定
if (mpu_set_sample_rate(100))
return -1;
// ひとまずマスク
/*
if (mpu_configure_fifo(0))
return -1;
if (int_param)
reg_int_cb(int_param);
*/
#ifdef USE_AK8963
if (setup_compass())
return -1;
if (mpu_set_compass_sample_rate(100))
return -1;
#else
/* Already disabled by setup_compass. */
if (mpu_set_bypass(0))
return -1;
#endif
/*
mpu_set_sensors(0);
*/
#endif
return 0;
}
