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; }