return -1;
memcpy(gyro_regs, gyro_sign, 3); memcpy(accel_regs, accel_sign, 3); if (orient & 4) {
gyro_regs[0] |= 1; accel_regs[0] |= 1; }
if (orient & 0x20) { gyro_regs[1] |= 1; accel_regs[1] |= 1; }
if (orient & 0x100) { gyro_regs[2] |= 1; accel_regs[2] |= 1; }
/* Chip-to-body, sign only. */
if (mpu_write_mem(FCFG_3, 3, gyro_regs)) return -1;
if (mpu_write_mem(FCFG_7, 3, accel_regs)) return -1;
dmp.orient = orient; return 0; } /**
* @brief Push gyro biases to the DMP.
* Because the gyro integration is handled in the DMP, any gyro biases
* calculated by the MPL should be pushed down to DMP memory to remove * 3-axis quaternion drift.
* \\n NOTE: If the DMP-based gyro calibration is enabled, the DMP will
* overwrite the biases written to this location once a new one is computed. * @param[in] bias Gyro biases in q16. * @return 0 if successful. */
int dmp_set_gyro_bias(long *bias) {
long gyro_bias_body[3]; unsigned char regs[4];
gyro_bias_body[0] = bias[dmp.orient & 3]; if (dmp.orient & 4)
gyro_bias_body[0] *= -1;
gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3]; if (dmp.orient & 0x20)
gyro_bias_body[1] *= -1;
gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3]; if (dmp.orient & 0x100)
gyro_bias_body[2] *= -1;
#ifdef EMPL_NO_64BIT
gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f);
gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f);
gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f); #else
gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30); gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30); gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30); #endif
regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF); regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF); regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF); regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF); if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs)) return -1;
regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF); regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF); regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF); regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF); if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs)) return -1;
regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF); regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF); regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF); regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF); return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs); } /**
* @brief Push accel biases to the DMP.
* These biases will be removed from the DMP 6-axis quaternion.
* @param[in] bias Accel biases in q16. * @return 0 if successful. */
int dmp_set_accel_bias(long *bias) {
long accel_bias_body[3]; unsigned char regs[12]; long long accel_sf;
unsigned short accel_sens;
mpu_get_accel_sens(&accel_sens); accel_sf = (long long)accel_sens << 15; //__no_operation();
accel_bias_body[0] = bias[dmp.orient & 3]; if (dmp.orient & 4)
accel_bias_body[0] *= -1;
accel_bias_body[1] = bias[(dmp.orient >> 3) & 3]; if (dmp.orient & 0x20)
accel_bias_body[1] *= -1;
accel_bias_body[2] = bias[(dmp.orient >> 6) & 3]; if (dmp.orient & 0x100)
accel_bias_body[2] *= -1;
#ifdef EMPL_NO_64BIT
accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f);
accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f);
accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f); #else
accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30); accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30); accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30); #endif
regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF); regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF); regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF); regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF);
regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF); regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF); regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF);
regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF);
regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF); regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF); regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF); regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF); return mpu_write_mem(D_ACCEL_BIAS, 12, regs); } /**
* @brief Set DMP output rate. * Only used when DMP is on.
* @param[in] rate Desired fifo rate (Hz). * @return 0 if successful. */
int dmp_set_fifo_rate(unsigned short rate) {
const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB,
0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF}; unsigned short div; unsigned char tmp[8];
if (rate > DMP_SAMPLE_RATE) return -1;
div = DMP_SAMPLE_RATE / rate - 1;
tmp[0] = (unsigned char)((div >> 8) & 0xFF); tmp[1] = (unsigned char)(div & 0xFF); if (mpu_write_mem(D_0_22, 2, tmp)) return -1;
if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end)) return -1;
dmp.fifo_rate = rate; return 0; } /**
* @brief Get DMP output rate.
* @param[out] rate Current fifo rate (Hz). * @return 0 if successful. */
int dmp_get_fifo_rate(unsigned short *rate) {
rate[0] = dmp.fifo_rate; return 0;
} /**
* @brief Set tap threshold for a specific axis.
* @param[in] axis 1, 2, and 4 for XYZ accel, respectively. * @param[in] thresh Tap threshold, in mg/ms. * @return 0 if successful. */
int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh) {
unsigned char tmp[4], accel_fsr; float scaled_thresh;
unsigned short dmp_thresh, dmp_thresh_2; if (!(axis & TAP_XYZ) || thresh > 1600) return -1;
scaled_thresh = (float)thresh / DMP_SAMPLE_RATE;
mpu_get_accel_fsr(&accel_fsr); switch (accel_fsr) { case 2:
dmp_thresh = (unsigned short)(scaled_thresh * 16384); /* dmp_thresh * 0.75 */
dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288); break; case 4:
dmp_thresh = (unsigned short)(scaled_thresh * 8192); /* dmp_thresh * 0.75 */
dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144); break; case 8:
dmp_thresh = (unsigned short)(scaled_thresh * 4096); /* dmp_thresh * 0.75 */
dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072); break; case 16:
dmp_thresh = (unsigned short)(scaled_thresh * 2048); /* dmp_thresh * 0.75 */
dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536); break; default:
return -1; }
tmp[0] = (unsigned char)(dmp_thresh >> 8);