ICM20948 DMP代码详解（15）

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

上一回开始对icm20948_sensor_setup函数中第3段代码即inv_icm20948_initialize函数进行解析。为了便于理解和回顾，再次贴出其源码，在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948Setup.c中，如下：

int inv_icm20948_initialize(struct inv_icm20948 * s, const uint8_t *dmp3_image, uint32_t dmp3_image_size)
{
	if(s->serif.is_spi) {
		/* Hardware initialization */
		// No image to be loaded from flash, no pointer to pass.
		if (inv_icm20948_initialize_lower_driver(s, SERIAL_INTERFACE_SPI, dmp3_image, dmp3_image_size)) {
			return -1;
		}
	}
	else {
		/* Hardware initialization */
		// No image to be loaded from flash, no pointer to pass.
		if (inv_icm20948_initialize_lower_driver(s, SERIAL_INTERFACE_I2C, dmp3_image, dmp3_image_size)) {
			return -1;
		}
	}
	return 0;
}

该函数主要是调用了inv_icm20948_initialize_lower_driver，其在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataBaseDriver.c中，代码如下：

/** Should be called once on power up. Loads DMP3, initializes internal variables needed 
*   for other lower driver functions.
*/
int inv_icm20948_initialize_lower_driver(struct inv_icm20948 *s, enum SMARTSENSOR_SERIAL_INTERFACE type, 
	const uint8_t *dmp3_image, uint32_t dmp3_image_size)
{
	int result = 0;
	static unsigned char data;
	// set static variable
	s->sAllowLpEn = 1;
	s->s_compass_available = 0;
	// ICM20948 do not support the proximity sensor for the moment.
	// s_proximity_available variable is nerver changes
	s->s_proximity_available = 0;
 
	// Set varialbes to default values
	memset(&s->base_state, 0, sizeof(s->base_state));
	s->base_state.pwr_mgmt_1 = BIT_CLK_PLL;
	s->base_state.pwr_mgmt_2 = BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY | BIT_PWR_PRESSURE_STBY;
	s->base_state.serial_interface = type;
	result |= inv_icm20948_read_mems_reg(s, REG_USER_CTRL, 1, &s->base_state.user_ctrl);
 
	result |= inv_icm20948_wakeup_mems(s);
 
	result |= inv_icm20948_read_mems_reg(s, REG_WHO_AM_I, 1, &data);
 
	/* secondary cycle mode should be set all the time */
	data = BIT_I2C_MST_CYCLE|BIT_ACCEL_CYCLE|BIT_GYRO_CYCLE;
 
	// Set default mode to low power mode
	result |= inv_icm20948_set_lowpower_or_highperformance(s, 0);
	
	// Disable Ivory DMP.
	if(s->base_state.serial_interface == SERIAL_INTERFACE_SPI)   
		s->base_state.user_ctrl = BIT_I2C_IF_DIS;
	else
		s->base_state.user_ctrl = 0;
 
	result |= inv_icm20948_write_single_mems_reg(s, REG_USER_CTRL, s->base_state.user_ctrl);
 
	//Setup Ivory DMP.
	result |= inv_icm20948_load_firmware(s, dmp3_image, dmp3_image_size);
	if(result)
		return result;
	else
		s->base_state.firmware_loaded = 1;
	result |= inv_icm20948_set_dmp_address(s);
	// Turn off all sensors on DMP by default.
	//result |= dmp_set_data_output_control1(0);   // FIXME in DMP, these should be off by default.
	result |= dmp_icm20948_reset_control_registers(s);
	
	// set FIFO watermark to 80% of actual FIFO size
	result |= dmp_icm20948_set_FIFO_watermark(s, 800);
 
	// Enable Interrupts.
	data = 0x2;
	result |= inv_icm20948_write_mems_reg(s, REG_INT_ENABLE, 1, &data); // Enable DMP Interrupt
	data = 0x1;
	result |= inv_icm20948_write_mems_reg(s, REG_INT_ENABLE_2, 1, &data); // Enable FIFO Overflow Interrupt
 
	// TRACKING : To have accelerometers datas and the interrupt without gyro enables.
	data = 0XE4;
	result |= inv_icm20948_write_mems_reg(s, REG_SINGLE_FIFO_PRIORITY_SEL, 1, &data);
 
	// Disable HW temp fix
	inv_icm20948_read_mems_reg(s, REG_HW_FIX_DISABLE,1,&data);
	data |= 0x08;
	inv_icm20948_write_mems_reg(s, REG_HW_FIX_DISABLE,1,&data);
 
	// Setup MEMs properties.
	s->base_state.accel_averaging = 1; //Change this value if higher sensor sample avergaing is required.
	s->base_state.gyro_averaging = 1;  //Change this value if higher sensor sample avergaing is required.
	inv_icm20948_set_gyro_divider(s, FIFO_DIVIDER);       //Initial sampling rate 1125Hz/19+1 = 56Hz.
	inv_icm20948_set_accel_divider(s, FIFO_DIVIDER);      //Initial sampling rate 1125Hz/19+1 = 56Hz.
 
	// Init the sample rate to 56 Hz for BAC,STEPC and B2S
	dmp_icm20948_set_bac_rate(s, DMP_ALGO_FREQ_56);
	dmp_icm20948_set_b2s_rate(s, DMP_ALGO_FREQ_56);
 
	// FIFO Setup.
	result |= inv_icm20948_write_single_mems_reg(s, REG_FIFO_CFG, BIT_SINGLE_FIFO_CFG); // FIFO Config. fixme do once? burst write?
	result |= inv_icm20948_write_single_mems_reg(s, REG_FIFO_RST, 0x1f); // Reset all FIFOs.
	result |= inv_icm20948_write_single_mems_reg(s, REG_FIFO_RST, 0x1e); // Keep all but Gyro FIFO in reset.
	result |= inv_icm20948_write_single_mems_reg(s, REG_FIFO_EN, 0x0); // Slave FIFO turned off.
	result |= inv_icm20948_write_single_mems_reg(s, REG_FIFO_EN_2, 0x0); // Hardware FIFO turned off.
    
	s->base_state.lp_en_support = 1;
	
	if(s->base_state.lp_en_support == 1)
		inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 1);
 
	result |= inv_icm20948_sleep_mems(s);   
        
	return result;
}

