How to read fifo data in TWI?

Question

Hello.

I use nRF52 & mpu9250. I want to use dmp. But now it's not going well.

Now, there is no error,but fifo_count doesn't change after fifo_count became 512. I think fifo_count should reset after finished reading fifo data. DMP is enable. It checks code of reading fifo_stream.

I think the cause is method of loading from fifo register.

I test code below,but it's not going well.

static uint32_t mpu9250_read_fifo_register(uint8_t address, uint8_t reg, uint32_t length, uint8_t * p_data)
{
	uint32_t err_code;

	err_code = nrf_drv_twi_tx(&m_twi_instance, address, &reg, 1, false);
	if(err_code != NRF_SUCCESS) {
		printf("err_code = %d @read_register_#1\n\r",err_code);
		return err_code;
	}
	
	if(!waitInt(MPU9250_TWI_TIMEOUT)){// for Check timeout
		printf("Timeout read_reg #1\r\n");
		return NRF_ERROR_TIMEOUT;
	}

	twi_done = false;

	for(unsigned char iii = 0; iii < length; iii++){
		uint8_t * p_tmp;
		err_code = nrf_drv_twi_rx(&m_twi_instance, address, p_tmp, 1);
		if(err_code != NRF_SUCCESS) {
			printf("err_code = %d @read_register_#2\n\r",err_code);
			return err_code;
		}
		
		if(!waitInt(MPU9250_TWI_TIMEOUT)){// for Check timeout
				printf("Timeout read_reg #2\r\n");
				return NRF_ERROR_TIMEOUT;
		}
		p_data[iii] = p_tmp[0];
		twi_done = false;
	}
	return err_code;
}

unsigned short fifo_count;
int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
    unsigned char *more)
{
    unsigned char tmp[2];
    unsigned short fifo_count;
	
		//printf("mpu_read_fifo_stream st@rt\r\n");
    if (!st->chip_cfg.dmp_on){
			printf("mpu_read_fifo_stream return -1\r\n");
			return -1;
			
		}
    if (!st->chip_cfg.sensors){
			printf("mpu_read_fifo_stream return -2\r\n");
      return -2;
		}

		unsigned char tmp1[1];
		unsigned char tmp2[1];
		if (i2c_read(st->hw->addr, st->reg->fifo_count_h, 1, tmp1)){
				printf("mpu_read_fifo_stream return -3\r\n");
        return -3;
		}
		if (i2c_read(st->hw->addr, (st->reg->fifo_count_h)+1, 1, tmp2)){
				printf("mpu_read_fifo_stream return -3\r\n");
        return -3;
		}
		
    fifo_count = (tmp1[0] << 8)| tmp2[0];
		printf("temp %x | %x temp - > fifoCount = %d , length = %d\r\n",tmp1[0],tmp2[0],fifo_count,length);
    if (fifo_count < length) {
		more[0] = 0;
		printf("return 1\r\n");
        return 1;
    }
		
//fifo count > 512
    if (fifo_count > (st->hw->max_fifo >> 1)) {
        /* FIFO is 50% full, better check overflow bit. */
        if (i2c_read(st->hw->addr, st->reg->int_status, 1, tmp)){
					printf("return -5\r\n");	
					return -5;
				}
        if (tmp[0] & BIT_FIFO_OVERFLOW) {
            mpu_reset_fifo();
						printf("return 2\r\n");
            return 2;
        }
    }
// fifo_count > 512

		
    if (i2c_read_fifo(st->hw->addr, st->reg->fifo_r_w, length, data)){
		printf("return -7\r\n");
        return -7;
	}
	for(unsigned char iii = 0;iii<length;iii++){
		printf("%d:%d\r\n",iii,data[iii]);
		delay_ms(10);
	}
    more[0] = fifo_count / length - 1;
	printf("more = %d\r\n",more[0]);
	delay_ms(10);
    return 0;
}

Please tell me how to read data from fifo register?

added 2017/06/01