接前一篇文章：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 I2C
		if (whoami == 0xff) {				// if whoami fails try the other I2C Address
			switch_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 body
	s->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 matrix
	memset(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 matrix
	s->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 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);
}

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