ICM20948 DMP代码详解（11）

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

上一回讲解完了icm20948_sensor_setup函数的第1段代码，本回继续往下解析。为了便于理解和回顾，再次贴出icm20948_sensor_setup函数源码，在EMD-App\src\ICM20948\sensor.c中，如下：

int icm20948_sensor_setup(void){int rc;uint8_t i, whoami = 0xff;/** Just get the whoami*/rc = inv_icm20948_get_whoami(&icm_device, &whoami);if (interface_is_SPI() == 0)	{		// If we're using I2Cif (whoami == 0xff) {				// if whoami fails try the other I2C Addressswitch_I2C_to_revA();rc = inv_icm20948_get_whoami(&icm_device, &whoami);}}INV_MSG(INV_MSG_LEVEL_INFO, "ICM20948 WHOAMI value=0x%02x", whoami);/** Check if WHOAMI value corresponds to any value from EXPECTED_WHOAMI array*/for(i = 0; i < sizeof(EXPECTED_WHOAMI)/sizeof(EXPECTED_WHOAMI[0]); ++i) {if(whoami == EXPECTED_WHOAMI[i]) {break;}}if(i == sizeof(EXPECTED_WHOAMI)/sizeof(EXPECTED_WHOAMI[0])) {INV_MSG(INV_MSG_LEVEL_ERROR, "Bad WHOAMI value. Got 0x%02x.", whoami);return rc;}/* Setup accel and gyro mounting matrix and associated angle for current board */inv_icm20948_init_matrix(&icm_device);/* set default power mode */INV_MSG(INV_MSG_LEVEL_VERBOSE, "Putting Icm20948 in sleep mode...");rc = inv_icm20948_initialize(&icm_device, dmp3_image, sizeof(dmp3_image));if (rc != 0) {INV_MSG(INV_MSG_LEVEL_ERROR, "Initialization failed. Error loading DMP3...");return rc;}/** Configure and initialize the ICM20948 for normal use*/INV_MSG(INV_MSG_LEVEL_INFO, "Booting up icm20948...");/* Initialize auxiliary sensors */inv_icm20948_register_aux_compass(&icm_device, INV_ICM20948_COMPASS_ID_AK09916, AK0991x_DEFAULT_I2C_ADDR);rc = inv_icm20948_initialize_auxiliary(&icm_device);if (rc == -1) {INV_MSG(INV_MSG_LEVEL_ERROR, "Compass not detected...");}icm20948_apply_mounting_matrix();icm20948_set_fsr();/* re-initialize base state structure */inv_icm20948_init_structure(&icm_device);/* we should be good to go ! */INV_MSG(INV_MSG_LEVEL_VERBOSE, "We're good to go !");return 0;
}

接下来是第2段代码，代码片段如下：

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

这段代码根据说明，功能是为当前电路板设置加速度和陀螺仪安装（悬挂）矩阵以及相关角度。

inv_icm20948_init_matrix函数在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948Setup.c中，代码如下：

void inv_icm20948_init_matrix(struct inv_icm20948 *s)
{// initialize chip to bodys->s_quat_chip_to_body[0] = (1L<<30);s->s_quat_chip_to_body[1] = 0;s->s_quat_chip_to_body[2] = 0;s->s_quat_chip_to_body[3] = 0;//initialize mounting matrixmemset(s->mounting_matrix, 0, sizeof(s->mounting_matrix));s->mounting_matrix[0] = 1;s->mounting_matrix[4] = 1;s->mounting_matrix[8] = 1;//initialize soft iron matrixs->soft_iron_matrix[0] = (1L<<30);s->soft_iron_matrix[4] = (1L<<30);s->soft_iron_matrix[8] = (1L<<30);inv_icm20948_set_chip_to_body_axis_quaternion(s, s->mounting_matrix, 0.0);
}

函数参数struct inv_icm20948 *s对应的实参为&icm_device。icm_device前文已讲过，为EMD-App\src\ICM20948\sensor.c中的全局变量，之前已在inv_icm20948_reset_states函数中做过初始化。

参数类型struct inv_icm20948前文也讲到过，当时说后续使用的时候再针对于具体成员进行讲解。之前在inv_icm20948_register_aux_compass函数中用到了该结构中的struct inv_icm20948_secondary_states secondary_state成员和signed char mounting_matrix_secondary_compass[9]成员，这里用到的是signed char mounting_matrix[9]成员、long soft_iron_matrix[9]成员、以及long s_quat_chip_to_body[4]成员。

typedef struct inv_icm20948 {struct inv_icm20948_serif serif;……/* data converter */long s_quat_chip_to_body[4];……/* Icm20948Fifo usage */signed char mounting_matrix[9];signed char mounting_matrix_secondary_compass[9];long soft_iron_matrix[9];……
} inv_icm20948_t;

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

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);
}

对于该函数的解析，放在下一回中。