/** * @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 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); }
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"); } }
/** * @brief MPU (6500)初始化 * @param None * @retval None */ void InvMPU_Driver_Init(void) { u8 ret = 0; long accel[3]; struct int_param_s int_param; assert_param(MPU_SPI == &hspi1); MPU_SPIDeselect(); ret += mpu_init(&int_param); ret += mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); ret += mpu_set_sample_rate(1000); ret += mpu_set_gyro_fsr( GYRO_FSR); ret += mpu_set_accel_fsr( ACCEL_FSR); accel[0] = 46; accel[1] = 78; accel[2] = 0; mpu_set_accel_bias_6500_reg(accel); #ifdef USE_DMP dmp_load_motion_driver_firmware(); dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_SEND_RAW_ACCEL); dmp_set_fifo_rate(200); u8 offset[12] = {0,0, 0xd2, 0x2b, 0x00, 0x01, 0xba, 0xd8, 0x00, 0x00, 0x9f, 0xa0}; mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, &offset[0]); mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, &offset[4]); mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, &offset[8]); mpu_set_dmp_state(1); #endif }
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"); }
inv_error_t MPU9250_DMP::setSampleRate(unsigned short rate) { return mpu_set_sample_rate(rate); }
int mpu9250_init(int i2c_bus, int sample_rate) { 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; } linux_set_i2c_bus(i2c_bus); printf("\nInitializing IMU ."); fflush(stdout); //*******Work around in mpu_init werden sensoren gelöscht, müssen aber vorher != 0 sein!! if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { printf("\nmpu_set_sensors() failed\n"); return -1; } //************** if (mpu_init(NULL)) { printf("\nmpu_init() failed\n"); return -1; } if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { printf("\nmpu_set_sensors() failed\n"); return -1; } // if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { // printf("\nmpu_configure_fifo() failed\n"); // return -1; // } if (mpu_set_sample_rate(sample_rate)) { printf("\nmpu_set_sample_rate() failed\n"); return -1; } // if (dmp_load_motion_driver_firmware()) { // printf("\ndmp_load_motion_driver_firmware() failed\n"); // return -1; // } // // if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { // printf("\ndmp_set_orientation() failed\n"); // return -1; // } // // 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; // } // // if (dmp_set_fifo_rate(sample_rate)) { // printf("\ndmp_set_fifo_rate() failed\n"); // return -1; // } // // if (mpu_set_dmp_state(1)) { // printf("\nmpu_set_dmp_state(1) failed\n"); // return -1; // } printf(" done\n\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; }
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; }
// 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; }
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(){ 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; }
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 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; }
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 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; }
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; }
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; }
bool MPUManager::__StartDevice( bool p_bSelfTest ) { if( 0 != mpu_init() ) { printf("mpu_init failed\n"); return false; } #ifdef __DEBUG __OutputDeviceStatus(); #endif /* Wake up all sensors. */ if( 0 != mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) ) { printf("mpu_set_sensors failed\n"); return false; } /* Push both gyro and accel data into the FIFO. */ if( 0 != mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) ) { printf("mpu_configure_fifo failed\n"); return false; } if( 0 != mpu_set_sample_rate(m_usSampleRate) ) { printf("mpu_set_sample_rate failed\n"); return false; } if( 0!= dmp_load_motion_driver_firmware() ) { printf("dmp_load_motion_driver_firmware failed\n"); return false; } if( 0 != dmp_enable_feature(m_usDefaultFeatures) ) { printf("dmp_enable_feature failed\n"); return false; } if( 0 != dmp_set_fifo_rate(m_usSampleRate) ) { printf("dmp_set_fifo_rate failed\n"); return false; } if( 0 != mpu_get_gyro_sens( &m_fCurGyroSensitivity ) ) { printf("mpu_get_gyro_sens failed\n"); return false; } else { printf("Current Gyro Sensitivity: %.2f\n", m_fCurGyroSensitivity); } if( 0 != mpu_get_accel_sens(&m_usCurAccelSensitivity) ) { printf("mpu_get_accel_sens failed\n"); return false; } else { printf("Current Accel Sensitivity: %d\n", m_usCurAccelSensitivity); } m_usPackageLength = __dmp_get_packet_length(); unsigned short usfr = 0; dmp_get_fifo_rate( &usfr ); printf("DMP start success\npackage length = %d\nFIFO rate = %d\n", m_usPackageLength, usfr); if( p_bSelfTest ) { if( __RunSelfTest() ) { printf("Self test passed\n"); } } if( 0 != m_sGyroOffset[0] ) { if( !__mpu_set_user_x_gyro_offset(m_sGyroOffset[0]) ) { printf("set_user_x_gyro_offset %d failed\n", m_sGyroOffset[0]); } } if( 0 != m_sGyroOffset[1] ) { if( !__mpu_set_user_y_gyro_offset(m_sGyroOffset[1]) ) { printf("set_user_y_gyro_offset %d failed\n", m_sGyroOffset[1]); } } if( 0 != m_sGyroOffset[2] ) { if( !__mpu_set_user_z_gyro_offset(m_sGyroOffset[2]) ) { printf("set_user_z_gyro_offset %d failed\n", m_sGyroOffset[2]); } } if( 0 != m_sAccelOffset[0] ) { if( !__mpu_set_x_accel_offset(m_sAccelOffset[0]) ) { printf("mpu_set_x_accel_offset %d failed\n", m_sAccelOffset[0]); } } if( 0 != m_sAccelOffset[1] ) { if( !__mpu_set_y_accel_offset(m_sAccelOffset[1]) ) { printf("mpu_set_y_accel_offset %d failed\n", m_sAccelOffset[1]); } } if( 0 != m_sAccelOffset[2] ) { if( !__mpu_set_z_accel_offset(m_sAccelOffset[2]) ) { printf("mpu_set_z_accel_offset %d failed\n", m_sAccelOffset[2]); } } if( 0 != mpu_set_dmp_state(1) ) { printf("mpu_set_dmp_state 1 failed\n"); return false; } if( 0 == mpu_set_lpf(42) ) { printf("mpu_set_lpf failed\n"); return false; } #ifdef __DEBUG __OutputDeviceStatus(); #endif return true; }