56 lines
1.7 KiB
C++
56 lines
1.7 KiB
C++
// A very simple implementation of the MPU-6050 limited to the
|
|
// TWANG requirements
|
|
// I reused the function names to make it compatible
|
|
// B. Dring 2/2018
|
|
|
|
class Twang_MPU
|
|
{
|
|
|
|
public:
|
|
void initialize();
|
|
void getMotion6(int16_t* xAccel, int16_t* yAccel, int16_t* zAccel, int16_t* xGyro, int16_t* yGyro, int16_t* zGyro);
|
|
bool verify();
|
|
|
|
private:
|
|
static const uint8_t MPU_ADDR = 0x68;
|
|
static const uint8_t PWR_MGMT_1 = 0x6B;
|
|
static const uint8_t MPU_DATA_REG_START = 0x3B;
|
|
static const uint8_t MPU_DATA_LEN = 14;
|
|
static const uint8_t MPU_DATA_WHO_AM_I = 0x75;
|
|
|
|
};
|
|
|
|
void Twang_MPU::initialize()
|
|
{
|
|
Wire.beginTransmission(MPU_ADDR);
|
|
Wire.write(PWR_MGMT_1); // PWR_MGMT_1 register
|
|
Wire.write(1); // wakes up the MPU-6050
|
|
Wire.endTransmission(true);
|
|
}
|
|
|
|
bool Twang_MPU::verify()
|
|
{
|
|
Wire.beginTransmission(MPU_ADDR);
|
|
Wire.write(MPU_DATA_WHO_AM_I);
|
|
Wire.endTransmission(false);
|
|
Wire.requestFrom(MPU_ADDR,1,true); // read the whole MPU data section
|
|
return (Wire.read() == MPU_ADDR);
|
|
}
|
|
|
|
void Twang_MPU::getMotion6(int16_t* xAccel, int16_t* yAccel, int16_t* zAccel, int16_t* xGyro, int16_t* yGyro, int16_t* zGyro)
|
|
{
|
|
|
|
Wire.beginTransmission(MPU_ADDR);
|
|
Wire.write(MPU_DATA_REG_START); // starting with register 0x3B (ACCEL_XOUT_H)
|
|
Wire.endTransmission(false);
|
|
Wire.requestFrom(MPU_ADDR,MPU_DATA_LEN,true); // read the whole MPU data section
|
|
*xAccel=Wire.read()<<8|Wire.read(); // x Accel
|
|
*yAccel=Wire.read()<<8|Wire.read(); // y Accel
|
|
*zAccel=Wire.read()<<8|Wire.read(); // z Accel
|
|
Wire.read(); Wire.read(); // Temperature..not used, but need to read it
|
|
*xGyro=Wire.read()<<8|Wire.read(); // x Gyro
|
|
*yGyro=Wire.read()<<8|Wire.read(); // y Gyro
|
|
*zGyro=Wire.read()<<8|Wire.read(); // z Gyro
|
|
}
|
|
|