Ejemplo n.º 1
0
byte MPU9250_DMP::begin(bool fusion, int rate)
{
	i2c_port_t i2c_master_port = I2C_NUM_0;
	i2c_config_t conf;
	conf.mode = I2C_MODE_MASTER;
	conf.sda_io_num = (gpio_num_t)21;
	conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
	conf.scl_io_num = (gpio_num_t)22;
	conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
	conf.master.clk_speed = 100000;
	if (i2c_param_config(i2c_master_port, &conf) != ESP_OK || i2c_driver_install(i2c_master_port, conf.mode, 0, 0, 0) != ESP_OK)
		return 0;

    struct int_param_s int_param;
	inv_error_t result = mpu_init(&int_param);
	if (result)
		return 0;

	mpu_set_bypass(1); // Place all slaves (including compass) on primary bus

	setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS | INV_XYZ_COMPASS);

	_gSense = getGyroSens();
	_aSense = getAccelSens();

	_features = DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_CAL_GYRO;
	if (fusion) _features |= DMP_FEATURE_6X_LP_QUAT;
	// Set DMP FIFO rate to 10 Hz
	if (dmpBegin(_features, rate) == INV_ERROR)
		return 0;

	return 1;
}
Ejemplo n.º 2
0
/******************************************************************************
 * 函数名称: 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);
}
Ejemplo n.º 3
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;
}
Ejemplo n.º 4
0
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");
	}
}
Ejemplo n.º 5
0
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;
}
Ejemplo n.º 6
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);
}
Ejemplo n.º 7
0
Archivo: main.c Proyecto: qartis/dori
int main(void)
{
    uint8_t rc;

    node_init();

    sei();

    i2c_init(I2C_FREQ(400000));

    rc = hmc_init();
    if (rc) {
//        puts_P(PSTR("hmc init"));
    }

    rc = mpu_init();
    if (rc) {
//        puts_P(PSTR("mpu init"));
    }

    rc = nunchuck_init();
    if (rc) {
//        puts_P(PSTR("nunch init"));
    }

    hmc_set_continuous();

    node_main();
}
Ejemplo n.º 8
0
 /**
 * @brief  mpu6050_init
 * @param  void
 * @retval 成功为0
 * @note 初始化mpu6050    频率为100*1000HZ
 */
int16_t mpu6050_init(void)
{
    
    I2C_QuickInit(I2C0_SCL_PB02_SDA_PB03,100*1000);
    
    
    int result=0;
    
    result=mpu_init();
	if(!result)
	{	 		 
	
		PrintChar("mpu initialization complete......\n ");		//mpu initialization complete	 	  

		if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))		//mpu_set_sensor
			PrintChar("mpu_set_sensor complete ......\n");
		else
			PrintChar("mpu_set_sensor come across error ......\n");

		if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))	//mpu_configure_fifo
			PrintChar("mpu_configure_fifo complete ......\n");
		else
			PrintChar("mpu_configure_fifo come across error ......\n");

		if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))	   	  		//mpu_set_sample_rate
		 PrintChar("mpu_set_sample_rate complete ......\n");
		else
		 	PrintChar("mpu_set_sample_rate error ......\n");

		if(!dmp_load_motion_driver_firmware())   	  			//dmp_load_motion_driver_firmvare
			PrintChar("dmp_load_motion_driver_firmware complete ......\n");
		else
			PrintChar("dmp_load_motion_driver_firmware come across error ......\n");

		if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) 	  //dmp_set_orientation
		 	PrintChar("dmp_set_orientation complete ......\n");
		else
		 	PrintChar("dmp_set_orientation come across error ......\n");

		if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
		    DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
		    DMP_FEATURE_GYRO_CAL))		   	 					 //dmp_enable_feature
		 	PrintChar("dmp_enable_feature complete ......\n");
		else
		 	PrintChar("dmp_enable_feature come across error ......\n");

		if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))   	 			 //dmp_set_fifo_rate
		 	PrintChar("dmp_set_fifo_rate complete ......\n");
		else
		 	PrintChar("dmp_set_fifo_rate come across error ......\n");

		run_self_test();		//自检

		if(!mpu_set_dmp_state(1))
		 	PrintChar("mpu_set_dmp_state complete ......\n");
		else
		 	PrintChar("mpu_set_dmp_state come across error ......\n");
	}
    return 0;
}
Ejemplo n.º 9
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");
  }
}
Ejemplo n.º 10
0
/**
 * This function will initial LPC17xx board.
 */
