inv_error_t MPU9250_DMP::dmpSetTap(
unsigned short xThresh, unsigned short yThresh, unsigned short zThresh,
unsigned char taps, unsigned short tapTime, unsigned short tapMulti)
{
unsigned char axes = 0;
if (xThresh > 0)
{
axes |= TAP_X;
xThresh = constrain(xThresh, 1, 1600);
if (dmp_set_tap_thresh(1<<X_AXIS, xThresh) != INV_SUCCESS)
return INV_ERROR;
}
if (yThresh > 0)
{
axes |= TAP_Y;
yThresh = constrain(yThresh, 1, 1600);
if (dmp_set_tap_thresh(1<<Y_AXIS, yThresh) != INV_SUCCESS)
return INV_ERROR;
}
if (zThresh > 0)
{
axes |= TAP_Z;
zThresh = constrain(zThresh, 1, 1600);
if (dmp_set_tap_thresh(1<<Z_AXIS, zThresh) != INV_SUCCESS)
return INV_ERROR;
}
if (dmp_set_tap_axes(axes) != INV_SUCCESS)
return INV_ERROR;
if (dmp_set_tap_count(taps) != INV_SUCCESS)
return INV_ERROR;
if (dmp_set_tap_time(tapTime) != INV_SUCCESS)
return INV_ERROR;
if (dmp_set_tap_time_multi(tapMulti) != INV_SUCCESS)
return INV_ERROR;
dmp_register_tap_cb(tap_cb);
return INV_SUCCESS;
}
// Private
int8_t IMUTask::MPUSetup()
{
// TODO: should check here to see if IMU is already setup and just init the lib
int8_t result = 0;
struct int_param_s int_param;
int_param.cb = IMU_interrupt;
int_param.pin = MPU_INT;
int_param.arg = FALLING;
unsigned char accel_fsr;
unsigned short gyro_rate, gyro_fsr;
unsigned long timestamp;
result = mpu_init(&int_param);
debug::log("IMUTask: mpu_init returned " + String(result));
if (result != 0) {
return -1;
}
/* Wake up all sensors. */
result = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
debug::log("IMUTask: mpu_set_sensors returned " + String(result));
result = mpu_set_sample_rate(IMU_DEFAULT_MPU_HZ);
debug::log("IMUTask: mpu_set_sample_rate returned " + String(result));
debug::log("Pre laod");
result = dmp_load_motion_driver_firmware();
debug::log("IMUTask: dmp_load_motion_driver_firmware returned " + String(result));
if (result != 0) {
return -2;
}
result = dmp_set_orientation( inv_orientation_matrix_to_scalar(gyro_orientation) );
debug::log("IMUTask: dmp_set_orientation returned " + String(result));
if (result != 0) {
return -3;
}
/*
* Known Bug -
* DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate
* specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once
* a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz
* there will be a 25Hz interrupt from the MPU device.
*
* There is a known issue in which if you do not enable DMP_FEATURE_TAP
* then the interrupts will be at 200Hz even if fifo rate
* is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP
*/
unsigned short dmp_features = DMP_FEATURE_TAP | // Don't remove this, see above
DMP_FEATURE_ANDROID_ORIENT; // we use this for the screen
// Disableing the rest for now as we dont yet have a use for them
// DMP_FEATURE_SEND_RAW_ACCEL |
// DMP_FEATURE_SEND_RAW_GYRO;
result = dmp_enable_feature(dmp_features);
if (result != 0) {
debug::log("IMUTask: mp_enable_feature returned " + String(result));
return -4;
}
result = dmp_set_fifo_rate(IMU_DEFAULT_MPU_HZ);
if (result != 0) {
debug::log("IMUTask: dmp_set_fifo_rate returned " + String(result));
return -5;
}
dmp_register_tap_cb(IMU_tap_cb);
dmp_register_android_orient_cb(IMU_android_orient_cb);
dmp_set_interrupt_mode(DMP_INT_GESTURE);
return 0;
}