How to read fifo data in TWI?

[Sagisawa Yukichi]

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

[MartinBL]

I haven't used the FIFO in the MPU, so I'm not sure how it works. The way I understand it though, is that you are supposed to read the entire buffer out of the "FIFO Read/Write" register (0x74). So after reading one byte, a new byte from the FIFO is latched into the FIFO Read/Write, and so on, until you have read out all the data. In your mpu9250_read_fifo_register() function however, it looks like you start reading at a certain address (I can only assume it is correct since it doesn't say in your code) and then you continue reading one by one byte in a for() loop. However, if you do it this way I believe you increment the register number your are reading from on every read operation. You have to tell the MPU to read from the same address every time. Otherwise you will read a bunch of sequential registers, which I don't think is what you want to do.