问题描述
我正在尝试使用Arduino Wire库编写一个类来控制mpu6050，但是当我在Arduino mini中运行代码时，它会在几秒钟后冻结。
// 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& gz)
{
int raw_x,raw_y,raw_z;
getRawGyroData(raw_x,raw_z);
gx = convertGyroToDPS(raw_x);
gy = convertGyroToDPS(raw_y);
gz = convertGyroToDPS(raw_z);
}
void mpu6050::getRawGyroData (int& gx,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* 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_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);
}
}
使用Serial.println进行的某些测试指向Wire库中的function requestFrom。可能是什么原因？
解决方法
对不起，我将其写为答案，但还不能发表评论。
1st。您的代码中有多个requestFrom（）调用，因此最好指定问题发生的确切位置（如果可以）。
第二。您完全确定这是代码挂起的requestFrom（）。在readData（）中，在requestFrom（）之后有一个while（）。也许它挂在那里，因为另一台设备发送的字节数不够（出于某些原因）。
无论如何这可能会帮助一小撮（link），在这里他们建议始终检查endTransmission（）的返回值。