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; }
/****************************************************************************** * 函数名称: 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); }
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; }
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"); } }
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; }
/******************************************************************************* * 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); }
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(); }
/** * @brief mpu6050_init * @param void * @retval 成功为0 * @note 初始化mpu6050 频率为100*1000HZ */ int16_t mpu6050_init(void) { I2C_QuickInit(I2C0_SCL_PB02_SDA_PB03,100*1000); int result=0; result=mpu_init(); if(!result) { PrintChar("mpu initialization complete......\n "); //mpu initialization complete if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor PrintChar("mpu_set_sensor complete ......\n"); else PrintChar("mpu_set_sensor come across error ......\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo PrintChar("mpu_configure_fifo complete ......\n"); else PrintChar("mpu_configure_fifo come across error ......\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate PrintChar("mpu_set_sample_rate complete ......\n"); else PrintChar("mpu_set_sample_rate error ......\n"); if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare PrintChar("dmp_load_motion_driver_firmware complete ......\n"); else PrintChar("dmp_load_motion_driver_firmware come across error ......\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation PrintChar("dmp_set_orientation complete ......\n"); else PrintChar("dmp_set_orientation come across error ......\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature PrintChar("dmp_enable_feature complete ......\n"); else PrintChar("dmp_enable_feature come across error ......\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate PrintChar("dmp_set_fifo_rate complete ......\n"); else PrintChar("dmp_set_fifo_rate come across error ......\n"); run_self_test(); //自检 if(!mpu_set_dmp_state(1)) PrintChar("mpu_set_dmp_state complete ......\n"); else PrintChar("mpu_set_dmp_state come across error ......\n"); } return 0; }
/************************************************************************** 函数功能:MPU6050内置DMP的初始化 入口参数:无 返回 值:无 作 者:平衡小车之家 **************************************************************************/ void DMP_Init(void) { u8 temp[1]={0}; i2cRead(0x68,0x75,1,temp); printf("mpu_set_sensor complete ......\r\n"); if(temp[0]!=0x68)NVIC_SystemReset(); if(!mpu_init()) { if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) printf("mpu_set_sensor complete ......\r\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) printf("mpu_configure_fifo complete ......\r\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) printf("mpu_set_sample_rate complete ......\r\n"); if(!dmp_load_motion_driver_firmware()) printf("dmp_load_motion_driver_firmware complete ......\r\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) printf("dmp_set_orientation complete ......\r\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) printf("dmp_enable_feature complete ......\r\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) printf("dmp_set_fifo_rate complete ......\r\n"); run_self_test(); if(!mpu_set_dmp_state(1)) printf("mpu_set_dmp_state complete ......\r\n"); } }
/** * 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 }
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); }
/** **************************************************************************************** * @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, ¶ms); return 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 }
/* * @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; }
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"); }
/**********************************MPU6050*************************************/ void MPU6050_INIT() { mpu_init(); mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);//#(INV_XYZ_GYRO | INV_XYZ_ACCEL)在inv_mpu.h定义 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); mpu_set_sample_rate(DEFAULT_MPU_HZ);//#本文件开头定义 dmp_load_motion_driver_firmware(); dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//#本文定义 dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |DMP_FEATURE_GYRO_CAL;//#全部在(inv_mpu_dmp_motion_driver.h)中定义 dmp_enable_feature(dmp_features); dmp_set_fifo_rate(DEFAULT_MPU_HZ); run_self_test(); mpu_set_dmp_state(1); }
/** * @brief 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); } }
/** * @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); } } }
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(); }
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"); }
/** * @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 }
int mpulib_init(int i2c_bus, int sample_rate, int mix_factor) { signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) { printf("Invalid I2C bus %d\n", i2c_bus); return -1; } if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) { printf("Invalid sample rate %d\n", sample_rate); return -1; } if (mix_factor < 0 || mix_factor > 100) { printf("Invalid mag mixing factor %d\n", mix_factor); return -1; } yaw_mixing_factor = mix_factor; linux_set_i2c_bus(i2c_bus); printf("\nInitializing IMU ."); fflush(stdout); if (mpu_init(NULL)) { printf("\nmpu_init() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { printf("\nmpu_set_sensors() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("\nmpu_configure_fifo() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_set_sample_rate(sample_rate)) { printf("\nmpu_set_sample_rate() failed\n"); return -1; } printf("."); fflush(stdout); int compass_sample_rate = sample_rate; if( compass_sample_rate > MAX_COMPASS_SAMPLE_RATE ) { compass_sample_rate = MAX_COMPASS_SAMPLE_RATE; } if (mpu_set_compass_sample_rate(compass_sample_rate)) { printf("\nmpu_set_compass_sample_rate() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_load_motion_driver_firmware()) { printf("\ndmp_load_motion_driver_firmware() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { printf("\ndmp_set_orientation() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { printf("\ndmp_enable_feature() failed\n"); return -1; } printf("."); fflush(stdout); if (dmp_set_fifo_rate(sample_rate)) { printf("\ndmp_set_fifo_rate() failed\n"); return -1; } printf("."); fflush(stdout); if (mpu_set_dmp_state(1)) { printf("\nmpu_set_dmp_state(1) failed\n"); return -1; } printf(" done\n\n"); return 0; }
int INVMPU9150::init(int i2cBus, int frequency) { // Set linux i2c bus linux_set_i2c_bus(i2cBus); // The gyro orientation signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; // Init MPU if (mpu_init(NULL)) { return MPU9150_INIT_MPU_INIT_ERROR; } // Set sensors if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { return MPU9150_INIT_MPU_SET_SENSORS_ERROR; } // Get gyro sensitivity scale factor and convert from [LSB/(degree/s)] to [LSB/(rad/s)] float gyroSsfDegreePerSecond; if (mpu_get_gyro_sens(& gyroSsfDegreePerSecond)) { return MPU9150_INIT_MPU_GET_GYRO_FSR_ERROR; } gyroSsf = gyroSsfDegreePerSecond * 180 / M_PI; // Get accel sensitivity scale factor if (mpu_get_accel_sens(& accelSsf)) { return MPU9150_INIT_MPU_GET_ACCEL_FSR_ERROR; } // Configure FIFO if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { return MPU9150_INIT_MPU_CONFIGURE_FIFO_ERROR; } // Set sample rate for gyro/acc if (mpu_set_sample_rate(frequency)) { return MPU9150_INIT_MPU_SET_SAMPLE_RATE_ERROR; } // Load motion driver firmare (DMP) if (dmp_load_motion_driver_firmware()) { return MPU9150_INIT_DMP_LOAD_MOTION_DRIVER_FIRMWARE_ERROR; } // Set gyro orientation if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { return MPU9150_INIT_DMP_SET_ORIENTATION_ERROR; } // Enable DMP features if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { return MPU9150_INIT_DMP_ENABLE_FEATURES_ERROR; } // Set sample rate for DMP if (dmp_set_fifo_rate(frequency)) { return MPU9150_INIT_DMP_SET_FIFO_RATE_ERROR; } // Enable DMP if (mpu_set_dmp_state(1)) { return MPU9150_INIT_DMP_SET_STATE_ERROR; } return MPU9150_OK; }
int 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; }
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor) { signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) { printf("Invalid sample rate %d\n", sample_rate); return -1; } if (mix_factor < 0 || mix_factor > 100) { printf("Invalid mag mixing factor %d\n", mix_factor); return -1; } yaw_mixing_factor = mix_factor; linux_set_i2c_bus(i2c_bus); printf("\nInitializing IMU ."); if (mpu_init(NULL)) { printf("\nmpu_init() failed\n"); return -1; } printf("."); #if defined AK8963_SECONDARY || defined AK8975_SECONDARY if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) { printf("\nmpu_set_sensors() failed\n"); return -1; } #else if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("\nmpu_set_sensors() failed\n"); return -1; } #endif printf("."); if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("\nmpu_configure_fifo() failed\n"); return -1; } printf("."); if (mpu_set_sample_rate(200)) { printf("\nmpu_set_sample_rate() failed\n"); return -1; } printf("."); #if defined AK8963_SECONDARY || defined AK8975_SECONDARY if (mpu_set_compass_sample_rate(50)) { printf("\nmpu_set_compass_sample_rate() failed\n"); return -1; } #endif printf("."); if (dmp_load_motion_driver_firmware()) { printf("\ndmp_load_motion_driver_firmware() failed\n"); return -1; } printf("."); if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { printf("\ndmp_set_orientation() failed\n"); return -1; } printf("."); if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { printf("\ndmp_enable_feature() failed\n"); return -1; } printf("."); if (dmp_set_fifo_rate(sample_rate)) { printf("\ndmp_set_fifo_rate() failed\n"); return -1; } printf("."); if (mpu_set_dmp_state(1)) { printf("\nmpu_set_dmp_state(1) failed\n"); return -1; } printf(" done\n\n"); return 0; }
int mpu6050_init() { /* ** Configures I2C to Master mode to generate start ** condition on I2C bus and to transmit data at a ** speed of 100khz */ SetupI2C(); rt_hw_interrupt_install(SYS_INT_I2C0INT, mpu6050_isr, NULL, "mpu6050"); rt_hw_interrupt_mask(SYS_INT_I2C0INT); int result = mpu_init(); if(!result) { rt_kprintf("mpu initialization complete......\n "); //mpu_set_sensor if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) rt_kprintf("mpu_set_sensor complete ......\n"); else rt_kprintf("mpu_set_sensor come across error ......\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo rt_kprintf("mpu_configure_fifo complete ......\n"); else rt_kprintf("mpu_configure_fifo come across error ......\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate rt_kprintf("mpu_set_sample_rate complete ......\n"); else rt_kprintf("mpu_set_sample_rate error ......\n"); if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare rt_kprintf("dmp_load_motion_driver_firmware complete ......\n"); else rt_kprintf("dmp_load_motion_driver_firmware come across error ......\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation rt_kprintf("dmp_set_orientation complete ......\n"); else rt_kprintf("dmp_set_orientation come across error ......\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature rt_kprintf("dmp_enable_feature complete ......\n"); else rt_kprintf("dmp_enable_feature come across error ......\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate rt_kprintf("dmp_set_fifo_rate complete ......\n"); else rt_kprintf("dmp_set_fifo_rate come across error ......\n"); run_self_test(); if(!mpu_set_dmp_state(1)) rt_kprintf("mpu_set_dmp_state complete ......\n"); else rt_kprintf("mpu_set_dmp_state come across error ......\n"); } while(1) { dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); /* Gyro and accel data are written to the FIFO by the DMP in chip * frame and hardware units. This behavior is convenient because it * keeps the gyro and accel outputs of dmp_read_fifo and * mpu_read_fifo consistent. */ /* if (sensors & INV_XYZ_GYRO ) send_packet(PACKET_TYPE_GYRO, gyro); if (sensors & INV_XYZ_ACCEL) send_packet(PACKET_TYPE_ACCEL, accel); */ /* Unlike gyro and accel, quaternions are written to the FIFO in * the body frame, q30. The orientation is set by the scalar passed * to dmp_set_orientation during initialization. */ if (sensors & INV_WXYZ_QUAT ) { q0=quat[0] / q30; q1=quat[1] / q30; q2=quat[2] / q30; q3=quat[3] / q30; Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; // printf("Pitch=%.2f ,Roll=%.2f ,Yaw=%.2f \n",Pitch,Roll,Yaw); UART1_ReportIMU(Yaw*10, Pitch*10, Roll*10,0,0,0,100); delay_ms(10); } } return 0; }
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; }
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) }
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; }