//// MPU9150 IMU //// int initialize_imu(int sample_rate, signed char orientation[9]){ printf("Initializing IMU\n"); //set up gpio interrupt pin connected to imu if(gpio_export(INTERRUPT_PIN)){ printf("can't export gpio %d \n", INTERRUPT_PIN); return (-1); } gpio_set_dir(INTERRUPT_PIN, INPUT_PIN); gpio_set_edge(INTERRUPT_PIN, "falling"); // Can be rising, falling or both linux_set_i2c_bus(1); if (mpu_init(NULL)) { printf("\nmpu_init() failed\n"); return -1; } mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); mpu_set_sample_rate(sample_rate); // compass run at 100hz max. if(sample_rate > 100){ // best to sample at a fraction of the gyro/accel mpu_set_compass_sample_rate(sample_rate/2); } else{ mpu_set_compass_sample_rate(sample_rate); } mpu_set_lpf(188); //as little filtering as possible dmp_load_motion_driver_firmware(sample_rate); dmp_set_orientation(inv_orientation_matrix_to_scalar(orientation)); dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL); dmp_set_fifo_rate(sample_rate); if (mpu_set_dmp_state(1)) { printf("\nmpu_set_dmp_state(1) failed\n"); return -1; } if(loadGyroCalibration()){ printf("\nGyro Calibration File Doesn't Exist Yet\n"); printf("Use calibrate_gyro example to create one\n"); printf("Using 0 offset for now\n"); }; // set the imu_interrupt_thread to highest priority since this is used // for real-time control with time-sensitive IMU data set_imu_interrupt_func(&null_func); pthread_t imu_interrupt_thread; struct sched_param params; pthread_create(&imu_interrupt_thread, NULL, imu_interrupt_handler, (void*) NULL); params.sched_priority = sched_get_priority_max(SCHED_FIFO); pthread_setschedparam(imu_interrupt_thread, SCHED_FIFO, ¶ms); return 0; }
void MPU9250Device::init() { mpu_init(&this->revision); mpu_set_compass_sample_rate(100); // defaults to 100 in the libs /* Get/set hardware configuration. Start gyro. Wake up all sensors. */ mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // mpu_set_gyro_fsr (2000);//250 // mpu_set_accel_fsr(2);//4 /* Push both gyro and accel data into the FIFO. */ mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); mpu_set_sample_rate(DEFAULT_MPU_HZ); dmp_load_motion_driver_firmware(); dmp_set_orientation(GYRO_ORIENTATION); //dmp_register_tap_cb(&tap_cb); unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL; //dmp_features = dmp_features | DMP_FEATURE_TAP ; dmp_enable_feature(dmp_features); dmp_set_fifo_rate(DEFAULT_MPU_HZ); }
uint8_t mpu_set_sample_rate(uint16_t rate){ uint8_t data; // センサー使用しない場合は終了 if (!(mpu9250_st.sensors)) return -1; if (mpu9250_st.dmp_on){ return -1; } else { // ローパワーモード?とりあえずマスク #if 0 if (mpu9250_st.lp_accel_mode) { if (rate && (rate <= 40)) { /* Just stay in low-power accel mode. */ mpu_lp_accel_mode(rate); return 0; } /* Requested rate exceeds the allowed frequencies in LP accel mode, * switch back to full-power mode. */ mpu_lp_accel_mode(0); } #endif // 上下限カット if (rate < 4) rate = 4; else if (rate > 1000) rate = 1000; // レジスタ書き込み値に変換 data = 1000 / rate - 1; // レジスタ書き込み if (mpu9250_writes(MPU6500_RATE_DIV, 1, &data)) return -1; // 値の保存 mpu9250_st.sample_rate = 1000 / (1 + data); #ifdef USE_AK8963 mpu_set_compass_sample_rate(min(mpu9250_st.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE)); #endif /* Automatically set LPF to 1/2 sampling rate. */ mpu_set_lpf(mpu9250_st.sample_rate >> 1); return 0; } }
int __init mpu9150_init(int sample_rate, int mix_factor) { signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) { printk(KERN_WARNING "Invalid sample rate %d\n", sample_rate); sprintf(status_string,"Invalid sample rate %d\n", sample_rate); return -1; } if (mix_factor < 0 || mix_factor > 100) { printk(KERN_WARNING "Invalid mag mixing factor %d\n", mix_factor); return -1; } yaw_mixing_factor = mix_factor; printk("\nInitializing IMU ..."); if (mmpu_init()) { printk(KERN_WARNING "\nmpu_init() failed\n"); sprintf(status_string,"mpu_init() failed\n"); return -1; } //printk("."); if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { printk(KERN_WARNING "\nmpu_set_sensors() failed\n"); sprintf(status_string,"mpu_set_sensors() failed\n"); return -1; } //printk("."); if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printk(KERN_WARNING "\nmpu_configure_fifo() failed\n"); sprintf(status_string,"mpu_configure_fifo() failed\n"); return -1; } //printk("."); if (mpu_set_sample_rate(sample_rate)) { printk(KERN_WARNING "\nmpu_set_sample_rate() failed\n"); sprintf(status_string,"mpu_set_sample_rate() failed\n"); return -1; } //printk("."); if (mpu_set_compass_sample_rate(sample_rate)) { printk(KERN_WARNING "\nmpu_set_compass_sample_rate() failed\n"); return -1; } //printk("."); if (dmp_load_motion_driver_firmware()) { printk(KERN_WARNING "\ndmp_load_motion_driver_firmware() failed\n"); return -1; } //printk("."); if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { printk(KERN_WARNING "\ndmp_set_orientation() failed\n"); return -1; } //printk("."); if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { printk(KERN_WARNING "\ndmp_enable_feature() failed\n"); return -1; } //printk("."); if (dmp_set_fifo_rate(sample_rate)) { printk(KERN_WARNING "\ndmp_set_fifo_rate() failed\n"); return -1; } //printk("."); if (mpu_set_dmp_state(1)) { printk(KERN_WARNING "\nmpu_set_dmp_state(1) failed\n"); return -1; } printk(" done\n"); return 0; }
int mpulib_init(int i2c_bus, int sample_rate, int mix_factor) { signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) { printf("Invalid I2C bus %d\n", i2c_bus); return -1; } if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) { printf("Invalid sample rate %d\n", sample_rate); return -1; } if (mix_factor < 0 || mix_factor > 100) { printf("Invalid mag mixing factor %d\n", mix_factor); return -1; } yaw_mixing_factor = mix_factor; linux_set_i2c_bus(i2c_bus); printf("\nInitializing IMU ."); fflush(stdout); if (mpu_init(NULL)) { printf("\nmpu_init() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { printf("\nmpu_set_sensors() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("\nmpu_configure_fifo() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_set_sample_rate(sample_rate)) { printf("\nmpu_set_sample_rate() failed\n"); return -1; } printf("."); fflush(stdout); int compass_sample_rate = sample_rate; if( compass_sample_rate > MAX_COMPASS_SAMPLE_RATE ) { compass_sample_rate = MAX_COMPASS_SAMPLE_RATE; } if (mpu_set_compass_sample_rate(compass_sample_rate)) { printf("\nmpu_set_compass_sample_rate() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_load_motion_driver_firmware()) { printf("\ndmp_load_motion_driver_firmware() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { printf("\ndmp_set_orientation() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { printf("\ndmp_enable_feature() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_set_fifo_rate(sample_rate)) { printf("\ndmp_set_fifo_rate() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_set_dmp_state(1)) { printf("\nmpu_set_dmp_state(1) failed\n"); return -1; } printf(" done\n\n"); return 0; }
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor) { signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) { printf("Invalid sample rate %d\n", sample_rate); return -1; } if (mix_factor < 0 || mix_factor > 100) { printf("Invalid mag mixing factor %d\n", mix_factor); return -1; } yaw_mixing_factor = mix_factor; linux_set_i2c_bus(i2c_bus); printf("\nInitializing IMU ."); if (mpu_init(NULL)) { printf("\nmpu_init() failed\n"); return -1; } printf("."); #if defined AK8963_SECONDARY || defined AK8975_SECONDARY if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { printf("\nmpu_set_sensors() failed\n"); return -1; } #else if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("\nmpu_set_sensors() failed\n"); return -1; } #endif printf("."); if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("\nmpu_configure_fifo() failed\n"); return -1; } printf("."); if (mpu_set_sample_rate(200)) { printf("\nmpu_set_sample_rate() failed\n"); return -1; } printf("."); #if defined AK8963_SECONDARY || defined AK8975_SECONDARY if (mpu_set_compass_sample_rate(50)) { printf("\nmpu_set_compass_sample_rate() failed\n"); return -1; } #endif printf("."); if (dmp_load_motion_driver_firmware()) { printf("\ndmp_load_motion_driver_firmware() failed\n"); return -1; } printf("."); if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { printf("\ndmp_set_orientation() failed\n"); return -1; } printf("."); if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { printf("\ndmp_enable_feature() failed\n"); return -1; } printf("."); if (dmp_set_fifo_rate(sample_rate)) { printf("\ndmp_set_fifo_rate() failed\n"); return -1; } printf("."); if (mpu_set_dmp_state(1)) { printf("\nmpu_set_dmp_state(1) failed\n"); return -1; } printf(" done\n\n"); 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; }
boolean MPU9150Lib::init(int mpuRate, int magMix) { struct int_param_s int_param; int result; long accelOffset[3]; m_magMix = magMix; m_lastDMPYaw = 0; m_lastYaw = 0; // get calibration data if it's there if (calSensRead(&m_calData)) { // use calibration data if it's there and wanted m_useMagCalibration &= m_calData.magValid; m_useAccelCalibration &= m_calData.accelValid; // Process calibration data for runtime if (m_useMagCalibration) { m_magXOffset = (short)(((long)m_calData.magMaxX + (long)m_calData.magMinX) / 2); m_magXRange = m_calData.magMaxX - m_magXOffset; m_magYOffset = (short)(((long)m_calData.magMaxY + (long)m_calData.magMinY) / 2); m_magYRange = m_calData.magMaxY - m_magYOffset; m_magZOffset = (short)(((long)m_calData.magMaxZ + (long)m_calData.magMinZ) / 2); m_magZRange = m_calData.magMaxZ - m_magZOffset; } if (m_useAccelCalibration) { accelOffset[0] = -((long)m_calData.accelMaxX + (long)m_calData.accelMinX) / 2; accelOffset[1] = -((long)m_calData.accelMaxY + (long)m_calData.accelMinY) / 2; accelOffset[2] = -((long)m_calData.accelMaxZ + (long)m_calData.accelMinZ) / 2; mpu_set_accel_bias(accelOffset); m_accelXRange = m_calData.accelMaxX + accelOffset[0]; m_accelYRange = m_calData.accelMaxY + accelOffset[1]; m_accelZRange = m_calData.accelMaxZ + accelOffset[2]; } } #ifdef MPULIB_DEBUG if (m_useMagCalibration) Serial.println("Using mag cal"); if (m_useAccelCalibration) Serial.println("Using accel cal"); #endif mpu_init_structures(); // Not using interrupts so set up this structure to keep the driver happy int_param.cb = NULL; int_param.pin = 0; int_param.lp_exit = 0; int_param.active_low = 1; result = mpu_init(&int_param); if (result != 0) { #ifdef MPULIB_DEBUG Serial.print("mpu_init failed with code: "); Serial.println(result); #endif return false; } mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // enable all of the sensors mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // get accel and gyro data in the FIFO also mpu_set_sample_rate(mpuRate); // set the update rate mpu_set_compass_sample_rate(mpuRate); // set the compass update rate to match #ifdef MPULIB_DEBUG Serial.println("Loading firmware"); #endif if ((result = dmp_load_motion_driver_firmware()) != 0) { // try to load the DMP firmware #ifdef MPULIB_DEBUG Serial.print("Failed to load dmp firmware: "); Serial.println(result); #endif return false; } dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // set up the correct orientation dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL); dmp_set_fifo_rate(mpuRate); if (mpu_set_dmp_state(1) != 0) { #ifdef MPULIB_DEBUG Serial.println("mpu_set_dmp_state failed"); #endif return false; } return true; }
inv_error_t MPU9250_DMP::setCompassSampleRate(unsigned short rate) { return mpu_set_compass_sample_rate(rate); }
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor) { signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; if (i2c_bus < 0 || i2c_bus > 3) return -1; if (sample_rate < 2 || sample_rate > 50) return -1; if (mix_factor < 0 || mix_factor > 100) return -1; yaw_mixing_factor = mix_factor; linux_set_i2c_bus(i2c_bus); printf("\nInitializing IMU ."); fflush(stdout); if (mpu_init(NULL)) { printf("\nmpu_init() failed\n"); return -1; } printf("."); fflush(stdout); #ifdef AK89xx_SECONDARY if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { #else if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { #endif printf("\nmpu_set_sensors() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("\nmpu_configure_fifo() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_set_sample_rate(sample_rate)) { printf("\nmpu_set_sample_rate() failed\n"); return -1; } printf("."); fflush(stdout); #ifdef AK89xx_SECONDARY if (mpu_set_compass_sample_rate(sample_rate)) { printf("\nmpu_set_compass_sample_rate() failed\n"); return -1; } #endif printf("."); fflush(stdout); if (dmp_load_motion_driver_firmware()) { printf("\ndmp_load_motion_driver_firmware() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { printf("\ndmp_set_orientation() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { printf("\ndmp_enable_feature() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_set_fifo_rate(sample_rate)) { printf("\ndmp_set_fifo_rate() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_set_dmp_state(1)) { printf("\nmpu_set_dmp_state(1) failed\n"); return -1; } printf(" done\n\n"); return 0; } /* New functions to enable / disable 6axis on the fly */ int enableAccelerometerFusion(void) { if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { printf("Failure enabling accelerometer fusion\n"); return -1; } printf("mpu9150.c: Accelerometer fusion enabled\n"); return 0; } int disableAccelerometerFusion(void) { if (dmp_enable_feature(DMP_FEATURE_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { printf("Failure disabling accelerometer fusion\n"); return -1; } printf("mpu9150.c: Accelerometer fusion disabled\n"); return 0; } void mpu9150_exit() { // turn off the DMP on exit if (mpu_set_dmp_state(0)) printf("mpu_set_dmp_state(0) failed\n"); // TODO: Should turn off the sensors too } void mpu9150_set_accel_cal(caldata_t *cal) { int i; long bias[3]; if (!cal) { use_accel_cal = 0; return; } memcpy(&accel_cal_data, cal, sizeof(caldata_t)); for (i = 0; i < 3; i++) { if (accel_cal_data.range[i] < 1) accel_cal_data.range[i] = 1; else if (accel_cal_data.range[i] > ACCEL_SENSOR_RANGE) accel_cal_data.range[i] = ACCEL_SENSOR_RANGE; bias[i] = -accel_cal_data.offset[i]; } if (debug_on) { printf("\naccel cal (range : offset)\n"); for (i = 0; i < 3; i++) printf("%d : %d\n", accel_cal_data.range[i], accel_cal_data.offset[i]); } mpu_set_accel_bias(bias); use_accel_cal = 1; }