ICM20948 DMP代码详解（13）

接前一篇文章：ICM20948 DMP代码详解（12）

上一回完成了对inv_icm20948_set_chip_to_body_axis_quaternion函数第2步即inv_rotation_to_quaternion函数的解析。回到inv_icm20948_set_chip_to_body_axis_quaternion中来，继续往下进行解析。为了便于理解和回顾，再次贴出该函数源码：

void inv_icm20948_set_chip_to_body_axis_quaternion(struct inv_icm20948 *s, signed char *accel_gyro_matrix, float angle)
{int i;float rot[9];long qcb[4];long q_all[4];long q_adjust[4];for (i=0; i<9; i++)rot[i] = (float)accel_gyro_matrix[i];//convert Chip to Body transformation matrix to quaternion//inv_icm20948_convert_matrix_to_quat_fxp(rot, qcb);inv_rotation_to_quaternion(rot, qcb);//The quaterion generated is the inverse, take the inverse again.qcb[1] = -qcb[1];qcb[2] = -qcb[2];qcb[3] = -qcb[3];//now rotate by angle, negate angle to rotate other wayq_adjust[0] = (long)((1L<<30) * cosf(-angle*(float)M_PI/180.f/2.f));q_adjust[1] = 0;q_adjust[2] = (long)((1L<<30) * sinf(-angle*(float)M_PI/180.f/2.f));q_adjust[3] = 0;invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);inv_icm20948_set_chip_to_body(s, q_all);
}

接下来是第3段代码：

    //The quaterion generated is the inverse, take the inverse again.qcb[1] = -qcb[1];qcb[2] = -qcb[2];qcb[3] = -qcb[3];

在第2步中由旋转矩阵（数组rot[9]）得到了四元数，在这里还需要进一步对于qcb[1]~qcb[3]做乘以-1操作。

接下来是第4段代码：

    //now rotate by angle, negate angle to rotate other wayq_adjust[0] = (long)((1L<<30) * cosf(-angle*(float)M_PI/180.f/2.f));q_adjust[1] = 0;q_adjust[2] = (long)((1L<<30) * sinf(-angle*(float)M_PI/180.f/2.f));q_adjust[3] = 0;invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);inv_icm20948_set_chip_to_body(s, q_all);

代码中的angle来自inv_icm20948_set_chip_to_body_axis_quaternion函数的第3个参数float angle，其对应的实参在inv_icm20948_init_matrix函数中传入，为0.0。

由于这里传入的值为0.0，因此以上代码片段

    //now rotate by angle, negate angle to rotate other wayq_adjust[0] = (long)((1L<<30) * cosf(-angle*(float)M_PI/180.f/2.f));q_adjust[1] = 0;q_adjust[2] = (long)((1L<<30) * sinf(-angle*(float)M_PI/180.f/2.f));q_adjust[3] = 0;

实际上是：

    //now rotate by angle, negate angle to rotate other wayq_adjust[0] = (long)((1L<<30);q_adjust[1] = 0;q_adjust[2] = 0;q_adjust[3] = 0;

接下来是invn_convert_quat_mult_fxp函数。

    invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);

其在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.c中，代码如下：

static void invn_convert_quat_mult_fxp(const long *quat1_q30, const long *quat2_q30, long *quatProd_q30)
{quatProd_q30[0] = inv_icm20948_convert_mult_q30_fxp(quat1_q30[0], quat2_q30[0]) - inv_icm20948_convert_mult_q30_fxp(quat1_q30[1], quat2_q30[1]) -inv_icm20948_convert_mult_q30_fxp(quat1_q30[2], quat2_q30[2]) - inv_icm20948_convert_mult_q30_fxp(quat1_q30[3], quat2_q30[3]);quatProd_q30[1] = inv_icm20948_convert_mult_q30_fxp(quat1_q30[0], quat2_q30[1]) + inv_icm20948_convert_mult_q30_fxp(quat1_q30[1], quat2_q30[0]) +inv_icm20948_convert_mult_q30_fxp(quat1_q30[2], quat2_q30[3]) - inv_icm20948_convert_mult_q30_fxp(quat1_q30[3], quat2_q30[2]);quatProd_q30[2] = inv_icm20948_convert_mult_q30_fxp(quat1_q30[0], quat2_q30[2]) - inv_icm20948_convert_mult_q30_fxp(quat1_q30[1], quat2_q30[3]) +inv_icm20948_convert_mult_q30_fxp(quat1_q30[2], quat2_q30[0]) + inv_icm20948_convert_mult_q30_fxp(quat1_q30[3], quat2_q30[1]);quatProd_q30[3] = inv_icm20948_convert_mult_q30_fxp(quat1_q30[0], quat2_q30[3]) + inv_icm20948_convert_mult_q30_fxp(quat1_q30[1], quat2_q30[2]) -inv_icm20948_convert_mult_q30_fxp(quat1_q30[2], quat2_q30[1]) + inv_icm20948_convert_mult_q30_fxp(quat1_q30[3], quat2_q30[0]);
}

这个函数中最主要就是调用了inv_icm20948_convert_mult_q30_fxp函数进行计算。inv_icm20948_convert_mult_q30_fxp函数在同文件中，代码如下：

long inv_icm20948_convert_mult_q30_fxp(long a_q30, long b_q30)
{long long temp;long result;temp = (long long)a_q30 * b_q30;result = (long)(temp >> 30);return result;
}

其实就是简单的乘法，之后再右移30位。

那么invn_convert_quat_mult_fxp函数的作用实际上就是计算两个四元数相乘的结果。

参考：四元数乘法计算-CSDN博客

回到inv_icm20948_set_chip_to_body_axis_quaternion函数中，接下来是inv_icm20948_set_chip_to_body函数。

    invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);inv_icm20948_set_chip_to_body(s, q_all);

其也在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.c中，代码如下：

/** Set the transformation used for chip to body frame
*/
void inv_icm20948_set_chip_to_body(struct inv_icm20948 * s, long *quat)
{memcpy(s->s_quat_chip_to_body, quat, sizeof(s->s_quat_chip_to_body));
}

这个函数很简单、也很好理解，就是把上一步计算好的q_all[4]，赋给s->s_quat_chip_to_body。s_quat_chip_to_body是struct inv_icm20948的成员，相关定义如下：

typedef struct inv_icm20948 {struct inv_icm20948_serif serif;……/* data converter */long s_quat_chip_to_body[4];……
} inv_icm20948_t;

s->s_quat_chip_to_body在前文书解析inv_icm20948_init_matrix函数的时候讲到过，相关代码如下：

和上边的q_adjust数组类似。

    //now rotate by angle, negate angle to rotate other wayq_adjust[0] = (long)((1L<<30);q_adjust[1] = 0;q_adjust[2] = 0;q_adjust[3] = 0;

至此，inv_icm20948_set_chip_to_body_axis_quaternion函数就解析完了。inv_icm20948_init_matrix函数也就解析完了。

回到icm20948_sensor_setup函数中，当前完成了第2段代码的解析，

	/* Setup accel and gyro mounting matrix and associated angle for current board */inv_icm20948_init_matrix(&icm_device);

对于接下来步骤的解析请看下回。