void rt_hw_board_init()
{
    /* NVIC Configuration */
#define NVIC_VTOR_MASK              0x3FFFFF80
#ifdef  VECT_TAB_RAM
    /* Set the Vector Table base location at 0x10000000 */
    SCB->VTOR  = (0x10000000 & NVIC_VTOR_MASK);
#else  /* VECT_TAB_FLASH  */
    /* Set the Vector Table base location at 0x00000000 */
    SCB->VTOR  = (0x00000000 & NVIC_VTOR_MASK);
#endif

    /* init systick */
    SysTick_Config(SystemCoreClock / RT_TICK_PER_SECOND - 1);
    /* set pend exception priority */
    NVIC_SetPriority(PendSV_IRQn, (1 << __NVIC_PRIO_BITS) - 1);

    /*init uart device*/
    rt_hw_uart_init();
    rt_console_set_device(RT_CONSOLE_DEVICE_NAME);

#if LPC_EXT_SDRAM == 1
    lpc_sdram_hw_init();
    mpu_init();
#endif

#ifdef RT_USING_COMPONENTS_INIT
    /* initialization board with RT-Thread Components */
    rt_components_board_init();
#endif
}
Ejemplo n.º 11
0
void MPU9250Device::init() {
  mpu_init(&this->revision);
  mpu_set_compass_sample_rate(100); // defaults to 100 in the libs

  /* Get/set hardware configuration. Start gyro. Wake up all sensors. */
  mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
  //  mpu_set_gyro_fsr (2000);//250
  //  mpu_set_accel_fsr(2);//4

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

  dmp_load_motion_driver_firmware();
  dmp_set_orientation(GYRO_ORIENTATION);

  //dmp_register_tap_cb(&tap_cb);

  unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL |
                                DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;

  //dmp_features = dmp_features |  DMP_FEATURE_TAP ;

  dmp_enable_feature(dmp_features);
  dmp_set_fifo_rate(DEFAULT_MPU_HZ);
}
Ejemplo n.º 12
0
/**
****************************************************************************************
* @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");
    }
}
//// 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;
}
Ejemplo n.º 14
0
//  connect to the serial port
void SerialInterface::initializePort(char* portname) {
#ifndef _WIN32
    _serialDescriptor = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
    
    qDebug("Opening SerialUSB %s: ", portname);
    
    if (_serialDescriptor == -1) {
        qDebug("Failed.\n");
        return;
    }
    
    struct termios options;
    tcgetattr(_serialDescriptor, &options);
        
    options.c_cflag |= (CLOCAL | CREAD | CS8);
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    tcsetattr(_serialDescriptor, TCSANOW, &options);
    
    cfsetispeed(&options,B115200);
    cfsetospeed(&options,B115200);
    
    if (USING_INVENSENSE_MPU9150) {
        // block on invensense reads until there is data to read
        int currentFlags = fcntl(_serialDescriptor, F_GETFL);
        fcntl(_serialDescriptor, F_SETFL, currentFlags & ~O_NONBLOCK);
        
        // make sure there's nothing queued up to be read
        tcflush(_serialDescriptor, TCIOFLUSH);
        
        // this disables streaming so there's no garbage data on reads
        if (write(_serialDescriptor, "SD\n", 3) != 3) {
            qDebug("Failed.\n");
            return;
        }
        char result[4];
        if (read(_serialDescriptor, result, 4) != 4) {
            qDebug("Failed.\n");
            return;
        }
        
        tty_set_file_descriptor(_serialDescriptor);
        mpu_init(0);
        mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
    }
    
    qDebug("Connected.\n");
    resetSerial();
    
    _active = true;
 #endif
}
Ejemplo n.º 15
0
/*
 * @brief MPU default configuration
 *
 * This function provides the default configuration mechanism for the Memory
 * Protection Unit (MPU).
 */
