|
|
int mpu_run_self_test(long *gyro, long *accel)
{
#ifdef MPU6050
const unsigned char tries = 2;
long gyro_st[3], accel_st[3];
unsigned char accel_result, gyro_result;
#ifdef AK89xx_SECONDARY
unsigned char compass_result;
#endif
int ii;
#endif
int result;
unsigned char accel_fsr, fifo_sensors, sensors_on;
unsigned short gyro_fsr, sample_rate, lpf;
unsigned char dmp_was_on;
if (st.chip_cfg.dmp_on) {
mpu_set_dmp_state(0);
dmp_was_on = 1;
} else
dmp_was_on = 0;
/* Get initial settings. */
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
mpu_get_lpf(&lpf);
mpu_get_sample_rate(&sample_rate);
sensors_on = st.chip_cfg.sensors;
mpu_get_fifo_config(&fifo_sensors);
/* For older chips, the self-test will be different. */
#if defined MPU6050
for (ii = 0; ii < tries; ii++)
if (!get_st_biases(gyro, accel, 0))
break;
if (ii == tries) {
/* If we reach this point, we most likely encountered an I2C error.
* We'll just report an error for all three sensors.
*/
result = 0;
goto restore;
}
for (ii = 0; ii < tries; ii++)
if (!get_st_biases(gyro_st, accel_st, 1))
break;
if (ii == tries) {
/* Again, probably an I2C error. */
result = 0;
goto restore;
}
accel_result = accel_self_test(accel, accel_st);
gyro_result = gyro_self_test(gyro, gyro_st);
result = 0;
if (!gyro_result)
result |= 0x01;
if (!accel_result)
result |= 0x02;
#ifdef AK89xx_SECONDARY
compass_result = compass_self_test();
if (!compass_result)
result |= 0x04;
#endif
restore:
#elif defined MPU6500
/* For now, this function will return a "pass" result for all three sensors
* for compatibility with current test applications.
*/
get_st_biases(gyro, accel, 0);
result = 0x7;
#endif
/* Set to invalid values to ensure no I2C writes are skipped. */
st.chip_cfg.gyro_fsr = 0xFF;
st.chip_cfg.accel_fsr = 0xFF;
st.chip_cfg.lpf = 0xFF;
st.chip_cfg.sample_rate = 0xFFFF;
st.chip_cfg.sensors = 0xFF;
st.chip_cfg.fifo_enable = 0xFF;
st.chip_cfg.clk_src = INV_CLK_PLL;
mpu_set_gyro_fsr(gyro_fsr);
mpu_set_accel_fsr(accel_fsr);
mpu_set_lpf(lpf);
mpu_set_sample_rate(sample_rate);
mpu_set_sensors(sensors_on);
mpu_configure_fifo(fifo_sensors);
if (dmp_was_on)
mpu_set_dmp_state(1);
return result;
}
上面这个函数是自检函数在 accel_result = accel_self_test(accel, accel_st);
gyro_result = gyro_self_test(gyro, gyro_st);
result = 0;
if (!gyro_result)
result |= 0x01;
if (!accel_result)
result |= 0x02;这里accel_result 和gyro_result 都是0x07所以自检不过，有遇到过类似情况的吗，怎么解决？ |
阿莫论坛20周年了！感谢大家的支持与爱护！！
一只鸟敢站在脆弱的枝条上歇脚，它依仗的不是枝条不会断，而是自己有翅膀，会飞。
|
发表于 2014-9-17 11:52:40
楼主
