unsigned short MPU9250_DMP::getGyroFSR(void)
{
unsigned short tmp;
if (mpu_get_gyro_fsr(&tmp) == INV_SUCCESS)
{
return tmp;
}
return 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;
}
