Modifications, please check if it can be used

[rtek1000]

• Allows selection of gyroscope range
• Allows accelerometer selection range
• Allows selection of device address I2C (0x68 / 0x69)

Modifications at setup:

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin(GYRO_CONFIG_500, ACCEL_CONFIG_16G, MPU6050_ADDR);
  mpu6050.calcGyroOffsets(true);
}

Modifications MPU6050_tockn.h:

#ifndef MPU6050_TOCKN_H
#define MPU6050_TOCKN_H

#include "Arduino.h"
#include "Wire.h"

#define MPU6050_ADDR         0x68
#define MPU6050_ADDR1        0x69
#define MPU6050_SMPLRT_DIV   0x19
#define MPU6050_CONFIG       0x1a
#define MPU6050_GYRO_CONFIG  0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_WHO_AM_I     0x75
#define MPU6050_PWR_MGMT_1   0x6b
#define MPU6050_TEMP_H       0x41
#define MPU6050_TEMP_L       0x42

#define ACCEL_CONFIG_2G      2
#define ACCEL_CONFIG_4G      4
#define ACCEL_CONFIG_8G      8
#define ACCEL_CONFIG_16G     16

#define GYRO_CONFIG_250      250
#define GYRO_CONFIG_500      500
#define GYRO_CONFIG_1000     1000
#define GYRO_CONFIG_2000     2000

class MPU6050{
    public:

    MPU6050(TwoWire &w);
    MPU6050(TwoWire &w, float aC, float gC);

    void begin(int gyros = GYRO_CONFIG_500, int accel = ACCEL_CONFIG_2G, int addr = MPU6050_ADDR);

    void setGyroOffsets(float x, float y, float z);

    void writeMPU6050(byte reg, byte data);
    byte readMPU6050(byte reg);

    int16_t getRawAccX(){ return rawAccX; };
    int16_t getRawAccY(){ return rawAccY; };
    int16_t getRawAccZ(){ return rawAccZ; };

    int16_t getRawTemp(){ return rawTemp; };

    int16_t getRawGyroX(){ return rawGyroX; };
    int16_t getRawGyroY(){ return rawGyroY; };
    int16_t getRawGyroZ(){ return rawGyroZ; };

    float getTemp(){ return temp; };

    float getAccX(){ return accX; };
    float getAccY(){ return accY; };
    float getAccZ(){ return accZ; };

    float getGyroX(){ return gyroX; };
    float getGyroY(){ return gyroY; };
    float getGyroZ(){ return gyroZ; };

    void calcGyroOffsets(bool console = false);

    float getGyroXoffset(){ return gyroXoffset; };
    float getGyroYoffset(){ return gyroYoffset; };
    float getGyroZoffset(){ return gyroZoffset; };

    void update();

    float getAccAngleX(){ return angleAccX; };
    float getAccAngleY(){ return angleAccY; };

    float getGyroAngleX(){ return angleGyroX; };
    float getGyroAngleY(){ return angleGyroY; };
    float getGyroAngleZ(){ return angleGyroZ; };

    float getAngleX(){ return angleX; };
    float getAngleY(){ return angleY; };
    float getAngleZ(){ return angleZ; };

    private:

    TwoWire *wire;

    int16_t rawAccX, rawAccY, rawAccZ, rawTemp,
    rawGyroX, rawGyroY, rawGyroZ, mpuAddr;

    float gyroXoffset, gyroYoffset, gyroZoffset;

    float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;

    float angleGyroX, angleGyroY, angleGyroZ,
    angleAccX, angleAccY, angleAccZ;

    float angleX, angleY, angleZ;

    long interval, preInterval;

    float accCoef, gyroCoef, gyrosFactor, accelFactor,
    gForce, gForceInv;
};

#endif

Modifications MPU6050_tockn.ccp:

#include "MPU6050_tockn.h"
#include "Arduino.h"

MPU6050::MPU6050(TwoWire &w){
    wire = &w;
    accCoef = 0.02f;
    gyroCoef = 0.98f;
}

