/*====================================================================================================*/ void QMV_IMU_Config( void ) { MPU_ConfigTypeDef MPU_ConfigStruct; MPU6500_Config(); delay_ms(10); printf("IMU Init ..."); MPU_ConfigStruct.MPU_Gyr_FullScale = MPU_GyrFS_2000dps; MPU_ConfigStruct.MPU_Gyr_LowPassFilter = MPU_GyrLPS_41Hz; MPU_ConfigStruct.MPU_Acc_FullScale = MPU_AccFS_4g; MPU_ConfigStruct.MPU_Acc_LowPassFilter = MPU_AccLPS_41Hz; if(MPU6500_Init(&MPU_ConfigStruct) != SUCCESS) { printf("ERROR\r\n"); while(1) { LED_R_Toggle(); delay_ms(100); } } printf("SUCCESS\r\n"); delay_ms(50); }
void app_mpu6500_main(void* parameter) { rt_uint32_t i; rt_int16_t acc_raw[3]; rt_int16_t gyro_raw[3]; accelerometer_raw_s acc; gyroscopes_raw_s gyro; mpu6500_running = true; rt_kprintf("mpu6500 start!\r\n"); while(run_6500) { if(!(MPU6500_Init() == RT_EOK)){ rt_kprintf("ERROR:MPU6500 selftest failed !\r\n"); }else{ rt_kprintf("MPU6500 init success.\n"); break; } rt_thread_delay(RT_TICK_PER_SECOND * 5); } acc.scale = MPU6500_GetFullScaleAccelGPL() * GRAVITY; gyro.scale = MPU6500_GetFullScaleGyroDPL() * DEGTORAD; /* main loop*/ while(run_6500){ MPU6500_GetMotion6(&acc_raw[0], &acc_raw[1], &acc_raw[2], &gyro_raw[0], &gyro_raw[1], &gyro_raw[2]); /* we can do rotation here if we need*/ /*rotate to NED coordinates*/ acc_raw[2] = (acc_raw[2] == -32768)?32767:-acc_raw[2]; acc_raw[0] = (acc_raw[0] == -32768)?32767:-acc_raw[0]; gyro_raw[0] = (gyro_raw[0] == -32768)?32767:-gyro_raw[0]; gyro_raw[2] = (gyro_raw[2] == -32768)?32767:-gyro_raw[2]; for(i = 0;i < 3;i++) acc.data[i] = acc_raw[i]; acc.timestamp = hrt_absolute_time(); acc.valid = true; for(i = 0;i < 3;i++) gyro.data[i] = gyro_raw[i]; gyro.timestamp = hrt_absolute_time(); gyro.valid = true; // orb_publish(ORB_ID(accelerometer_raw),&acc); // orb_publish(ORB_ID(gyroscopes_raw),&gyro); rt_thread_delay(RT_TICK_PER_SECOND/500); } mpu6500_running = false; rt_kprintf("MPU6500 stopped!\r\n"); }