DMP功能启用和配置 - 陀螺仪数据校准、四元数生成、手势识别 - 常规

/**

@brief 启用DMP功能。
输入掩码中使用以下#define：
\n DMP_FEATURE_TAP
\n DMP_FEATURE_ANDROID_ORIENT
\n DMP_FEATURE_LP_QUAT
\n DMP_FEATURE_6X_LP_QUAT
\n DMP_FEATURE_GYRO_CAL
\n DMP_FEATURE_SEND_RAW_ACCEL
\n DMP_FEATURE_SEND_RAW_GYRO
注意：DMP_FEATURE_LP_QUAT和DMP_FEATURE_6X_LP_QUAT是互斥的。
注意：DMP_FEATURE_SEND_RAW_GYRO和DMP_FEATURE_SEND_CAL_GYRO也是互斥的。
@param[in] mask 启用功能的掩码。
@return 如果成功则返回0。 */ int dmp_enable_feature(unsigned short mask) { unsigned char tmp[10];

/* TODO: 所有这些设置可能都可以集成到默认的DMP映像中。/ / 设置积分比例因子。*/ 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); // 将积分比例因子写入DMP内存地址D_0_104

/* 将传感器数据发送到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); // 将传感器数据发送配置写入DMP内存地址CFG_15

/* 将手势数据发送到FIFO。*/ if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) tmp[0] = DINA20; // 启用手势数据发送 else tmp[0] = 0xD8; mpu_write_mem(CFG_27,1,tmp); // 将手势数据发送配置写入DMP内存地址CFG_27

if (mask & DMP_FEATURE_GYRO_CAL) dmp_enable_gyro_cal(1); // 启用陀螺仪校准 else dmp_enable_gyro_cal(0);

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); // 将陀螺仪数据发送配置写入DMP内存地址CFG_GYRO_RAW_DATA }

if (mask & DMP_FEATURE_TAP) { /* 启用敲击。*/ tmp[0] = 0xF8; mpu_write_mem(CFG_20, 1, tmp); // 启用敲击功能 dmp_set_tap_thresh(TAP_XYZ, 250); // 设置敲击阈值 dmp_set_tap_axes(TAP_XYZ); // 设置敲击轴 dmp_set_tap_count(1); // 设置敲击次数 dmp_set_tap_time(100); // 设置敲击时间间隔 dmp_set_tap_time_multi(500); // 设置敲击时间间隔倍数
```
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); // 禁用敲击功能 }

if (mask & DMP_FEATURE_ANDROID_ORIENT) { tmp[0] = 0xD9; // 启用Android方向传感器 } else tmp[0] = 0xD8; mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); // 将Android方向传感器配置写入DMP内存地址CFG_ANDROID_ORIENT_INT

if (mask & DMP_FEATURE_LP_QUAT) dmp_enable_lp_quat(1); // 启用低功耗3轴四元数 else dmp_enable_lp_quat(0);

if (mask & DMP_FEATURE_6X_LP_QUAT) dmp_enable_6x_lp_quat(1); // 启用低功耗6轴四元数 else dmp_enable_6x_lp_quat(0);

/* 计步器始终启用。*/ dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; // 设置DMP功能掩码 mpu_reset_fifo(); // 重置FIFO

dmp.packet_length = 0; if (mask & DMP_FEATURE_SEND_RAW_ACCEL) dmp.packet_length += 6; // 计算DMP数据包长度 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 获取当前启用的DMP功能列表。
@param[out] mask 启用功能的掩码。
@return 如果成功则返回0。 */ int dmp_get_enabled_features(unsigned short *mask) { mask[0] = dmp.feature_mask; // 获取DMP功能掩码 return 0; }

/**

@brief 在DMP中校准陀螺仪数据。
在没有运动的情况下，经过8秒钟，DMP将计算陀螺仪偏差并将其从四元数输出中减去。
如果使用@dmp_enable_feature调用@DMP_FEATURE_SEND_CAL_GYRO，则偏差也将从陀螺仪输出中减去。
@param[in] enable 1启用陀螺仪校准。
@return 如果成功则返回0。 */ 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 从DMP生成3轴四元数。
在此驱动程序中，3轴和6轴DMP四元数功能是互斥的。
@param[in] enable 1启用3轴四元数。
@return 如果成功则返回0。 */ 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); // 将3轴四元数配置写入DMP内存地址CFG_LP_QUAT

return mpu_reset_fifo(); // 重置FIFO }

/**

@brief 从DMP生成6轴四元数。
在此驱动程序中，3轴和6轴DMP四元数功能是互斥的。
@param[in] enable 1启用6轴四元数。
@return 如果成功则返回0。 */ 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); // 将6轴四元数配置写入DMP内存地址CFG_8

return mpu_reset_fifo(); // 重置FIFO }

/**

@brief 解码4字节的手势数据并执行任何回调。
@param[in] gesture 来自DMP数据包的手势数据。
@return 如果成功则返回0。 */ static int decode_gesture(unsigned char *gesture) { unsigned char tap, android_orient;

android_orient = gesture[3] & 0xC0; // 获取Android方向数据 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) { // 判断是否检测到Android方向变化 if (dmp.android_orient_cb) dmp.android_orient_cb(android_orient >> 6); // 执行Android方向回调 }

return 0; }

/**

@brief 指定DMP中断何时发生。
DMP中断可以配置为在以下两种条件之一下触发：
\n a. 一个FIFO周期已过（由@e mpu_set_sample_rate设置）。
\n b. 已检测到敲击事件。
@param[in] mode DMP_INT_GESTURE或DMP_INT_CONTINUOUS。
@return 如果成功则返回0。 */ 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: // 设置DMP中断模式为连续模式 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, (unsigned char*)regs_continuous); case DMP_INT_GESTURE: // 设置DMP中断模式为手势模式 return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, (unsigned char*)regs_gesture); default: return -1; } }