MPU6050::MPU6050(TwoWire &w, float aC, float gC){
    wire = &w;  
    accCoef = aC;
    gyroCoef = gC;
}

void MPU6050::begin(int gyros, int accel, int addr){
    int gyrosConfig;
    int accelConfig;

    mpuAddr = addr;

    switch (gyros) {
        case 250:
            gyrosConfig = 0x00;
            gyrosFactor = 131.0f;
            break;
        case 500:
            gyrosConfig = 0x08;
            gyrosFactor = 65.5f;
            break;
        case 1000:
            gyrosConfig = 0x10;
            gyrosFactor = 32.8f;
            break;
        case 2000:
            gyrosConfig = 0x18;
            gyrosFactor = 16.4f;
            break;
        default:
            gyrosConfig = 0x08;
            gyrosFactor = 65.5f;
            break;
    }

    switch (accel) {
        case 2:
            accelConfig = 0x00;
            accelFactor = 16384.0f;
            gForce = 2.0f;
            gForceInv = -2.0f;
            break;
        case 4:
            accelConfig = 0x08;
            accelFactor = 8192.0f;
            gForce = 4.0f;
            gForceInv = -4.0f;
            break;
        case 8:
            accelConfig = 0x10;
            accelFactor = 4096.0f;
            gForce = 8.0f;
            gForceInv = -8.0f;
            break;
        case 16:
            accelConfig = 0x18;
            accelFactor = 2048.0f;
            gForce = 16.0f;
            gForceInv = -16.0f;
            break;
        default:
            accelConfig = 0x00;
            accelFactor = 16384.0f;
            gForce = 2.0f;
            gForceInv = -2.0f;
            break;
    }

    writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);
    writeMPU6050(MPU6050_CONFIG, 0x00);
    writeMPU6050(MPU6050_GYRO_CONFIG, gyrosConfig);
    writeMPU6050(MPU6050_ACCEL_CONFIG, accelConfig); // 0x18 = 16G | 0x00 = 2G
    writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);
    this->update();
    angleGyroX = this->getAccAngleX();
    angleGyroY = this->getAccAngleY();
    Serial.println("=================");
    Serial.println(accCoef);
    Serial.println(gyroCoef);
}

void MPU6050::writeMPU6050(byte reg, byte data){
    wire->beginTransmission(mpuAddr);
    wire->write(reg);
    wire->write(data);
    wire->endTransmission();
}

byte MPU6050::readMPU6050(byte reg) {
    wire->beginTransmission(mpuAddr);
    wire->write(reg);
    wire->endTransmission(true);
    wire->requestFrom((uint8_t)mpuAddr, (size_t)2/*length*/);
    byte data =  wire->read();
    wire->read();
    return data;
}

void MPU6050::setGyroOffsets(float x, float y, float z){
    gyroXoffset = x;
    gyroYoffset = y;
    gyroZoffset = z;
}

void MPU6050::calcGyroOffsets(bool console){
    float x = 0, y = 0, z = 0;
    int16_t rx, ry, rz;

    if(console){
        Serial.println("calculate gyro offsets");
        Serial.print("DO NOT MOVE A MPU6050");
    }
    for(int i = 0; i < 5000; i++){
        if(console && i % 1000 == 0){
            Serial.print(".");
        }
        wire->beginTransmission(mpuAddr);
        wire->write(0x43); // from GYRO_XOUT_H to GYRO_ZOUT_L
        wire->endTransmission(false);
        wire->requestFrom((int)mpuAddr, 6, (int)true);

        rx = wire->read() << 8 | wire->read(); // GYRO_XOUT_H | GYRO_XOUT_L
        ry = wire->read() << 8 | wire->read();
        rz = wire->read() << 8 | wire->read(); // GYRO_ZOUT_H | GYRO_ZOUT_L

        x += ((float)rx) / gyrosFactor; // 500: 65.5
        y += ((float)ry) / gyrosFactor;
        z += ((float)rz) / gyrosFactor;
    }
    gyroXoffset = x / 5000;
    gyroYoffset = y / 5000;
    gyroZoffset = z / 5000;

    if(console){
        Serial.println("Done!!!");
        Serial.print("X : ");Serial.println(gyroXoffset);
        Serial.print("Y : ");Serial.println(gyroYoffset);
        Serial.print("Z : ");Serial.println(gyroYoffset);
        Serial.print("Program will start after 3 seconds");
        delay(3000);
    }
}