static int arm_mpu_init(struct device *arg)
{
	u32_t r_index;

	if (mpu_config.num_regions > get_num_regions()) {
		/* Attempt to configure more MPU regions than
		 * what is supported by hardware. As this operation
		 * is executed during system (pre-kernel) initialization,
		 * we want to ensure we can detect an attempt to
		 * perform invalid configuration.
		 */
		__ASSERT(0,
			"Request to configure: %u regions (supported: %u)\n",
			mpu_config.num_regions,
			get_num_regions()
		);
		return -1;
	}

	LOG_DBG("total region count: %d", get_num_regions());

	arm_core_mpu_disable();

	/* Architecture-specific configuration */
	mpu_init();

	/* Program fixed regions configured at SOC definition. */
	for (r_index = 0U; r_index < mpu_config.num_regions; r_index++) {
		region_init(r_index, &mpu_config.mpu_regions[r_index]);
	}

	/* Update the number of programmed MPU regions. */
	static_regions_num = mpu_config.num_regions;


	arm_core_mpu_enable();

	/* Sanity check for number of regions in Cortex-M0+, M3, and M4. */
#if defined(CONFIG_CPU_CORTEX_M0PLUS) || \
	defined(CONFIG_CPU_CORTEX_M3) || \
	defined(CONFIG_CPU_CORTEX_M4)
	__ASSERT(
		(MPU->TYPE & MPU_TYPE_DREGION_Msk) >> MPU_TYPE_DREGION_Pos == 8,
		"Invalid number of MPU regions\n");
#elif defined(DT_NUM_MPU_REGIONS)
	__ASSERT(
		(MPU->TYPE & MPU_TYPE_DREGION_Msk) >> MPU_TYPE_DREGION_Pos ==
		DT_NUM_MPU_REGIONS,
		"Invalid number of MPU regions\n");
#endif /* CORTEX_M0PLUS || CPU_CORTEX_M3 || CPU_CORTEX_M4 */
	return 0;
}
Ejemplo n.º 16
0
void hw_init()
{
	uint8_t c;
	
	P0DIR = 0xf0;		// P0.0 P0.1 P0.2 are the LEDs and they are outputs
                        // P0.3 is the UART TX - output
						// P0.5 is the push button - input
						// P0.6 is the MPU interrupt pin - input

	P0CON = 0x55;		// turn on the pullup for the recenter button
	
	// cycle the LEDs
	LED_RED		= 0;
	LED_YELLOW	= 0;
	LED_GREEN	= 0;
	
	for (c = 0; c < 3; ++c)
	{
		LED_RED		= 1;
		delay_ms(40);
		LED_RED		= 0;
		LED_YELLOW	= 1;
		delay_ms(40);
		LED_YELLOW	= 0;
		LED_GREEN	= 1;
		delay_ms(40);
		LED_GREEN	= 0;
		LED_YELLOW	= 1;
		delay_ms(40);
		LED_YELLOW	= 0;
	}
	
	dbgInit();
	i2c_init();

	dputs("init started");
	
	LED_YELLOW = 1;
	
	mpu_init(false);
	
	dbgFlush();
	
	rf_head_init();		// init the radio
	
	init_sleep();		// we need to wake up from RFIRQ

	LED_YELLOW = 0;

	dputs("init OK");
}
Ejemplo n.º 17
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);
}
Ejemplo n.º 18
0
/**
 * @brief Function for main application entry.
 */
int main(void) {
    uart_config();
    printf("\033[2J\033[;H MPU9150 example. Compiled @ %s\r\n", __TIME__);
    
	twi_init();
    mpu_init();
    
    accel_values_t acc_values;
    uint32_t sample_number = 0;
    while(1) {
        
        mpu9150_read_accel(&acc_values);										// Read accelerometer sensor values
        
		// Clear terminal and print values
        printf("\033[2J\033[;HSample # %d\r\nX: %06d\r\nY: %06d\r\nZ: %06d", ++sample_number, acc_values.x, acc_values.y, acc_values.z);
        nrf_delay_ms(250);
    }
}
Ejemplo n.º 19
0
/**
 * @brief Function for main application entry.
 */
