DMP Feature Enable and Configuration for MPU-9250

/**

@brief Enable DMP features.
The following #define's are used in the input mask:
\n DMP_FEATURE_TAP: enable tap feature
\n DMP_FEATURE_ANDROID_ORIENT: enable Android orientation feature
\n DMP_FEATURE_LP_QUAT: enable 3-axis quaternion feature
\n DMP_FEATURE_6X_LP_QUAT: enable 6-axis quaternion feature
\n DMP_FEATURE_GYRO_CAL: enable gyro calibration feature
\n DMP_FEATURE_SEND_RAW_ACCEL: send raw accelerometer data to FIFO
\n DMP_FEATURE_SEND_RAW_GYRO: send raw gyro data to FIFO
\n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually exclusive.
\n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also mutually exclusive.
@param[in] mask Mask of features to enable.
@return 0 if successful. */ int dmp_enable_feature(unsigned short mask) { unsigned char tmp[10];

/* Set integration scale factor. */ tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF); tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF); tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF); tmp[3] = (unsigned char)(GYRO_SF & 0xFF); mpu_write_mem(D_0_104, 4, tmp); // Set the integration scale factor in memory D_0_104

/* Send sensor data to the FIFO. */ tmp[0] = 0xA3; if (mask & DMP_FEATURE_SEND_RAW_ACCEL) { tmp[1] = 0xC0; tmp[2] = 0xC8; tmp[3] = 0xC2; } else { tmp[1] = 0xA3; tmp[2] = 0xA3; tmp[3] = 0xA3; } if (mask & DMP_FEATURE_SEND_ANY_GYRO) { tmp[4] = 0xC4; tmp[5] = 0xCC; tmp[6] = 0xC6; } else { tmp[4] = 0xA3; tmp[5] = 0xA3; tmp[6] = 0xA3; } tmp[7] = 0xA3; tmp[8] = 0xA3; tmp[9] = 0xA3; mpu_write_mem(CFG_15,10,tmp); // Send sensor data configuration to memory CFG_15

/* Send gesture data to the FIFO. */ if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) tmp[0] = DINA20; else tmp[0] = 0xD8; mpu_write_mem(CFG_27,1,tmp); // Send gesture data configuration to memory CFG_27

// Enable or disable gyro calibration feature based on the input mask if (mask & DMP_FEATURE_GYRO_CAL) dmp_enable_gyro_cal(1); else dmp_enable_gyro_cal(0);

// Send raw or calibrated gyro data to FIFO based on the input mask if (mask & DMP_FEATURE_SEND_ANY_GYRO) { if (mask & DMP_FEATURE_SEND_CAL_GYRO) { tmp[0] = 0xB2; tmp[1] = 0x8B; tmp[2] = 0xB6; tmp[3] = 0x9B; } else { tmp[0] = DINAC0; tmp[1] = DINA80; tmp[2] = DINAC2; tmp[3] = DINA90; } mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); // Send gyro data configuration to memory CFG_GYRO_RAW_DATA }

// Configure tap feature based on the input mask if (mask & DMP_FEATURE_TAP) { /* Enable tap. */ tmp[0] = 0xF8; mpu_write_mem(CFG_20, 1, tmp); // Enable tap feature dmp_set_tap_thresh(TAP_XYZ, 250); // Set tap threshold dmp_set_tap_axes(TAP_XYZ); // Set tap axes dmp_set_tap_count(1); // Set tap count dmp_set_tap_time(100); // Set tap time dmp_set_tap_time_multi(500); // Set tap multi time
```
// Set shake rejection parameters
dmp_set_shake_reject_thresh(GYRO_SF, 200);
dmp_set_shake_reject_time(40);
dmp_set_shake_reject_timeout(10);
```
} else { tmp[0] = 0xD8; mpu_write_mem(CFG_20, 1, tmp); }

// Configure Android orientation feature based on the input mask if (mask & DMP_FEATURE_ANDROID_ORIENT) { tmp[0] = 0xD9; } else tmp[0] = 0xD8; mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); // Configure Android orientation feature in memory CFG_ANDROID_ORIENT_INT