void MPU6050::update(){
    wire->beginTransmission(mpuAddr);
    wire->write(0x3B); // from ACCEL_XOUT_H  to GYRO_ZOUT_L
    wire->endTransmission(false);
    wire->requestFrom((int)mpuAddr, 14, (int)true);

    rawAccX = wire->read() << 8 | wire->read(); // ACCEL_XOUT_H | ACCEL_XOUT_L
    rawAccY = wire->read() << 8 | wire->read();
    rawAccZ = wire->read() << 8 | wire->read();
    rawTemp = wire->read() << 8 | wire->read();
    rawGyroX = wire->read() << 8 | wire->read();
    rawGyroY = wire->read() << 8 | wire->read();
    rawGyroZ = wire->read() << 8 | wire->read(); // GYRO_ZOUT_H | GYRO_ZOUT_L

    temp = (rawTemp + 12412.0) / 340.0;

    accX = ((float)rawAccX) / accelFactor; // 2G: 16384 | 16G: 2048
    accY = ((float)rawAccY) / accelFactor;
    accZ = ((float)rawAccZ) / accelFactor;

    angleAccX = atan2(accY, accZ + abs(accX)) * 360 / gForce / PI;    // 2.0
    angleAccY = atan2(accX, accZ + abs(accY)) * 360 / gForceInv / PI; // -2.0

    gyroX = ((float)rawGyroX) / gyrosFactor;
    gyroY = ((float)rawGyroY) / gyrosFactor;
    gyroZ = ((float)rawGyroZ) / gyrosFactor;

    interval = millis() - preInterval;

    gyroX -= gyroXoffset;
    gyroY -= gyroYoffset;
    gyroZ -= gyroZoffset;

    angleGyroX += gyroX * (interval * 0.001);
    angleGyroY += gyroY * (interval * 0.001);
    angleGyroZ += gyroZ * (interval * 0.001);

    preInterval = millis();

    angleX = (gyroCoef * angleGyroX) + (accCoef * angleAccX);
    angleY = (gyroCoef * angleGyroY) + (accCoef * angleAccY);
    angleZ = angleGyroZ;
}

[tockn]

Thanks!
I wanna merge your code after fix some bugs.
Could you send me a Pull Request?

[artsrunimargaryanfd]

in calcGyroOffsets function why did you read from 0x3B 14bytes, as I see you need only gyroscope data so you can read from 0x43 6 bytes.

[tormes]

In file MPU6050_tockn.cpp the first switch (accel) should be switch (gyros) .

[rtek1000]

In file MPU6050_tockn.cpp the first switch (accel) should be switch (gyros) .

Done! Thank you!

[rtek1000]

in calcGyroOffsets function why did you read from 0x3B 14bytes, as I see you need only gyroscope data so you can read from 0x43 6 bytes.

Let's do this, according to the record map, you're correct!

Thank you!

https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf

MPU-6000-Register-Map1.pdf

[rtek1000]

Thanks! I wanna merge your code after fix some bugs. Could you send me a Pull Request?

Sorry for the delay my friend,

I made some corrections as it was suggested,

If I have time again, I will try to learn how to do a pull request,

Thank you!

[ghost]

Just made a similar patch (see pull requests).

[AchimSto]

Thank you ver much fpr extending the files for using two sensors at once on I"C bus. Unfortunately a mini test program compiles perect but the upload stops near the end.

Are there any tweaks neccessary in the modified libraries or where can I get actual libraries which are tested with two sensors used?

Uups! Sorry! - The Flash Issue is resolved: AVR-Dude doesn't like "!" .... I added a few and removed them .... I try to get two sensors run in parallel. I hope it'll work as well as id does with one sensor only! - Thanky you very, very much for your library!