int init(){ open_bus(); unsigned char whoami=0; i2c_read(MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &whoami); if(!silent_flag) { printf("WHO_AM_I: %x\n", whoami); } struct int_param_s int_param; if(!silent_flag) { printf("MPU init: %i\n", mpu_init(&int_param)); printf("MPU sensor init: %i\n", mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)); printf("MPU configure fifo: %i\n", mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)); printf("DMP firmware: %i\n ",dmp_load_motion_driver_firmware()); printf("DMP orientation: %i\n ",dmp_set_orientation( inv_orientation_matrix_to_scalar(gyro_orientation))); } unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL; if(!silent_flag) { printf("DMP feature enable: %i\n", dmp_enable_feature(dmp_features)); printf("DMP set fifo rate: %i\n", dmp_set_fifo_rate(DEFAULT_MPU_HZ)); printf("DMP enable %i\n", mpu_set_dmp_state(1)); } if(!no_interrupt_flag) { mpu_set_int_level(1); // Interrupt is low when firing dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); // Fire interrupt on new FIFO value } return 0; }
/** * @brief mpu6050_init * @param void * @retval 成功为0 * @note 初始化mpu6050 频率为100*1000HZ */ int16_t mpu6050_init(void) { I2C_QuickInit(I2C0_SCL_PB02_SDA_PB03,100*1000); int result=0; result=mpu_init(); if(!result) { PrintChar("mpu initialization complete......\n "); //mpu initialization complete if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor PrintChar("mpu_set_sensor complete ......\n"); else PrintChar("mpu_set_sensor come across error ......\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo PrintChar("mpu_configure_fifo complete ......\n"); else PrintChar("mpu_configure_fifo come across error ......\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate PrintChar("mpu_set_sample_rate complete ......\n"); else PrintChar("mpu_set_sample_rate error ......\n"); if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare PrintChar("dmp_load_motion_driver_firmware complete ......\n"); else PrintChar("dmp_load_motion_driver_firmware come across error ......\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation PrintChar("dmp_set_orientation complete ......\n"); else PrintChar("dmp_set_orientation come across error ......\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature PrintChar("dmp_enable_feature complete ......\n"); else PrintChar("dmp_enable_feature come across error ......\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate PrintChar("dmp_set_fifo_rate complete ......\n"); else PrintChar("dmp_set_fifo_rate come across error ......\n"); run_self_test(); //自检 if(!mpu_set_dmp_state(1)) PrintChar("mpu_set_dmp_state complete ......\n"); else PrintChar("mpu_set_dmp_state come across error ......\n"); } return 0; }
/************************************************************************** 函数功能:MPU6050内置DMP的初始化 入口参数:无 返回 值:无 作 者:平衡小车之家 **************************************************************************/ void DMP_Init(void) { u8 temp[1]={0}; i2cRead(0x68,0x75,1,temp); printf("mpu_set_sensor complete ......\r\n"); if(temp[0]!=0x68)NVIC_SystemReset(); if(!mpu_init()) { if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) printf("mpu_set_sensor complete ......\r\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) printf("mpu_configure_fifo complete ......\r\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) printf("mpu_set_sample_rate complete ......\r\n"); if(!dmp_load_motion_driver_firmware()) printf("dmp_load_motion_driver_firmware complete ......\r\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) printf("dmp_set_orientation complete ......\r\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) printf("dmp_enable_feature complete ......\r\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) printf("dmp_set_fifo_rate complete ......\r\n"); run_self_test(); if(!mpu_set_dmp_state(1)) printf("mpu_set_dmp_state complete ......\r\n"); } }
void init_mpu6050(struct eeprom *e) { int result=0; die_if(eeprom_open("/dev/i2c/0", 0x68, EEPROM_TYPE_8BIT_ADDR, e) < 0, "unable to open MPU6050 device file "); result=mpu_init(); if(!result) { printf("mpu initialization complete......\n "); //mpu initialization complete if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor printf("mpu_set_sensor complete ......\n"); else printf("mpu_set_sensor come across error ......\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo printf("mpu_configure_fifo complete ......\n"); else printf("mpu_configure_fifo come across error ......\n"); if(!mpu_set_sample_rate(50)) //mpu_set_sample_rate printf("mpu_set_sample_rate complete ......\n"); else printf("mpu_set_sample_rate error ......\n"); if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare printf("dmp_load_motion_driver_firmware complete ......\n"); else printf("dmp_load_motion_driver_firmware come across error ......\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation printf("dmp_set_orientation complete ......\n"); else printf("dmp_set_orientation come across error ......\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature printf("dmp_enable_feature complete ......\n"); else printf("dmp_enable_feature come across error ......\n"); if(!dmp_set_fifo_rate(50)) //dmp_set_fifo_rate printf("dmp_set_fifo_rate complete ......\n"); else printf("dmp_set_fifo_rate come across error ......\n"); run_self_test(); //自检 if(!mpu_set_dmp_state(1)) printf("mpu_set_dmp_state complete ......\n"); else printf("mpu_set_dmp_state come across error ......\n"); } else { printf("mpu INIT INIT INIT error ......\r\n"); } }
/****************************************************************************** * 函数名称: MPU6050_Dmp_Init * 函数功能: MPU dmp初始化 * 入口参数: 无 * 返回值 无 ******************************************************************************/ void MPU9250_Dmp_Init(void) { SPI_Init(); mpu_set_lpf(5); while(mpu_init()); //mpu_set_sensor mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); //mpu_configure_fifo mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); //mpu_set_sample_rate mpu_set_sample_rate(DEFAULT_MPU_HZ); //dmp_load_motion_driver_firmvare dmp_load_motion_driver_firmware(); //dmp_set_orientation dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); //dmp_enable_feature dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL); //dmp_set_fifo_rate dmp_set_fifo_rate(DEFAULT_MPU_HZ); //run_self_test(); mpu_set_dmp_state(1); }
//// 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; }
/**********************************MPU6050*************************************/ void MPU6050_INIT() { mpu_init(); mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);//#(INV_XYZ_GYRO | INV_XYZ_ACCEL)在inv_mpu.h定义 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); mpu_set_sample_rate(DEFAULT_MPU_HZ);//#本文件开头定义 dmp_load_motion_driver_firmware(); dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//#本文定义 dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |DMP_FEATURE_GYRO_CAL;//#全部在(inv_mpu_dmp_motion_driver.h)中定义 dmp_enable_feature(dmp_features); dmp_set_fifo_rate(DEFAULT_MPU_HZ); run_self_test(); mpu_set_dmp_state(1); }
/** **************************************************************************************** * @brief start mpu6050 dmp * @description * This function is used to initialize mpu6050 dmp. ***************************************************************************************** */ void mpu_start() { int result = mpu_init(NULL); while(result != MPU_OK) { result = mpu_init(NULL); } if(result == MPU_OK) { printf("mpu initialization complete......\n "); //mpu_set_sensor if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK) printf("mpu_set_sensor complete ......\n"); else printf("mpu_set_sensor come across error ......\n"); if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK) //mpu_configure_fifo printf("mpu_configure_fifo complete ......\n"); else printf("mpu_configure_fifo come across error ......\n"); if(mpu_set_sample_rate(DEFAULT_MPU_HZ) == MPU_OK) //mpu_set_sample_rate printf("mpu_set_sample_rate complete ......\n"); else printf("mpu_set_sample_rate error ......\n"); if(dmp_load_motion_driver_firmware() == MPU_OK) //dmp_load_motion_driver_firmvare printf("dmp_load_motion_driver_firmware complete ......\n"); else printf("dmp_load_motion_driver_firmware come across error ......\n"); if(dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)) == MPU_OK) //dmp_set_orientation printf("dmp_set_orientation complete ......\n"); else printf("dmp_set_orientation come across error ......\n"); if(dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL) == MPU_OK) //dmp_enable_feature printf("dmp_enable_feature complete ......\n"); else printf("dmp_enable_feature come across error ......\n"); if(dmp_set_fifo_rate(DEFAULT_MPU_HZ) == MPU_OK) //dmp_set_fifo_rate printf("dmp_set_fifo_rate complete ......\n"); else printf("dmp_set_fifo_rate come across error ......\n"); run_self_test(); if(mpu_set_dmp_state(1) == MPU_OK) printf("mpu_set_dmp_state complete ......\n"); else printf("mpu_set_dmp_state come across error ......\n"); } }
void sendConfig() { if (verbose) printf("AVRSPI: Sending config\n"); spi_sendIntPacket(3,0); //initial mode spi_sendIntPacket(2 ,log_mode); //log mode int gyro_orientation = inv_orientation_matrix_to_scalar(config.gyro_orient); spi_sendIntPacket(4,gyro_orientation); spi_sendIntPacket(9,config.mpu_addr); spi_sendIntPacket(17,config.throttle_min); spi_sendIntPacket(18,config.throttle_inflight); spi_sendIntPacket(19,config.throttle_midflight); uint8_t motor_order = (config.motor_pin[0]) | (config.motor_pin[1] << 2) | (config.motor_pin[2] << 4) | (config.motor_pin[3] << 6); spi_sendIntPacket(5,motor_order); spi_sendIntPacket(69,config.baro_f); //PIDS for (int i=0;i<3;i++) for (int j=0;j<5;j++) { spi_sendIntPacket(100+i*10+j,config.r_pid[i][j]); spi_sendIntPacket(200+i*10+j,config.s_pid[i][j]); } for (int i=0;i<5;i++) { spi_sendIntPacket(70+i,config.accel_pid[i]); spi_sendIntPacket(80+i,config.alt_pid[i]); spi_sendIntPacket(90+i,config.vz_pid[i]); } spi_sendIntPacket(130,config.a_pid[0]); spi_sendIntPacket(255,2); if (verbose) printf("AVRSPI: Config sent.\n"); }
void dmp_init() { Timer4_init(); while (1 == mpu_init()); //mpu_set_sensor while (1 == mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)); //mpu_configure_fifo while (1 == mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)); //mpu_set_sample_rate while (1 == mpu_set_sample_rate(DEFAULT_MPU_HZ)); //dmp_load_motion_driver_firmvare while (1 == dmp_load_motion_driver_firmware()); //dmp_set_orientation while (1 == dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))); //dmp_enable_feature while (1 == dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)); //dmp_set_fifo_rate while (1 == dmp_set_fifo_rate(DEFAULT_MPU_HZ)); run_self_test(); while (1 == mpu_set_dmp_state(1)); rt_kprintf("start mpu6050\n"); rt_kprintf("start ahrs\n"); }
int INVMPU9150::init(int i2cBus, int frequency) { // Set linux i2c bus linux_set_i2c_bus(i2cBus); // The gyro orientation signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; // Init MPU if (mpu_init(NULL)) { return MPU9150_INIT_MPU_INIT_ERROR; } // Set sensors if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { return MPU9150_INIT_MPU_SET_SENSORS_ERROR; } // Get gyro sensitivity scale factor and convert from [LSB/(degree/s)] to [LSB/(rad/s)] float gyroSsfDegreePerSecond; if (mpu_get_gyro_sens(& gyroSsfDegreePerSecond)) { return MPU9150_INIT_MPU_GET_GYRO_FSR_ERROR; } gyroSsf = gyroSsfDegreePerSecond * 180 / M_PI; // Get accel sensitivity scale factor if (mpu_get_accel_sens(& accelSsf)) { return MPU9150_INIT_MPU_GET_ACCEL_FSR_ERROR; } // Configure FIFO if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { return MPU9150_INIT_MPU_CONFIGURE_FIFO_ERROR; } // Set sample rate for gyro/acc if (mpu_set_sample_rate(frequency)) { return MPU9150_INIT_MPU_SET_SAMPLE_RATE_ERROR; } // Load motion driver firmare (DMP) if (dmp_load_motion_driver_firmware()) { return MPU9150_INIT_DMP_LOAD_MOTION_DRIVER_FIRMWARE_ERROR; } // Set gyro orientation if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { return MPU9150_INIT_DMP_SET_ORIENTATION_ERROR; } // Enable DMP features if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { return MPU9150_INIT_DMP_ENABLE_FEATURES_ERROR; } // Set sample rate for DMP if (dmp_set_fifo_rate(frequency)) { return MPU9150_INIT_DMP_SET_FIFO_RATE_ERROR; } // Enable DMP if (mpu_set_dmp_state(1)) { return MPU9150_INIT_DMP_SET_STATE_ERROR; } return MPU9150_OK; }
// Private int8_t IMUTask::MPUSetup() { // TODO: should check here to see if IMU is already setup and just init the lib int8_t result = 0; struct int_param_s int_param; int_param.cb = IMU_interrupt; int_param.pin = MPU_INT; int_param.arg = FALLING; unsigned char accel_fsr; unsigned short gyro_rate, gyro_fsr; unsigned long timestamp; result = mpu_init(&int_param); debug::log("IMUTask: mpu_init returned " + String(result)); if (result != 0) { return -1; } /* Wake up all sensors. */ result = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); debug::log("IMUTask: mpu_set_sensors returned " + String(result)); result = mpu_set_sample_rate(IMU_DEFAULT_MPU_HZ); debug::log("IMUTask: mpu_set_sample_rate returned " + String(result)); debug::log("Pre laod"); result = dmp_load_motion_driver_firmware(); debug::log("IMUTask: dmp_load_motion_driver_firmware returned " + String(result)); if (result != 0) { return -2; } result = dmp_set_orientation( inv_orientation_matrix_to_scalar(gyro_orientation) ); debug::log("IMUTask: dmp_set_orientation returned " + String(result)); if (result != 0) { return -3; } /* * Known Bug - * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate * specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once * a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz * there will be a 25Hz interrupt from the MPU device. * * There is a known issue in which if you do not enable DMP_FEATURE_TAP * then the interrupts will be at 200Hz even if fifo rate * is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP */ unsigned short dmp_features = DMP_FEATURE_TAP | // Don't remove this, see above DMP_FEATURE_ANDROID_ORIENT; // we use this for the screen // Disableing the rest for now as we dont yet have a use for them // DMP_FEATURE_SEND_RAW_ACCEL | // DMP_FEATURE_SEND_RAW_GYRO; result = dmp_enable_feature(dmp_features); if (result != 0) { debug::log("IMUTask: mp_enable_feature returned " + String(result)); return -4; } result = dmp_set_fifo_rate(IMU_DEFAULT_MPU_HZ); if (result != 0) { debug::log("IMUTask: dmp_set_fifo_rate returned " + String(result)); return -5; } dmp_register_tap_cb(IMU_tap_cb); dmp_register_android_orient_cb(IMU_android_orient_cb); dmp_set_interrupt_mode(DMP_INT_GESTURE); 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; }
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; }
uint8_t MPU_Init(void) { GPIO_InitTypeDef gpio; NVIC_InitTypeDef nvic; EXTI_InitTypeDef exti; int res=0; IIC_Init(); RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); gpio.GPIO_Pin = GPIO_Pin_5; gpio.GPIO_Mode = GPIO_Mode_IN; gpio.GPIO_OType = GPIO_OType_PP; gpio.GPIO_PuPd = GPIO_PuPd_UP; gpio.GPIO_Speed = GPIO_Speed_100MHz; GPIO_Init(GPIOB, &gpio); SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB,GPIO_PinSource5); exti.EXTI_Line = EXTI_Line5; exti.EXTI_Mode = EXTI_Mode_Interrupt; exti.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿中断 exti.EXTI_LineCmd = ENABLE; EXTI_Init(&exti); nvic.NVIC_IRQChannel = EXTI9_5_IRQn; nvic.NVIC_IRQChannelPreemptionPriority = ITP_MPU_EXTI9_5_PREEMPTION; nvic.NVIC_IRQChannelSubPriority = ITP_MPU_EXTI9_5_SUB; nvic.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&nvic); if(mpu_init()==0) //初始化MPU6050 { res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器 if(res)return 1; res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO if(res)return 2; res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率 if(res)return 3; res=dmp_load_motion_driver_firmware(); //加载dmp固件 if(res)return 4; res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向 if(res)return 5; res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能 DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO| DMP_FEATURE_GYRO_CAL); if(res)return 6; res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz) if(res)return 7; res=mpu_set_int_level(1); if(res)return 8; res=dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); if(res)return 9; res=run_self_test(); //自检 if(res)return 10; res=mpu_set_dmp_state(1); //使能DMP if(res)return 11; } else return 12; return 0; }
int ms_open(unsigned int rate) { dmpReady=1; initialized = 0; // initialize device printf("Initializing MPU...\n"); r=mpu_init(NULL); if (r != 0) { printf("MPU init failed! %i\n",r); return -1; } printf("Setting MPU sensors...\n"); if (mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) { printf("Failed to set sensors!\n"); return -1; } printf("Setting GYRO sensitivity...\n"); if (mpu_set_gyro_fsr(FSR)!=0) { printf("Failed to set gyro sensitivity!\n"); return -1; } printf("Setting ACCEL sensitivity...\n"); if (mpu_set_accel_fsr(2)!=0) { printf("Failed to set accel sensitivity!\n"); return -1; } // verify connection printf("Powering up MPU...\n"); mpu_get_power_state(&devStatus); printf(devStatus ? "MPU6050 connection successful\n" : "MPU6050 connection failed %u\n",devStatus); //fifo config printf("Setting MPU fifo...\n"); if (mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) { printf("Failed to initialize MPU fifo!\n"); return -1; } // load and configure the DMP printf("Loading DMP firmware...\n"); if (dmp_load_motion_driver_firmware()!=0) { printf("Failed to load DMP firmware!\n"); return -1; } printf("Setting GYRO orientation...\n"); if (dmp_set_orientation( inv_orientation_matrix_to_scalar( gyro_orientation ) )!=0) { printf("Failed to set gyro orientation!\n"); return -1; } printf("Setting DMP fifo rate to: %i\n",rate); if (dmp_set_fifo_rate(rate)!=0) { printf("Failed to set dmp fifo rate!\n"); return -1; } printf("Activating DMP...\n"); if (mpu_set_dmp_state(1)!=0) { printf("Failed to enable DMP!\n"); return -1; } //dmp_set_orientation() //if (dmp_enable_feature(DMP_FEATURE_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) { printf("Configuring DMP...\n"); if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) { //if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) { printf("Failed to enable DMP features!\n"); return -1; } printf("Resetting fifo queue...\n"); if (mpu_reset_fifo()!=0) { printf("Failed to reset fifo!\n"); return -1; } printf("Checking... "); do { delay_ms(5*1000/rate); //dmp will habve 4 (5-1) packets based on the fifo_rate r=dmp_read_fifo(g,a,_q,&sensors,&fifoCount); } while (r!=0 || fifoCount<5); //packtets!!! printf("Collected: %i packets.\n",fifoCount); ms_update(); initialized = 1; uint16_t lpf,g_fsr,a_sens,dmp_rate; uint8_t dmp,a_fsr; float g_sens; printf("MPU config:\n"); mpu_get_lpf(&lpf); printf("MPU LPF: %u\n",lpf); dmp_get_fifo_rate(&dmp_rate); printf("DMP Fifo Rate: %u\n",dmp_rate); mpu_get_dmp_state(&dmp); printf("MPU DMP State: %u\n",dmp); mpu_get_gyro_fsr(&g_fsr); printf("MPU gyro FSR: %u\n",g_fsr); mpu_get_gyro_sens(&g_sens); printf("MPU gyro sens: %2.3f\n",g_sens); mpu_get_accel_fsr(&a_fsr); printf("MPU accel FSR: %u\n",a_fsr); mpu_get_accel_sens(&a_sens); printf("MPU accel sens: %u\n",a_sens); 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 (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; }
int mpu6050_init() { /* ** Configures I2C to Master mode to generate start ** condition on I2C bus and to transmit data at a ** speed of 100khz */ SetupI2C(); rt_hw_interrupt_install(SYS_INT_I2C0INT, mpu6050_isr, NULL, "mpu6050"); rt_hw_interrupt_mask(SYS_INT_I2C0INT); int result = mpu_init(); if(!result) { rt_kprintf("mpu initialization complete......\n "); //mpu_set_sensor if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) rt_kprintf("mpu_set_sensor complete ......\n"); else rt_kprintf("mpu_set_sensor come across error ......\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo rt_kprintf("mpu_configure_fifo complete ......\n"); else rt_kprintf("mpu_configure_fifo come across error ......\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate rt_kprintf("mpu_set_sample_rate complete ......\n"); else rt_kprintf("mpu_set_sample_rate error ......\n"); if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare rt_kprintf("dmp_load_motion_driver_firmware complete ......\n"); else rt_kprintf("dmp_load_motion_driver_firmware come across error ......\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation rt_kprintf("dmp_set_orientation complete ......\n"); else rt_kprintf("dmp_set_orientation come across error ......\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature rt_kprintf("dmp_enable_feature complete ......\n"); else rt_kprintf("dmp_enable_feature come across error ......\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate rt_kprintf("dmp_set_fifo_rate complete ......\n"); else rt_kprintf("dmp_set_fifo_rate come across error ......\n"); run_self_test(); if(!mpu_set_dmp_state(1)) rt_kprintf("mpu_set_dmp_state complete ......\n"); else rt_kprintf("mpu_set_dmp_state come across error ......\n"); } while(1) { dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); /* Gyro and accel data are written to the FIFO by the DMP in chip * frame and hardware units. This behavior is convenient because it * keeps the gyro and accel outputs of dmp_read_fifo and * mpu_read_fifo consistent. */ /* if (sensors & INV_XYZ_GYRO ) send_packet(PACKET_TYPE_GYRO, gyro); if (sensors & INV_XYZ_ACCEL) send_packet(PACKET_TYPE_ACCEL, accel); */ /* Unlike gyro and accel, quaternions are written to the FIFO in * the body frame, q30. The orientation is set by the scalar passed * to dmp_set_orientation during initialization. */ if (sensors & INV_WXYZ_QUAT ) { q0=quat[0] / q30; q1=quat[1] / q30; q2=quat[2] / q30; q3=quat[3] / q30; Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; // printf("Pitch=%.2f ,Roll=%.2f ,Yaw=%.2f \n",Pitch,Roll,Yaw); UART1_ReportIMU(Yaw*10, Pitch*10, Roll*10,0,0,0,100); delay_ms(10); } } 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 __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 MPU_DMP_Init() { int result = 0; //mpu_init(); printf("mpu initialization complete......\n "); //mpu_set_sensor if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("mpu_set_sensor complete ......\n"); } else { printf("mpu_set_sensor come across error ......\n"); result |=0x0001; } //mpu_configure_fifo if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("mpu_configure_fifo complete ......\n"); } else { printf("mpu_configure_fifo come across error ......\n"); result |=0x0002; } //mpu_set_sample_rate if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) { printf("mpu_set_sample_rate complete ......\n"); } else { printf("mpu_set_sample_rate error ......\n"); result |=0x0004; } //dmp_load_motion_driver_firmvare if(!dmp_load_motion_driver_firmware()) { printf("dmp_load_motion_driver_firmware complete ......\n"); } else { printf("dmp_load_motion_driver_firmware come across error ......\n"); result |=0x0008; } //dmp_set_orientation if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { printf("dmp_set_orientation complete ......\n"); } else { printf("dmp_set_orientation come across error ......\n"); result |=0x0010; } //dmp_enable_feature if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | //DMP_FEATURE_SEND_RAW_GYRO | // DMP_FEATURE_ANDROID_ORIENT | //By Damm Stagner DMP_FEATURE_GYRO_CAL)) { printf("dmp_enable_feature complete ......\n"); } else { printf("dmp_enable_feature come across error ......\n"); result |=0x0020; } //dmp_set_fifo_rate if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) { printf("dmp_set_fifo_rate complete ......\n"); } else { printf("dmp_set_fifo_rate come across error ......\n"); result |=0x0040; } run_self_test(); if(!mpu_set_dmp_state(1)) { printf("mpu_set_dmp_state complete ......\n"); } else { printf("mpu_set_dmp_state come across error ......\n"); result |=0x0080; } return result; }