コード例 #1
0
ファイル: mpumanager.cpp プロジェクト: A-khodary/mpu6050Pi
bool MPUManager::SetAccelOffsetZ( short p_sVal )
{
	unsigned char ucEnabled;

	m_sAccelOffset[2] = p_sVal;
	if( !m_bRunning )
	{
		if( 0 != mpu_get_dmp_state(&ucEnabled) )
		{
			printf("mpu_get_dmp_statefailed\n");
			return false;
		}

		if( 0 != ucEnabled && 0 != mpu_set_dmp_state(0) )
		{
			printf("mpu_set_dmp_state 0 failed\n");
			return false;
		}

		if( !__mpu_set_z_accel_offset(p_sVal) )
		{
			printf("set_user_z_accel_offset %d failed\n", p_sVal);
		}

		if( 0 != mpu_set_dmp_state(ucEnabled) )
		{
			printf("mpu_set_dmp_state 1 failed\n");
		}
	}

	return true;
}
コード例 #2
0
ファイル: mpu.cpp プロジェクト: EdisonLiang/ArduinoDrone
int mympu_open(unsigned int rate) {
  	mpu_select_device(0);
   	mpu_init_structures();

	ret = mpu_init(NULL);
#ifdef MPU_DEBUG
	if (ret) return 10+ret;
#endif
	
	ret = mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL); 
#ifdef MPU_DEBUG
	if (ret) return 20+ret;
#endif

        ret = mpu_set_gyro_fsr(FSR);
#ifdef MPU_DEBUG
	if (ret) return 30+ret;
#endif

        ret = mpu_set_accel_fsr(2);
#ifdef MPU_DEBUG
	if (ret) return 40+ret;
#endif

        mpu_get_power_state((unsigned char *)&ret);
#ifdef MPU_DEBUG
	if (!ret) return 50+ret;
#endif

        ret = mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);
#ifdef MPU_DEBUG
	if (ret) return 60+ret;
#endif

	dmp_select_device(0);
	dmp_init_structures();

	ret = dmp_load_motion_driver_firmware();
#ifdef MPU_DEBUG
	if (ret) return 80+ret;
#endif

	ret = dmp_set_fifo_rate(rate);
#ifdef MPU_DEBUG
	if (ret) return 90+ret;
#endif

	ret = mpu_set_dmp_state(1);
#ifdef MPU_DEBUG
	if (ret) return 100+ret;
#endif

	ret = dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
//	ret = dmp_enable_feature(DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
#ifdef MPU_DEBUG
	if (ret) return 110+ret;
#endif

	return 0;
}
コード例 #3
0
/*******************************************************************************
 * FUNCTION: ssInitIMU
 *
 * PARAMETERS:
 * ~ void
 *
 * RETURN:
 * ~ Return 0 if successful.
 *
 * DESCRIPTIONS:
 * Initialized the IMU.
 *
 *******************************************************************************/
