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;
}
