Arduino Wire requestFrom freezes - c++

Question

I'm trying to program a class to control the MPU6050 with the Arduino Wire library but when I run the code in my Arduino mini it freezes after a few seconds.
There is the code of the library and a test sketch:
// Include Wire Library for I2C
#include <Wire.h>
enum MPU6050_filter {_256Hz, _188Hz, _98Hz, _42Hz, _20Hz, _10Hz, _5Hz};
enum MPU6050_gyro {_250dps, _500dps, _1000dps, _2000dps};
enum MPU6050_accel {_2g, _4g, _8g, _16Hz};
class MPU6050
{
public:
MPU6050 ();
bool start (bool AD0_value);
void goToSleep ();
void stopSleeping ();
void setFilterVal (MPU6050_filter filter_val);
void setGyroRange (MPU6050_gyro range);
void setAccelRange (MPU6050_accel range);
bool dataAvailable ();
void getLastGyroData (float& gx, float& gy, float& gz);
void getRawGyroData (int& gx, int& gy, int& gz);
private:
void writeRegister (byte address, byte data);
byte readRegister (byte address);
void readData (byte start_address, byte bytes, byte* data);
float convertGyroToDPS (int gyro);
bool AD0_val;
MPU6050_filter filter;
MPU6050_accel accel_range;
MPU6050_gyro gyro_range;
unsigned long last_read;
const unsigned long min_read_time = 1;
};
MPU6050::MPU6050 () : AD0_val(false),
filter(_256Hz),
accel_range(_2g),
gyro_range(_250dps) {}
bool MPU6050::start (bool AD0_value)
{
AD0_val = AD0_value;
// init sample rate div to 0 (max sample rate)
writeRegister(0x19, 0);
// activate FIFO for gyroscope data
writeRegister(0x23, 0x70);
// clear config setup register
writeRegister(0x6B, 0);
// setup the register
writeRegister(0x37, 0x10);
// set interrupt by data ready
writeRegister(0x38, 0x01);
}
void MPU6050::goToSleep ()
{
byte prev_data = readRegister(0x6B);
prev_data = (prev_data | 0x40);
writeRegister(0x6B, prev_data);
}
void MPU6050::stopSleeping ()
{
byte prev_data = readRegister(0x6B);
prev_data = (prev_data & 0xBF);
writeRegister(0x6B, prev_data);
}
void MPU6050::setFilterVal (MPU6050_filter filter_val)
{
int val;
if (filter_val == _256Hz) val = 0;
else if (filter_val == _188Hz) val = 1;
else if (filter_val == _98Hz) val = 2;
else if (filter_val == _42Hz) val = 3;
else if (filter_val == _20Hz) val = 4;
else if (filter_val == _10Hz) val = 5;
else val = 6;
byte data = readRegister(0x1A);
data = (data & 0xF8) | (val & 0x07);
writeRegister(0x1A, data);
filter = filter_val;
}
void MPU6050::setAccelRange (MPU6050_accel range)
{
byte value;
if (range == _2g) value = 0;
else if (range == _4g) value = 1;
else if (range == _8g) value = 2;
else value = 3;
byte reg_value = readRegister(0x1C);
reg_value = (reg_value & 0xE0) | (value << 3);
writeRegister(0x1C, reg_value);
accel_range = range;
}
void MPU6050::setGyroRange (MPU6050_gyro range)
{
byte value;
if (range == _250dps) value = 0;
else if (range == _500dps) value = 1;
else if (range == _1000dps) value = 2;
else value = 3;
byte reg_value = readRegister(0x1B);
reg_value = (reg_value & 0xE0) | (value << 3);
writeRegister(0x1B, reg_value);
gyro_range = range;
}
bool MPU6050::dataAvailable ()
{
return (readRegister(0x3A) & 0x01);
}
void MPU6050::getLastGyroData (float& gx, float& gy, float& gz)
{
int raw_x, raw_y, raw_z;
getRawGyroData(raw_x, raw_y, raw_z);
gx = convertGyroToDPS(raw_x);
gy = convertGyroToDPS(raw_y);
gz = convertGyroToDPS(raw_z);
}
void MPU6050::getRawGyroData (int& gx, int& gy, int& gz)
{
byte* data = new byte[6];
readData(0x43, 6, data);
gx = data[0] << 8 | data[1];
gy = data[2] << 8 | data[3];
gz = data[4] << 8 | data[5];
delete data;
}
void MPU6050::writeRegister (byte address, byte data)
{
Wire.beginTransmission(0x68 + AD0_val);
Wire.write(address);
Wire.write(data);
Wire.endTransmission();
}
byte MPU6050::readRegister (byte address)
{
byte data_buff = 0x00;
Wire.beginTransmission(byte(0x68 + AD0_val));
//Send the requested starting register
Wire.write(address);
//End the transmission
Wire.endTransmission(false);
//Request 14 bytes from the MPU-6050
Wire.requestFrom(byte(0x68 + AD0_val), byte(0x01), byte(true));
unsigned long initial_time = millis();
//Wait until all the bytes are received
while(Wire.available() == 0 and millis() < initial_time + 5);
if (millis() < initial_time + 5)
{
// read the data
data_buff = Wire.read();
}
// end the transmission
Wire.endTransmission();
return data_buff;
}
void MPU6050::readData (byte start_address, byte bytes, byte* data)
{
Wire.beginTransmission(byte(0x68 + AD0_val));
//Send the requested starting register
Wire.write(start_address);
//End the transmission
Wire.endTransmission(false);
//Request 14 bytes from the MPU-6050
Wire.requestFrom(byte(0x68 + AD0_val), bytes, byte(true));
//Wait until all the bytes are received
while(Wire.available() < bytes);
for (int i = 0; i < bytes; i++)
data[i] = Wire.read();
Wire.endTransmission();
}
float MPU6050::convertGyroToDPS (int gyro)
{
if (gyro_range == _250dps) return float(gyro)/131.0;
else if (gyro_range == _500dps) return float(gyro)/65.5;
else if (gyro_range == _1000dps) return float(gyro)/32.8;
else return float(gyro)/16.4;
}
#define SHOW_EACH 50
MPU6050 chip;
unsigned long last_shown = 0;
unsigned this_fps = 0;
unsigned last_fps = 0;
unsigned last_time = 0;
unsigned total_fps = 0;
float g_x, g_y, g_z;
void setup()
{
Serial.begin(115200);
Serial.println("--------");
chip.setFilterVal(_256Hz);
chip.setGyroRange(_250dps);
chip.start(false);
}
void loop()
{
if (chip.dataAvailable())
chip.getLastGyroData(g_x, g_y, g_z);
++this_fps;
++total_fps;
if (millis()/1000 != last_time)
{
last_time = millis()/1000;
last_fps = this_fps;
this_fps = 0;
}
if (millis() - last_shown >= SHOW_EACH)
{
last_shown = millis();
Serial.print(g_x);
Serial.print(" ");
Serial.print(g_y);
Serial.print(" ");
Serial.print(g_y);
Serial.print(" ");
Serial.print(last_fps);
Serial.print(" ");
Serial.println(total_fps);
}
}
Some testing with Serial.println points to the function requestFrom from the Wire library. What can be the cause?

Answer 1

Sorry that i write this as an answer, but I can't write comments yet.
1st. There are multiple requestFrom() calls in your code, so it would be better to specify where does the problem occure exactly (if you can).
2nd. Are you completely sure that, it's the requestFrom() where your code hang. In readData() there is a while() just after requestFrom(). Maybe it hangs there, as the other device don't send enough bytes (for some reasons).
Anyway this might help a litle (link), here they recommend to always check the return value of endTransmission().