上一回讲了函数一开始的成员设置，本回继续往下对该函数进行解析。当前来到以下代码片段：

	result |= inv_icm20948_read_mems_reg(s, REG_USER_CTRL, 1, &s->base_state.user_ctrl);

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

/**
*  @brief      Read data from a register on MEMs.
*  @param[in]  Register address
*  @param[in]  Length of data
*  @param[in]  Data to be written
*  @return     0 if successful.
*/
int inv_icm20948_read_mems_reg(struct inv_icm20948 *s, uint16_t reg, unsigned int length, unsigned char *data)
{
	int result = 0;
	unsigned int bytesRead = 0;
	unsigned char regOnly = (unsigned char)(reg & 0x7F);
	unsigned char i, dat[INV_MAX_SERIAL_READ];
	unsigned char power_state = inv_icm20948_get_chip_power_state(s);

	if((power_state & CHIP_AWAKE) == 0)   // Wake up chip since it is asleep
		result = inv_icm20948_set_chip_power_state(s, CHIP_AWAKE, 1);

	if(check_reg_access_lp_disable(s, reg))   // Check if register needs LP_EN to be disabled
		result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 0);  //Disable LP_EN

	result |= inv_set_bank(s, reg >> 7);

	while (bytesRead<length) 
	{
		int thisLen = min(INV_MAX_SERIAL_READ, length-bytesRead);
		if(s->base_state.serial_interface == SERIAL_INTERFACE_SPI) {
			result |= inv_icm20948_read_reg(s, regOnly+bytesRead, &dat[bytesRead], thisLen);
		} else {
			result |= inv_icm20948_read_reg(s, regOnly+bytesRead, &data[bytesRead],thisLen);
		}

		if (result)
			return result;

		bytesRead += thisLen;
	}

	if(s->base_state.serial_interface == SERIAL_INTERFACE_SPI) {
		for (i=0; i< length; i++) {
			*data= dat[i];
			data++;
		}
	}

	if(check_reg_access_lp_disable(s, reg))    // Check if register needs LP_EN to be enabled  
		result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 1);  //Enable LP_EN

	return result;
}

