unsigned short MPU9250_DMP::dmpGetFifoRate(void)
{
unsigned short rate;
if (dmp_get_fifo_rate(&rate) == INV_SUCCESS)
return rate;
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;
}
bool MPUManager::__StartDevice( bool p_bSelfTest )
{
if( 0 != mpu_init() )
{
printf("mpu_init failed\n");
return false;
}
#ifdef __DEBUG
__OutputDeviceStatus();
#endif
/* Wake up all sensors. */
if( 0 != mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
{
printf("mpu_set_sensors failed\n");
return false;
}
/* Push both gyro and accel data into the FIFO. */
if( 0 != mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
{
printf("mpu_configure_fifo failed\n");
return false;
}
if( 0 != mpu_set_sample_rate(m_usSampleRate) )
{
printf("mpu_set_sample_rate failed\n");
return false;
}
if( 0!= dmp_load_motion_driver_firmware() )
{
printf("dmp_load_motion_driver_firmware failed\n");
return false;
}
if( 0 != dmp_enable_feature(m_usDefaultFeatures) )
{
printf("dmp_enable_feature failed\n");
return false;
}
if( 0 != dmp_set_fifo_rate(m_usSampleRate) )
{
printf("dmp_set_fifo_rate failed\n");
return false;
}
if( 0 != mpu_get_gyro_sens( &m_fCurGyroSensitivity ) )
{
printf("mpu_get_gyro_sens failed\n");
return false;
}
else
{
printf("Current Gyro Sensitivity: %.2f\n", m_fCurGyroSensitivity);
}
if( 0 != mpu_get_accel_sens(&m_usCurAccelSensitivity) )
{
printf("mpu_get_accel_sens failed\n");
return false;
}
else
{
printf("Current Accel Sensitivity: %d\n", m_usCurAccelSensitivity);
}
m_usPackageLength = __dmp_get_packet_length();
unsigned short usfr = 0;
dmp_get_fifo_rate( &usfr );
printf("DMP start success\npackage length = %d\nFIFO rate = %d\n", m_usPackageLength, usfr);
if( p_bSelfTest )
{
if( __RunSelfTest() )
{
printf("Self test passed\n");
}
}
if( 0 != m_sGyroOffset[0] )
{
if( !__mpu_set_user_x_gyro_offset(m_sGyroOffset[0]) )
{
printf("set_user_x_gyro_offset %d failed\n", m_sGyroOffset[0]);
}
}
if( 0 != m_sGyroOffset[1] )
{
if( !__mpu_set_user_y_gyro_offset(m_sGyroOffset[1]) )
{
printf("set_user_y_gyro_offset %d failed\n", m_sGyroOffset[1]);
}
}
if( 0 != m_sGyroOffset[2] )
{
if( !__mpu_set_user_z_gyro_offset(m_sGyroOffset[2]) )
{
printf("set_user_z_gyro_offset %d failed\n", m_sGyroOffset[2]);
}
}
if( 0 != m_sAccelOffset[0] )
{
if( !__mpu_set_x_accel_offset(m_sAccelOffset[0]) )
{
printf("mpu_set_x_accel_offset %d failed\n", m_sAccelOffset[0]);
}
}
if( 0 != m_sAccelOffset[1] )
{
if( !__mpu_set_y_accel_offset(m_sAccelOffset[1]) )
{
printf("mpu_set_y_accel_offset %d failed\n", m_sAccelOffset[1]);
}
}
if( 0 != m_sAccelOffset[2] )
{
if( !__mpu_set_z_accel_offset(m_sAccelOffset[2]) )
{
printf("mpu_set_z_accel_offset %d failed\n", m_sAccelOffset[2]);
}
}
if( 0 != mpu_set_dmp_state(1) )
{
printf("mpu_set_dmp_state 1 failed\n");
return false;
}
if( 0 == mpu_set_lpf(42) )
{
printf("mpu_set_lpf failed\n");
return false;
}
#ifdef __DEBUG
__OutputDeviceStatus();
#endif
return true;
}
