/*====================================================================================================*/
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");
}