要弄懂这个函数，必须先弄清楚其中的几个函数。一个一个来看。

（1）inv_icm20948_get_chip_power_state函数

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

/*!
 ******************************************************************************
 *   @return    Current wake status of the Ivory chip.
 ******************************************************************************
 */
uint8_t inv_icm20948_get_chip_power_state(struct inv_icm20948 *s)
{
	return s->base_state.wake_state;
}

其实就是得到s->base_state.wake_state的值。前文书中曾讲到，在inv_icm20948_initialize_lower_driver函数中对于s->base_state整个进行了清零，因此这时的值必然是0。

（2）inv_icm20948_set_chip_power_state函数

inv_icm20948_set_chip_power_state函数也是在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataBaseDriver.c中，代码如下：

/*!
 ******************************************************************************
 *   @brief     This function sets the power state of the Ivory chip 
 *				loop
 *   @param[in] Function - CHIP_AWAKE, CHIP_LP_ENABLE
 *   @param[in] On/Off - The functions are enabled if previously disabled and 
                disabled if previously enabled based on the value of On/Off.
 ******************************************************************************
 */ 
int inv_icm20948_set_chip_power_state(struct inv_icm20948 *s, unsigned char func, unsigned char on_off)
{
	int status = 0;

	switch(func) {

		case CHIP_AWAKE:    
			if(on_off){
				if((s->base_state.wake_state & CHIP_AWAKE) == 0) {// undo sleep_en
					s->base_state.pwr_mgmt_1 &= ~BIT_SLEEP;
					status = inv_icm20948_write_single_mems_reg_core(s, REG_PWR_MGMT_1, s->base_state.pwr_mgmt_1);
					s->base_state.wake_state |= CHIP_AWAKE;
					inv_icm20948_sleep_100us(1); // after writing the bit wait 100 Micro Seconds
				}
			} else {
				if(s->base_state.wake_state & CHIP_AWAKE) {// set sleep_en
					s->base_state.pwr_mgmt_1 |= BIT_SLEEP;
					status = inv_icm20948_write_single_mems_reg_core(s, REG_PWR_MGMT_1, s->base_state.pwr_mgmt_1);
					s->base_state.wake_state &= ~CHIP_AWAKE;
					inv_icm20948_sleep_100us(1); // after writing the bit wait 100 Micro Seconds
				}
			}
		break;

		case CHIP_LP_ENABLE:
			if(s->base_state.lp_en_support == 1) {
				if(on_off) {
					if( (inv_icm20948_get_lpen_control(s)) && ((s->base_state.wake_state & CHIP_LP_ENABLE) == 0)){
						s->base_state.pwr_mgmt_1 |= BIT_LP_EN; // lp_en ON
						status = inv_icm20948_write_single_mems_reg_core(s, REG_PWR_MGMT_1, s->base_state.pwr_mgmt_1);
						s->base_state.wake_state |= CHIP_LP_ENABLE;
					}
				} else {
					if(s->base_state.wake_state & CHIP_LP_ENABLE){
						s->base_state.pwr_mgmt_1 &= ~BIT_LP_EN; // lp_en off
						status = inv_icm20948_write_single_mems_reg_core(s, REG_PWR_MGMT_1, s->base_state.pwr_mgmt_1);
						s->base_state.wake_state &= ~CHIP_LP_ENABLE;
						inv_icm20948_sleep_100us(1); // after writing the bit wait 100 Micro Seconds
					}
				}
			}
		break;

		default:
		break;

	}// end switch

	return status;
}

这要结合inv_icm20948_read_mems_reg函数中的调用来看，代码片段如下：

	if((power_state & CHIP_AWAKE) == 0)   // Wake up chip since it is asleep
		result = inv_icm20948_set_chip_power_state(s, CHIP_AWAKE, 1);

	if(check_reg_access_lp_disable(s, reg))   // Check if register needs LP_EN to be disabled
		result |= inv_icm20948_set_chip_power_state(s, CHIP_LP_ENABLE, 0);  //Disable LP_EN

一共两次调用，一次是CHIP_AWAKE，另一次是CHIP_LP_ENABLE。正好对应inv_icm20948_set_chip_power_state函数中switch的两个case。依次看一下这两个分支：

