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