接前一篇文章: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 way
q_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 way
q_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 way
q_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 way
q_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 way
q_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);
对于接下来步骤的解析请看下回。