1）case CHIP_AWAKE

相应代码片段如下：

		case CHIP_AWAKE:    
			if(on_off){
				if((s->base_state.wake_state & CHIP_AWAKE) == 0) {// undo sleep_en
					s->base_state.pwr_mgmt_1 &= ~BIT_SLEEP;
					status = inv_icm20948_write_single_mems_reg_core(s, REG_PWR_MGMT_1, s->base_state.pwr_mgmt_1);
					s->base_state.wake_state |= CHIP_AWAKE;
					inv_icm20948_sleep_100us(1); // after writing the bit wait 100 Micro Seconds
				}
			} else {
				if(s->base_state.wake_state & CHIP_AWAKE) {// set sleep_en
					s->base_state.pwr_mgmt_1 |= BIT_SLEEP;
					status = inv_icm20948_write_single_mems_reg_core(s, REG_PWR_MGMT_1, s->base_state.pwr_mgmt_1);
					s->base_state.wake_state &= ~CHIP_AWAKE;
					inv_icm20948_sleep_100us(1); // after writing the bit wait 100 Micro Seconds
				}
			}
		break;

如果inv_icm20948_set_chip_power_state函数的第3个参数unsigned char on_off传入的值为1，同时状态与本次设置不同，则清除s->base_state.pwr_mgmt_1中的BIT_SLEEP位；写入ICM20948的REG_PWR_MGMT_1寄存器；之后将s->base_state.wake_state中的CHIP_AWAKE置位，并延时100us。

如果on_off传入的值为0，同时状态与本次设置不同，则置位s->base_state.pwr_mgmt_1中的BIT_SLEEP位；写入ICM20948的REG_PWR_MGMT_1寄存器；之后清除s->base_state.wake_state中的CHIP_AWAKE位，并延时100us。

2）case CHIP_LP_ENABLE

相应代码片段如下：

		case CHIP_LP_ENABLE:
			if(s->base_state.lp_en_support == 1) {
				if(on_off) {
					if( (inv_icm20948_get_lpen_control(s)) && ((s->base_state.wake_state & CHIP_LP_ENABLE) == 0)){
						s->base_state.pwr_mgmt_1 |= BIT_LP_EN; // lp_en ON
						status = inv_icm20948_write_single_mems_reg_core(s, REG_PWR_MGMT_1, s->base_state.pwr_mgmt_1);
						s->base_state.wake_state |= CHIP_LP_ENABLE;
					}
				} else {
					if(s->base_state.wake_state & CHIP_LP_ENABLE){
						s->base_state.pwr_mgmt_1 &= ~BIT_LP_EN; // lp_en off
						status = inv_icm20948_write_single_mems_reg_core(s, REG_PWR_MGMT_1, s->base_state.pwr_mgmt_1);
						s->base_state.wake_state &= ~CHIP_LP_ENABLE;
						inv_icm20948_sleep_100us(1); // after writing the bit wait 100 Micro Seconds
					}
				}
			}
		break;

在s->base_state.lp_en_support设置的情况下，判断体代码才有效。此时并未设置过，还是之前在inv_icm20948_initialize_lower_driver函数中设置的清零状态。

不过这里也先解析一下，免得以后还得回来再解析。

如果inv_icm20948_set_chip_power_state函数的第3个参数unsigned char on_off传入的值为1，同时状态与本次设置不同，则置位s->base_state.pwr_mgmt_1中的BIT_LP_EN位；写入ICM20948的REG_PWR_MGMT_1寄存器；之后将s->base_state.wake_state中的CHIP_LP_ENABLE置位，并延时100us。

如果on_off传入的值为0，同时状态与本次设置不同，则清除s->base_state.pwr_mgmt_1中的BIT_LP_EN位；写入ICM20948的REG_PWR_MGMT_1寄存器；之后清除s->base_state.wake_state中的CHIP_LP_ENABLE位，并延时100us。

inv_icm20948_set_chip_power_state函数中还有三个函数要讲一下，分别是：

1）inv_icm20948_write_single_mems_reg_core函数

2）inv_icm20948_get_lpen_control函数

3）inv_icm20948_sleep_100us函数

对于这三个函数的解析，请看下回。