Problem with continues reading FIFO
williamesp2015 opened this issue · comments
I want to read FIFO continuously. So I copied calibrateMPU6050 in a new function and in the loop I wait for 80 msec and then read fifo_count but I always one data packet.
void setup(){
......
uint8_t c = mpu.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
Serial.print("I AM ");
Serial.print(c, HEX);
Serial.print(" I Should Be ");
Serial.println(0x68, HEX);
// my mpu6050 return 0x98
if((c == 0x68) || (c==0x98)){
{
Serial.println("MPU6050 is online...");
}
mpu.InitialteFIFO();
LastRead=millis();
}
}
while((millis()-LastRead)<80)// I already enabled FIFO in the InitializeByFIFO()
{
delayMicroseconds(50);
}
LastRead=millis();
fifo_count=0;
while(fifo_count<512)// I already enabled FIFO in the InitializeByFIFO()
{
// At end of sample accumulation, turn off FIFO sensor read
mpu.writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
mpu.readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
fifo_count = ((uint16_t)data[0] << 8) | data[1];
}
packet_count = fifo_count / 12; // How many sets of full gyro and accelerometer data for averaging
Serial.println("");
Serial.print(" FC:");
Serial.println( fifo_count);
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
mpu.readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
Serial.print(" X:");
Serial.print(accel_temp[0]); // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
Serial.print(" Y:");
Serial.print(accel_temp[1]);
Serial.print(" Z:");
Serial.print(accel_temp[2]);
Serial.print(" GX:");
Serial.print(gyro_temp[0]);
Serial.print(" GY:");
Serial.print(gyro_temp[1]);
Serial.print(" GZ:");
Serial.println(gyro_temp[2]);
}
}
In the MPU6050.cpp
void MPU6050lib::InitialteFIFO()
{
// reset device, reset all registers, clear gyro and accelerometer bias registers
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
delay(100);
// get stable time source
// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);
writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00);
delay(200);
// Configure device for bias calculation
writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
delay(15);
// Configure MPU6050 gyro and accelerometer for bias calculation
writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
uint16_t accelsensitivity = 16384; // = 16384 LSB/g
// Configure Interrupts and Bypass Enable
// Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
// can join the I2C bus and all can be controlled by the Arduino as master
writeByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x22);
writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
delay(15);
// Configure FIFO to capture accelerometer and gyro data for bias calculation
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 1024 bytes in MPU-6050)
}
I did changes and now its reading FIFO continuously but I need you to check the Acc, Gyro, DLF and sampling rate to be correct and add to your examples:
void MPU6050lib::InitialteFIFO(uint8_t _aFSR,uint8_t _gFSR,uint8_t _filter,uint8_t _sampleRateDiv)
{
uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
// reset device, reset all registers, clear gyro and accelerometer bias registers
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
delay(100);
// get stable time source
// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);
writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00);
delay(200);
// Configure device for bias calculation
writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
delay(15);
// Configure MPU6050 gyro and accelerometer for bias calculation
writeByte(MPU6050_ADDRESS, CONFIG, _filter); // Set low-pass filter to 188 Hz=0x01
writeByte(MPU6050_ADDRESS, SMPLRT_DIV, _sampleRateDiv); // Set sample rate to 1 kHz=0x00
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, _gFSR); // Set gyro full-scale to 250 degrees per second=0x00, maximum sensitivity
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, _aFSR); // Set accelerometer full-scale to 2 g=0x00, maximum sensitivity
delay(15);
writeByte(MPU6050_ADDRESS, FIFO_EN, 0x88);
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x44); // Enable FIFO
}
void setup()
{
.....
mpu.InitialteFIFO(0,0,0x01,0);
LastRead=millis();
}
void loop()
{
while((millis()-LastRead)<79)//480 fifo_count =<79 I already enabled FIFO in the InitializeByFIFO()
{
delayMicroseconds(50);
}
LastRead=millis();
mpu.writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO mpu.readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
fifo_count = ((uint16_t)data[0] << 8) | data[1];
packet_count = fifo_count / 12; // How many sets of full gyro and accelerometer data for averaging
Serial.print(" FC:");
Serial.println( fifo_count);
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
mpu.readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ;
accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
printtheResults();
}
fifo_count=0;
mpu.writeByte(MPU6050_ADDRESS, FIFO_EN, 0x88);
mpu.writeByte(MPU6050_ADDRESS, USER_CTRL, 0x44);
}