byte MPU9250_DMP::begin(bool fusion, int rate)
{
i2c_port_t i2c_master_port = I2C_NUM_0;
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = (gpio_num_t)21;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_io_num = (gpio_num_t)22;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = 100000;
if (i2c_param_config(i2c_master_port, &conf) != ESP_OK || i2c_driver_install(i2c_master_port, conf.mode, 0, 0, 0) != ESP_OK)
return 0;
struct int_param_s int_param;
inv_error_t result = mpu_init(&int_param);
if (result)
return 0;
mpu_set_bypass(1); // Place all slaves (including compass) on primary bus
setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS | INV_XYZ_COMPASS);
_gSense = getGyroSens();
_aSense = getAccelSens();
_features = DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_CAL_GYRO;
if (fusion) _features |= DMP_FEATURE_6X_LP_QUAT;
// Set DMP FIFO rate to 10 Hz
if (dmpBegin(_features, rate) == INV_ERROR)
return 0;
return 1;
}
/* This initialization is similar to the one in ak8975.c. */
uint8_t setup_compass(void){
uint8_t data[4], akm_addr;
uint8_t result;
mpu_set_bypass(0);
/* Find compass. Possible addresses range from 0x0C to 0x0F. */
for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
//result = mpu9250_writes(akm_addr, AKM_REG_WHOAMI, 1, data);
result = mpu9250_ak8963_reads(akm_addr, AKM_REG_WHOAMI, 1, data);
if (!result && (data[0] == AKM_WHOAMI))
break;
}
#if 0
if (akm_addr > 0x0F) {
/* TODO: Handle this case in all compass-related functions. */
log_e("Compass not found.\n");
return -1;
}
#endif
mpu9250_st.compass_addr = akm_addr;
data[0] = AKM_POWER_DOWN;
if (mpu9250_ak8963_writes(mpu9250_st.compass_addr, AKM_REG_CNTL, 1, data))
return -1;
delay_ms(1);
data[0] = AKM_FUSE_ROM_ACCESS;
if (mpu9250_ak8963_writes(mpu9250_st.compass_addr, AKM_REG_CNTL, 1, data))
return -1;
delay_ms(1);
/* Get sensitivity adjustment data from fuse ROM. */
if (mpu9250_ak8963_reads(mpu9250_st.compass_addr, AKM_REG_ASAX, 3, data))
return -1;
//bug?why plus 128? but must sub 128 from datasheet
mpu9250_st.mag_sens_adj[0] = (long)data[0] + 128;
mpu9250_st.mag_sens_adj[1] = (long)data[1] + 128;
mpu9250_st.mag_sens_adj[2] = (long)data[2] + 128;
data[0] = AKM_POWER_DOWN;
if (mpu9250_ak8963_writes(mpu9250_st.compass_addr, AKM_REG_CNTL, 1, data))
return -1;
delay_ms(1);
/* Set up master mode, master clock, and ES bit. */
//modified by hetao.su
//data[0] = 0x40;
data[0] = 0x4D;
if (mpu9250_writes(MPU6500_I2C_MST, 1, data))
return -1;
/* Slave 0 reads from AKM data registers. */
data[0] = BIT_I2C_READ | mpu9250_st.compass_addr;
if (mpu9250_writes(AK8963_S0_ADDR, 1, data))
return -1;
/* Compass reads start at this register. */
data[0] = AKM_REG_ST1;
if (mpu9250_writes(AK8963_S0_REG, 1, data))
return -1;
/* Enable slave 0, 8-byte reads. */
data[0] = BIT_SLAVE_EN | 8;
if (mpu9250_writes(AK8963_S0_CTRL, 1, data))
return -1;
/* Slave 1 changes AKM measurement mode. */
data[0] = mpu9250_st.compass_addr;
if (mpu9250_writes(AK8963_S1_ADDR, 1, data))
return -1;
/* AKM measurement mode register. */
data[0] = AKM_REG_CNTL;
if (mpu9250_writes(AK8963_S1_REG, 1, data))
return -1;
/* Enable slave 1, 1-byte writes. */
data[0] = BIT_SLAVE_EN | 1;
if (mpu9250_writes(AK8963_S1_CTRL, 1, data))
return -1;
/* Set slave 1 data. */
data[0] = AKM_SINGLE_MEASUREMENT;
if (mpu9250_writes(AK8963_S1_DO, 1, data))
return -1;
/* Trigger slave 0 and slave 1 actions at each sample. */
data[0] = 0x03;
if (mpu9250_writes(AK8963_I2C_DELAY_CTRL, 1, data))
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;
}
