MPU6050 DMP移植问题

橘子先生 · 发表于 2016-7-25 22:54:40

移植完原子的DMP之后串口打印pitch、roll、yaw，发现全部为0，然后用串口打印mpu_dmp_get_data（）的值，发现返回值是1，也就是说dmp_read_fifo（）函数返回值不为0，接着打印dmp_read_fifo（）的返回值，发现返回值为-1，此函数内有两处返回值都是-1，再往上找就懵逼了，到底是怎么回事？求教DMP怎么移植，下面是上述几个函数的函数体[mw_shl_code=c,true]u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))
return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
   * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
   * The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30¸ñê½×a»»Îa¸¡μãêy
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//¼ÆËãμÃμ½¸©Ñö½Ç/oá1ö½Ç/o½Ïò½Ç
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}
else
return 2;
return 0;
}
[/mw_shl_code]

[mw_shl_code=c,true]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;
}[/mw_shl_code]

wszdxmh · 发表于 2016-7-25 23:37:14

直接复制DMP相关的.c和.h，我就是这样干的，因为我懒。

橘子先生 · 发表于 2016-7-26 10:29:52

本帖最后由橘子先生于 2016-7-26 10:32 编辑

wszdxmh 发表于 2016-7-25 23:37
直接复制DMP相关的.c和.h，我就是这样干的，因为我懒。

dmpKey.h dmpmap.h inv_mpu.c inv_mpu.h inv_mpu_dmp_motion_driver.c inv_mpu_dmp_motion_driver.h 都是直接复制过来的，然后里面的一些相关的函数，像Delay_ms()，I2C_write()什么的都是自己写完之后改了的，fifo我也使能了，然后就不能用。。。