/**
* @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; } } /**
* @brief Get one packet from the FIFO.
* If @e sensors does not contain a particular sensor, disregard the data * returned to that pointer.
* \\n @e sensors can contain a combination of the following flags: * \\n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO * \\n INV_XYZ_GYRO * \\n INV_XYZ_ACCEL * \\n INV_WXYZ_QUAT
* \\n If the FIFO has no new data, @e sensors will be zero.
* \\n If the FIFO is disabled, @e sensors will be zero and this function will * return a non-zero error code.
* @param[out] gyro Gyro data in hardware units. * @param[out] accel Accel data in hardware units.
* @param[out] quat 3-axis quaternion data in hardware units. * @param[out] timestamp Timestamp in milliseconds. * @param[out] sensors Mask of sensors read from FIFO. * @param[out] more Number of remaining packets. * @return 0 if successful. */
int dmp_read_fifo(short *gyro, short *accel, long *quat,
unsigned long *timestamp, short *sensors, unsigned char *more) {
unsigned char fifo_data[MAX_PACKET_LENGTH]; unsigned char ii = 0;
/* TODO: sensors[0] only changes when dmp_enable_feature is called. We can * cache this value and save some cycles. */
sensors[0] = 0;
/* Get a packet. */
if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more)) return -1;
/* Parse DMP packet. */
if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) { #ifdef FIFO_CORRUPTION_CHECK
long quat_q14[4], quat_mag_sq; #endif
quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | ((long)fifo_data[2] << 8) | fifo_data[3];
quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | ((long)fifo_data[6] << 8) | fifo_data[7];
quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | ((long)fifo_data[10] << 8) | fifo_data[11];
quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | ((long)fifo_data[14] << 8) | fifo_data[15]; ii += 16;
#ifdef FIFO_CORRUPTION_CHECK
/* We can detect a corrupted FIFO by monitoring the quaternion data and * ensuring that the magnitude is always normalized to one. This * shouldn't happen in normal operation, but if an I2C error occurs, * the FIFO reads might become misaligned. *
* Let's start by scaling down the quaternion data to avoid long long * math. */
quat_q14[0] = quat[0] >> 16; quat_q14[1] = quat[1] >> 16; quat_q14[2] = quat[2] >> 16; quat_q14[3] = quat[3] >> 16;
quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3]; if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || (quat_mag_sq > QUAT_MAG_SQ_MAX)) {
/* Quaternion is outside of the acceptable threshold. */ mpu_reset_fifo(); sensors[0] = 0; return -1; }
sensors[0] |= INV_WXYZ_QUAT; #endif }
if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) { accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; ii += 6;
sensors[0] |= INV_XYZ_ACCEL; }
if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) { gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; ii += 6;
sensors[0] |= INV_XYZ_GYRO; }
/* Gesture data is at the end of the DMP packet. Parse it and call * the gesture callbacks (if registered). */
if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) decode_gesture(fifo_data + ii);
//get_ms(timestamp); return 0; } /**
* @brief Register a function to be executed on a tap event. * The tap direction is represented by one of the following: * \\n TAP_X_UP * \\n TAP_X_DOWN * \\n TAP_Y_UP * \\n TAP_Y_DOWN * \\n TAP_Z_UP * \\n TAP_Z_DOWN
* @param[in] func Callback function. * @return 0 if successful. */
int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)) {
dmp.tap_cb = func; return 0; }
/**
* @brief Register a function to be executed on a android orientation event.
* @param[in] func Callback function. * @return 0 if successful. */
int dmp_register_android_orient_cb(void (*func)(unsigned char)) {
dmp.android_orient_cb = func; return 0; } /** * @} */