// Enable or disable 3-axis quaternion feature based on the input mask if (mask & DMP_FEATURE_LP_QUAT) dmp_enable_lp_quat(1); else dmp_enable_lp_quat(0);

// Enable or disable 6-axis quaternion feature based on the input mask if (mask & DMP_FEATURE_6X_LP_QUAT) dmp_enable_6x_lp_quat(1); else dmp_enable_6x_lp_quat(0);

// Pedometer is always enabled dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; mpu_reset_fifo();

dmp.packet_length = 0; if (mask & DMP_FEATURE_SEND_RAW_ACCEL) dmp.packet_length += 6; if (mask & DMP_FEATURE_SEND_ANY_GYRO) dmp.packet_length += 6; if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) dmp.packet_length += 16; if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) dmp.packet_length += 4;

return 0; }

/**

@brief Get list of currently enabled DMP features.
@param[out] Mask of enabled features.
@return 0 if successful. */ int dmp_get_enabled_features(unsigned short *mask) { mask[0] = dmp.feature_mask; return 0; }

/**

@brief Calibrate the gyro data in the DMP.
After eight seconds of no motion, the DMP will compute gyro biases and
subtract them from the quaternion output. If @e dmp_enable_feature is
called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be
subtracted from the gyro output.
@param[in] enable 1 to enable gyro calibration.
@return 0 if successful. */ int dmp_enable_gyro_cal(unsigned char enable) { if (enable) { unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d}; return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); } else { unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7}; return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); } }

/**

@brief Generate 3-axis quaternions from the DMP.
In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
exclusive.
@param[in] enable 1 to enable 3-axis quaternion.
@return 0 if successful. */ int dmp_enable_lp_quat(unsigned char enable) { unsigned char regs[4]; if (enable) { regs[0] = DINBC0; regs[1] = DINBC2; regs[2] = DINBC4; regs[3] = DINBC6; } else memset(regs, 0x8B, 4);

mpu_write_mem(CFG_LP_QUAT, 4, regs);

return mpu_reset_fifo(); }

/**

@brief Generate 6-axis quaternions from the DMP.
In this driver, the 3-axis and 6-axis DMP quaternion features are mutually
exclusive.
@param[in] enable 1 to enable 6-axis quaternion.
@return 0 if successful. */ int dmp_enable_6x_lp_quat(unsigned char enable) { unsigned char regs[4]; if (enable) { regs[0] = DINA20; regs[1] = DINA28; regs[2] = DINA30; regs[3] = DINA38; } else memset(regs, 0xA3, 4);

mpu_write_mem(CFG_8, 4, regs);

return mpu_reset_fifo(); }

/**

@brief Decode the four-byte gesture data and execute any callbacks.
@param[in] gesture Gesture data from DMP packet.
@return 0 if successful. */ static int decode_gesture(unsigned char *gesture) { unsigned char tap, android_orient;

android_orient = gesture[3] & 0xC0; tap = 0x3F & gesture[3];

if (gesture[1] & INT_SRC_TAP) { unsigned char direction, count; direction = tap >> 3; count = (tap % 8) + 1; if (dmp.tap_cb) dmp.tap_cb(direction, count); }

if (gesture[1] & INT_SRC_ANDROID_ORIENT) { if (dmp.android_orient_cb) dmp.android_orient_cb(android_orient >> 6); }

return 0; }

/**

@brief Specify when a DMP interrupt should occur.
A DMP interrupt can be configured to trigger on either of the two
conditions below:
\n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate).
\n b. A tap event has been detected.
@param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS.
@return 0 if successful. */ int dmp_set_interrupt_mode(unsigned char mode) { const unsigned char regs_continuous[11] = {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9}; const unsigned char regs_gesture[11] = {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda};

switch (mode) { case DMP_INT_CONTINUOUS: return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, (unsigned char*)regs_continuous); case DMP_INT_GESTURE: return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, (unsigned char*)regs_gesture); default: return -1; } }

DMP Feature Enable and Configuration for MPU-9250