unsigned char MPU9250_DMP::getAccelFSR(void) { unsigned char tmp; if (mpu_get_accel_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; }