signed short ssInitIMU(void)
{
    // Initialize the IMU. Return if failed.
    signed short ssErrorCode = mpu_init(NULL);
    if (ssErrorCode != 0) {
        return ssErrorCode;
    }

    // Wake up all sensors.
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);

    // Push both gyro and accel data into the FIFO.
    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);

    /* To initialize the DMP:
     * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
     *    inv_mpu_dmp_motion_driver.h into the MPU memory.
     * 2. Push the gyro and accel orientation matrix to the DMP.
     * 3. Register gesture callbacks. Don't worry, these callbacks won't be
     *    executed unless the corresponding feature is enabled.
     * 4. Call dmp_enable_feature(mask) to enable different features.
     * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.
     * 6. Call any feature-specific control functions.
     *
     * To enable the DMP, just call mpu_set_dmp_state(1). This function can
     * be called repeatedly to enable and disable the DMP at runtime.
     *
     * The following is a short summary of the features supported in the DMP
     * image provided in inv_mpu_dmp_motion_driver.c:
     * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at
     * 200Hz. Integrating the gyro data at higher rates reduces numerical
     * errors (compared to integration on the MCU at a lower sampling rate).
     * DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at
     * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.
     * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.
     * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers
     * an event at the four orientations where the screen should rotate.
     * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of
     * no motion.
     * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.
     * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.
     * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
     * be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
     */
    dmp_load_motion_driver_firmware();
    /*
     * 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
     */
    dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_SEND_RAW_ACCEL);
    dmp_set_fifo_rate(10);
    mpu_set_dmp_state(1);
}
コード例 #4
0
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;
}
コード例 #5
0
ファイル: mpu_transplant.c プロジェクト: yanbib/smartcar
 /**
 * @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;
}
コード例 #6
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");
  }
}
コード例 #7
0
ファイル: mpulib.c プロジェクト: mickele77/MPULib
void mpulib_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
}
コード例 #8
0
ファイル: mpu6050.c プロジェクト: scczht/mvr_main
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");
	}
}
コード例 #9
0
ファイル: MPU9250DMP.c プロジェクト: nhlijiaming/Quadrotor
/******************************************************************************
 * 函数名称: 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);
}
コード例 #10
0
inv_error_t MPU9250_DMP::dmpBegin(unsigned short features, unsigned short fifoRate)
{
	unsigned short feat = features;
	unsigned short rate = fifoRate;

	if (dmpLoad() != INV_SUCCESS)
		return INV_ERROR;

	// 3-axis and 6-axis LP quat are mutually exclusive.
	// If both are selected, default to 3-axis
	if (feat & DMP_FEATURE_LP_QUAT)
	{
		feat &= ~(DMP_FEATURE_6X_LP_QUAT);
		dmp_enable_lp_quat(1);
	}
	else if (feat & DMP_FEATURE_6X_LP_QUAT)
		dmp_enable_6x_lp_quat(1);

	if (feat & DMP_FEATURE_GYRO_CAL)
		dmp_enable_gyro_cal(1);

	if (dmpEnableFeatures(feat) != INV_SUCCESS)
		return INV_ERROR;

	rate = constrain(rate, 1, 200);
	if (dmpSetFifoRate(rate) != INV_SUCCESS)
		return INV_ERROR;

	return mpu_set_dmp_state(1);
}
コード例 #11
0
//// 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, &params);
	
	return 0;
}
コード例 #12
0
void __exit mpu9150_exit()
{
	// turn off the DMP on exit 
	if (mpu_set_dmp_state(0))
		printk(KERN_WARNING"mpu_set_dmp_state(0) failed\n");

	// TODO: Should turn off the sensors too
}
コード例 #13
0
ファイル: mpu9150.cpp プロジェクト: trigrass2/geduino-ros
int INVMPU9150::dispose() {

	// Set dmp status to off
	if (mpu_set_dmp_state(0)) {
		return MPU9150_ERROR;
	} else {
		return MPU9150_OK;
	}

}
コード例 #14
0
ファイル: IMUTask.cpp プロジェクト: ThucVD2704/Mk2-Firmware
int8_t IMUTask::enable_dmp()
{
    int8_t result = 0;
    result = mpu_set_dmp_state(1);
    debug::log("IMUTask: mpu_set_dmp_state(1) returned " + String(result));
    if (result != 0) {
        return -1;
    }
    dmp_state = IMU_DMP_ON;
    return 0;
}
コード例 #15
0
ファイル: CtlFourAxis.c プロジェクト: zcatao/FourAxis
/**********************************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);
}
コード例 #16
0
ファイル: mpu.c プロジェクト: wo4fisher/Monotroch
/**
****************************************************************************************
* @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");
    }
}
コード例 #17
0
ファイル: application.c プロジェクト: KONGLZ123/TPDT.FC_407
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");
}
コード例 #18
0
ファイル: inv_mpu_driver.c プロジェクト: navipack/navi-panel
/**
* @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
}
コード例 #19
0
ファイル: mpumanager.cpp プロジェクト: A-khodary/mpu6050Pi
void MPUManager::Stop()
{
	if( 0 != m_iThreadID )
	{
		m_bStop = true;
		pthread_join( m_iThreadID, 0 );
	}

	if( 0 != mpu_set_dmp_state(0) )
	{
		printf("mpu_set_dmp_state 0 failed\n");
	}

	if( 0 != __mpu_set_sleep_mode(1) )
	{
		printf("__mpu_set_sleep_mode 1 failed\n");
	}

	m_bRunning = false;
	m_bStop = false;
	m_iThreadID = 0;

	printf("DMP stopped\n");
}
コード例 #20
0
ファイル: mpulib.c プロジェクト: mickele77/MPULib
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;
}
コード例 #21
0
ファイル: mpu9150.cpp プロジェクト: trigrass2/geduino-ros
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;

}
コード例 #22
0
ファイル: mpu9150.c プロジェクト: brNX/purpins_firmware
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;
}
コード例 #23
0
ファイル: MPU9150Lib.cpp プロジェクト: 9team9/Quadu
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;
}
コード例 #24
0
ファイル: mpu9150.c プロジェクト: jpoirier/linux-mpu9150
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;
}
コード例 #25
0
int ms_open() {
	dmpReady=1;
	initialized = 0;
	for (int i=0;i<DIM;i++){
		lastval[i]=10;
	}

	// initialize device
	printf("Initializing MPU...\n");
	if (mpu_init(NULL) != 0) {
		printf("MPU init failed!\n");
		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(2000)!=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 enable DMP!\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_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) {
		printf("Failed to enable DMP features!\n");
		return -1;
	}


	printf("Setting DMP fifo rate...\n");
	if (dmp_set_fifo_rate(rate)!=0) {
		printf("Failed to set dmp fifo rate!\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(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("Done.\n");

	initialized = 1;
	return 0;
}
コード例 #26
0
ファイル: mpu6050.c プロジェクト: TechV/controller
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;
}
コード例 #27
0
ファイル: mpu6050.c プロジェクト: wzyy2/Embedded-Multi-OS-TOY
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;
}
コード例 #28
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;
}
コード例 #29
0
ファイル: mpu6050.c プロジェクト: tete1030/RM_Gimbal
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;
}
コード例 #30
0
ファイル: MPU.c プロジェクト: dammstanger/FCU_UCOSII150617
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;
}