static void run_self_test(void) { int result; // char test_packet[4] = {0}; long gyro[3], accel[3]; result = mpu_run_self_test(gyro, accel); if(result == 0) printf("self-test is executed succesfully ......\n"); //added by Damm Stanger if (result == 0x7) // if(result == 0x03) //Curently we don't have compass sensor so we don't need to test it Damm Stanger { /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); printf("setting bias succesfully ......\n"); } else { printf("bias has not been modified ......\n"); } }
int mpulib_run_self_test(void) { long gyro[3], accel[3]; if( mpu_run_self_test(gyro, accel) ) { /* Test passed: we can trust the gyro data here, * so let's push it down to the DMP. */ float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); printf("\nsetting bias succesfully ......\n"); return 0; } else { printf("\nbias has not been modified ......\n"); return -1; } }
static void run_self_test(void) { int result; long gyro[3], accel[3]; result = mpu_run_self_test(gyro, accel); if (result == 0x7) { /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); printf("setting bias succesfully ......\r\n"); } }
static inline void run_self_test(void) { int result; long gyro[3], accel[3]; result = mpu_run_self_test(gyro, accel); if (result == 0x7) { /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); } /* Report results. */ //#报告结果 改为N5110 //test_packet[0] = 't'; //test_packet[1] = result; // N5110_Set_XY(0,0);//# // N5110_Write_Char('T'); // N5110_Write_Char(':'); // N5110_Write_Char(result+48); //send_packet(PACKET_TYPE_MISC, test_packet); }
void mpu6050_run_self_test(void) { int rtn; long gyro[3], accel[3]; float sens; unsigned short accel_sens; rtn = mpu_run_self_test(gyro, accel); if (rtn == 0x3) { /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); printf("\r\nSelf Test Passed!\r\n"); } }
unsigned short MPU9250_DMP::getAccelSens(void) { unsigned short sens; if (mpu_get_accel_sens(&sens) == INV_SUCCESS) { return sens; } return 0; }
void run_self_test(void) { extern bool Update_10Hz; int result; // char test_packet[4] = {0}; long gyro[3], accel[3]; result = mpu_run_self_test(gyro, accel); if (result == 0x7) { /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); } else { //PrintChar("bias has not been modified ......\n"); while (1) { if(Update_10Hz) { Update_10Hz = false; GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_5, ~GPIOPinRead(GPIO_PORTE_BASE, GPIO_PIN_5) ); } } } }
bool MPUManager::__RunSelfTest() { int result; long gyro[3], accel[3]; int iSuccess = 0x01|0x02; #ifdef AK89xx_SECONDARY iSuccess |= 0x04; #endif result = mpu_run_self_test(gyro, accel); if (result == iSuccess) { /* Test passed. We can trust the gyro data here, so let's push it down * to the DMP. */ float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); return true; } else { #ifdef AK89xx_SECONDARY printf("Self test failed: gyro %s; accel %s; comp %s\n", (result&0x01)?"pass":"******", (result&0x02)?"pass":"******", (result&0x04)?"pass":"******" ); #else printf("Self test failed: gyro %s; accel %s\n", (result&0x01)?"pass":"******", (result&0x02)?"pass":"******" ); #endif return false; } }
int INVMPU9150::init(int i2cBus, int frequency) { // Set linux i2c bus linux_set_i2c_bus(i2cBus); // The gyro orientation signed char gyro_orientation[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; // Init MPU if (mpu_init(NULL)) { return MPU9150_INIT_MPU_INIT_ERROR; } // Set sensors if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { return MPU9150_INIT_MPU_SET_SENSORS_ERROR; } // Get gyro sensitivity scale factor and convert from [LSB/(degree/s)] to [LSB/(rad/s)] float gyroSsfDegreePerSecond; if (mpu_get_gyro_sens(& gyroSsfDegreePerSecond)) { return MPU9150_INIT_MPU_GET_GYRO_FSR_ERROR; } gyroSsf = gyroSsfDegreePerSecond * 180 / M_PI; // Get accel sensitivity scale factor if (mpu_get_accel_sens(& accelSsf)) { return MPU9150_INIT_MPU_GET_ACCEL_FSR_ERROR; } // Configure FIFO if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { return MPU9150_INIT_MPU_CONFIGURE_FIFO_ERROR; } // Set sample rate for gyro/acc if (mpu_set_sample_rate(frequency)) { return MPU9150_INIT_MPU_SET_SAMPLE_RATE_ERROR; } // Load motion driver firmare (DMP) if (dmp_load_motion_driver_firmware()) { return MPU9150_INIT_DMP_LOAD_MOTION_DRIVER_FIRMWARE_ERROR; } // Set gyro orientation if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { return MPU9150_INIT_DMP_SET_ORIENTATION_ERROR; } // Enable DMP features if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) { return MPU9150_INIT_DMP_ENABLE_FEATURES_ERROR; } // Set sample rate for DMP if (dmp_set_fifo_rate(frequency)) { return MPU9150_INIT_DMP_SET_FIFO_RATE_ERROR; } // Enable DMP if (mpu_set_dmp_state(1)) { return MPU9150_INIT_DMP_SET_STATE_ERROR; } return MPU9150_OK; }
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; }