int main(void)
{    
    uint32_t err_code;
    
    // Initialize.
    log_init();
	NRF_LOG_INFO("\033[2J\033[;H"); // Clear screen
    
    mpu_init();
    
    // Start execution.
    NRF_LOG_INFO("MPU Magnetometer example.");
    
    accel_values_t acc_values;
	magn_values_t magn_values;
    uint32_t sample_number = 0;
    
    const uint8_t MAG_DATA_SIZE = 10;
    uint8_t magn_data[MAG_DATA_SIZE];
    memset(magn_data, 0, MAG_DATA_SIZE);
    
    while(1)
    {
        if(NRF_LOG_PROCESS() == false)
        {
            // Read accelerometer sensor values
            err_code = app_mpu_read_accel(&acc_values);
            APP_ERROR_CHECK(err_code);
            // Clear terminal and print values
            NRF_LOG_INFO("\033[3;1HAccel Sample # %d\r\nX: %06d\r\nY: %06d\r\nZ: %06d\r\n", ++sample_number, acc_values.x, acc_values.y, acc_values.z);
            
            // Read and print magnetometer sensor values
            err_code = app_mpu_read_magnetometer(&magn_values, NULL);
            APP_ERROR_CHECK(err_code);
            NRF_LOG_INFO("\n\rMagno Sample # %d\r\nX: %06d\r\nY: %06d\r\nZ: %06d\r\n", sample_number, magn_values.x, magn_values.y, magn_values.z);
     
            nrf_delay_ms(200);
        }
    }
}
Ejemplo n.º 20
0
int main() {
    board_init();
    uart_init();
    esc_init();
    rgbled_init();
    rgbled_set(0xFF8000, 100);
    spektrum_init();
    rgbled_set(0xFF8000, 100);
    xbee_init();
    i2c_init();
    alt_init();
    mag_init();
    mpu_init();
    sonar_init();
    ins_init();
    inscomp_init();
    altitude_init();
    controller_init();
    basestation_init();
    flight_init();

    controlpanel_run();
}
Ejemplo n.º 21
0
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");
}
Ejemplo n.º 22
0
/**
* @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
}
Ejemplo n.º 23
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;
}
Ejemplo n.º 24
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;

}
Ejemplo n.º 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;
}
Ejemplo n.º 26
0
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
{
	signed char gyro_orientation[9] = { 1, 0, 0,
                                        0, 1, 0,
                                        0, 0, 1 };


	if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
		printf("Invalid sample rate %d\n", sample_rate);
		return -1;
	}

	if (mix_factor < 0 || mix_factor > 100) {
		printf("Invalid mag mixing factor %d\n", mix_factor);
		return -1;
	}

	yaw_mixing_factor = mix_factor;

	linux_set_i2c_bus(i2c_bus);

	printf("\nInitializing IMU .");


	if (mpu_init(NULL)) {
		printf("\nmpu_init() failed\n");
		return -1;
	}

	printf(".");


#if defined AK8963_SECONDARY || defined AK8975_SECONDARY


	if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
		printf("\nmpu_set_sensors() failed\n");
		return -1;
	}
#else
	if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
		printf("\nmpu_set_sensors() failed\n");
		return -1;
	}
#endif
	printf(".");


	if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
		printf("\nmpu_configure_fifo() failed\n");
		return -1;
	}

	printf(".");
	

	if (mpu_set_sample_rate(200)) {
		printf("\nmpu_set_sample_rate() failed\n");
		return -1;
	}

	printf(".");


#if defined AK8963_SECONDARY || defined AK8975_SECONDARY

	if (mpu_set_compass_sample_rate(50)) {
		printf("\nmpu_set_compass_sample_rate() failed\n");
		return -1;
	}
#endif

	printf(".");


	if (dmp_load_motion_driver_firmware()) {
		printf("\ndmp_load_motion_driver_firmware() failed\n");
		return -1;
	}

	printf(".");


	if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
		printf("\ndmp_set_orientation() failed\n");
		return -1;
	}

	printf(".");


  	if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL 
						| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
		printf("\ndmp_enable_feature() failed\n");
		return -1;
	}

	printf(".");

 
	if (dmp_set_fifo_rate(sample_rate)) {
		printf("\ndmp_set_fifo_rate() failed\n");
		return -1;
	}
	printf(".");


	if (mpu_set_dmp_state(1)) {
		printf("\nmpu_set_dmp_state(1) failed\n");
		return -1;
	}

	printf(" done\n\n");

	return 0;
}
Ejemplo n.º 27
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;
}
Ejemplo n.º 28
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;
}
Ejemplo n.º 29
0
void prf_init_func(void)
{
    #if (BLE_ACCEL)
    accel_init();
    #endif // (BLE_ACCEL)

    #if (BLE_HT_THERMOM)
    htpt_init();
    #endif // (BLE_HT_THERMOM)

    #if (BLE_HT_COLLECTOR)
    htpc_init();
    #endif // (BLE_HT_COLLECTOR)

    #if (BLE_DIS_SERVER)
    diss_init();
    #endif // (BLE_DIS_SERVER)

    #if (BLE_DIS_CLIENT)
    disc_init();
    #endif // (BLE_DIS_CLIENT)

    #if (BLE_BP_SENSOR)
    blps_init();
    #endif // (BLE_BP_SENSOR)

    #if (BLE_BP_COLLECTOR)
    blpc_init();
    #endif // (BLE_BP_COLLECTOR)

    #if (BLE_TIP_SERVER)
    tips_init();
    #endif // (BLE_TIP_SERVER)

    #if (BLE_TIP_CLIENT)
    tipc_init();
    #endif // (BLE_TIP_CLIENT)

    #if (BLE_HR_SENSOR)
    hrps_init();
    #endif // (BLE_HR_SENSOR)

    #if (BLE_HR_COLLECTOR)
    hrpc_init();
    #endif // (BLE_HR_COLLECTOR)

    #if (BLE_FINDME_LOCATOR)
    findl_init();
    #endif // (BLE_FINDME_LOCATOR)

    #if (BLE_FINDME_TARGET)
    findt_init();
    #endif // (BLE_FINDME_TARGET)

    #if (BLE_PROX_MONITOR)
    proxm_init();
    #endif // (BLE_PROX_MONITOR)

    #if (BLE_PROX_REPORTER)
    proxr_init();
    #endif // (BLE_PROX_REPORTER)
    #if (BLE_STREAMDATA_HOST)
    streamdatah_init();
    #endif // (BLE_STREAMDATA_HOST)
    #if (BLE_STREAMDATA_DEVICE)
    streamdatad_init();
    #endif // (BLE_STREAMDATA_DEVICE)

    #if (BLE_SPOTA_RECEIVER)
    spotar_init();
    #endif // (BLE_SPOTA_RECEIVER)
    
    #if (BLE_SPS_CLIENT)
    sps_client_init();
    #endif // (BLE_SPS_CLIENT)
    #if (BLE_SPS_SERVER)
    sps_server_init();
    #endif // (BLE_SPS_SERVER)
    
    #if (BLE_SP_SERVER)
    scpps_init();
    #endif // (BLE_SP_SERVER)

    #if (BLE_SP_CLIENT)
    scppc_init();
    #endif // (BLE_SP_CLIENT)

    #if (BLE_BATT_SERVER)
    bass_init();
    #endif // (BLE_BATT_SERVER)

    #if (BLE_BATT_CLIENT)
    basc_init();
    #endif // (BLE_BATT_CLIENT)

    #if (BLE_HID_DEVICE)
    hogpd_init();
    #endif // (BLE_HID_DEVICE)

    #if (BLE_HID_BOOT_HOST)
    hogpbh_init();
    #endif // (BLE_HID_BOOT_HOST)

    #if (BLE_HID_REPORT_HOST)
    hogprh_init();
    #endif // (BLE_HID_REPORT_HOST)

    #if (BLE_GL_COLLECTOR)
    glpc_init();
    #endif // (BLE_GL_COLLECTOR)

    #if (BLE_GL_SENSOR)
    glps_init();
    #endif // (BLE_GL_SENSOR)

    #if (BLE_NEB_CLIENT)
    nbpc_init();
    #endif // (BLE_NEB_CLIENT)

    #if (BLE_NEB_SERVER)
    nbps_init();
    #endif // (BLE_NEB_SERVER)

    #if (BLE_RSC_COLLECTOR)
    rscpc_init();
    #endif // (BLE_RSC_COLLECTOR)

    #if (BLE_RSC_SENSOR)
    rscps_init();
    #endif // (BLE_RSC_SENSOR)

    #if (BLE_CSC_COLLECTOR)
    cscpc_init();
    #endif // (BLE_CSC_COLLECTOR)

    #if (BLE_CSC_SENSOR)
    cscps_init();
    #endif // (BLE_CSC_SENSOR)

	#if (BLE_CP_COLLECTOR)
	cppc_init();
	#endif // (BLE_CP_COLLECTOR)

	#if (BLE_CP_SENSOR)
	cpps_init();
	#endif // (BLE_CP_SENSOR)

    #if (BLE_LN_COLLECTOR)
    lanc_init();
    #endif // (BLE_LN_COLLECTOR)

    #if (BLE_LN_SENSOR)
    lans_init();
    #endif // (BLE_LN_SENSOR)

    #if (BLE_AN_CLIENT)
    anpc_init();
    #endif // (BLE_AN_CLIENT)

    #if (BLE_AN_SERVER)
    anps_init();
    #endif // (BLE_AN_SERVER)

    #if (BLE_ANC_CLIENT)
    ancc_init();
    #endif // (BLE_ANC_CLIENT)
    
    #if (BLE_PAS_CLIENT)
    paspc_init();
    #endif // (BLE_PAS_CLIENT)

    #if (BLE_PAS_SERVER)
    pasps_init();
    #endif // (BLE_PAS_SERVER)
    
    #if (BLE_SAMPLE128)
    sample128_init();
    #endif // (BLE_SAMPLE128)
    
	#if (BLE_WPT_CLIENT)
    wptc_init();
    #endif // WPT_CLIENT
    
    #if (BLE_WPTS)
    wpts_init();
    #endif // BLE_WPTS
	
    #if (BLE_IEU)  
    ieu_init();
    #endif // (BLE_IEU)

    #if (BLE_MPU)   
    mpu_init();
    #endif // (BLE_MPU)
	
    #if (BLE_UDS_SERVER)
    udss_init();
    #endif // (BLE_UDS_SERVER)

    #if (BLE_WSS_SERVER)
    wsss_init();
    #endif // (BLE_WSS_SERVER)

}
Ejemplo n.º 30
0
int ms_open(unsigned int rate) {
	dmpReady=1;
	initialized = 0;

	// initialize device
	printf("Initializing MPU...\n");
	r=mpu_init(NULL); 
	if (r != 0) {
		printf("MPU init failed! %i\n",r);
		return -1;
	}
	printf("Setting MPU sensors...\n");
	if (mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
		printf("Failed to set sensors!\n");
		return -1;
	}
	printf("Setting GYRO sensitivity...\n");
	if (mpu_set_gyro_fsr(FSR)!=0) {
		printf("Failed to set gyro sensitivity!\n");
		return -1;
	}
	printf("Setting ACCEL sensitivity...\n");
	if (mpu_set_accel_fsr(2)!=0) {
		printf("Failed to set accel sensitivity!\n");
		return -1;
	}
	// verify connection
	printf("Powering up MPU...\n");
	mpu_get_power_state(&devStatus);
	printf(devStatus ? "MPU6050 connection successful\n" : "MPU6050 connection failed %u\n",devStatus);

	//fifo config
	printf("Setting MPU fifo...\n");
	if (mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
		printf("Failed to initialize MPU fifo!\n");
		return -1;
	}

	// load and configure the DMP
	printf("Loading DMP firmware...\n");
	if (dmp_load_motion_driver_firmware()!=0) {
		printf("Failed to load DMP firmware!\n");
		return -1;
	}

	printf("Setting GYRO orientation...\n");
	if (dmp_set_orientation( inv_orientation_matrix_to_scalar( gyro_orientation ) )!=0) {
		printf("Failed to set gyro orientation!\n");
		return -1;
	}

	printf("Setting DMP fifo rate to: %i\n",rate);
	if (dmp_set_fifo_rate(rate)!=0) {
		printf("Failed to set dmp fifo rate!\n");
		return -1;
	}

	printf("Activating DMP...\n");
	if (mpu_set_dmp_state(1)!=0) {
		printf("Failed to enable DMP!\n");
		return -1;
	}

	//dmp_set_orientation()
	//if (dmp_enable_feature(DMP_FEATURE_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
	printf("Configuring DMP...\n");
	if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) {
	//if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
		printf("Failed to enable DMP features!\n");
		return -1;
	}

	printf("Resetting fifo queue...\n");
	if (mpu_reset_fifo()!=0) {
		printf("Failed to reset fifo!\n");
		return -1;
	}

	printf("Checking... ");
	do {
		delay_ms(5*1000/rate);  //dmp will habve 4 (5-1) packets based on the fifo_rate
		r=dmp_read_fifo(g,a,_q,&sensors,&fifoCount);
	} while (r!=0 || fifoCount<5); //packtets!!!
	printf("Collected: %i packets.\n",fifoCount);

	ms_update();
	initialized = 1;

	uint16_t lpf,g_fsr,a_sens,dmp_rate;
	uint8_t dmp,a_fsr;
	float g_sens;
	printf("MPU config:\n");
	mpu_get_lpf(&lpf);
	printf("MPU LPF: %u\n",lpf);
	dmp_get_fifo_rate(&dmp_rate);
	printf("DMP Fifo Rate: %u\n",dmp_rate);
	mpu_get_dmp_state(&dmp);
	printf("MPU DMP State: %u\n",dmp);
	mpu_get_gyro_fsr(&g_fsr);
	printf("MPU gyro FSR: %u\n",g_fsr);
	mpu_get_gyro_sens(&g_sens);
	printf("MPU gyro sens: %2.3f\n",g_sens);
	mpu_get_accel_fsr(&a_fsr);
	printf("MPU accel FSR: %u\n",a_fsr);
	mpu_get_accel_sens(&a_sens);
	printf("MPU accel sens: %u\n",a_sens);
	
	return 0;
}