diff --git a/lib/mpu6050/I2Cdev.cpp b/lib/mpu6050/I2Cdev.cpp index 892274b..173f2ac 100644 --- a/lib/mpu6050/I2Cdev.cpp +++ b/lib/mpu6050/I2Cdev.cpp @@ -282,7 +282,8 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8 useWire->beginTransmission(devAddr); useWire->write(regAddr); useWire->endTransmission(); - useWire->beginTransmission(devAddr); + // See: https://github.com/espressif/arduino-esp32/issues/6674 + // useWire->beginTransmission(devAddr); useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { @@ -1485,4 +1486,4 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; return rxBufferLength - rxBufferIndex; } -#endif +#endif \ No newline at end of file diff --git a/lib/mpu6050/I2Cdev.h b/lib/mpu6050/I2Cdev.h index 5b59c56..c4fe7f5 100644 --- a/lib/mpu6050/I2Cdev.h +++ b/lib/mpu6050/I2Cdev.h @@ -308,4 +308,4 @@ class I2Cdev { #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE -#endif /* _I2CDEV_H_ */ +#endif /* _I2CDEV_H_ */ \ No newline at end of file diff --git a/lib/mpu6050/MPU6050.cpp b/lib/mpu6050/MPU6050.cpp index 4467c12..16784ac 100644 --- a/lib/mpu6050/MPU6050.cpp +++ b/lib/mpu6050/MPU6050.cpp @@ -66,7 +66,7 @@ void MPU6050_Base::initialize() { * @return True if connection is valid, false otherwise */ bool MPU6050_Base::testConnection() { - return getDeviceID() == 0x34 || getDeviceID() == 0x38; // Allow both MPU6050 and MPU6000 + return getDeviceID() == 0x34; } // AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC) @@ -2556,8 +2556,8 @@ void MPU6050_Base::setClockSource(uint8_t source) { * -------------+------------------ * 0 | 1.25 Hz * 1 | 2.5 Hz - * 2 | 5 Hz - * 3 | 10 Hz + * 2 | 20 Hz + * 3 | 40 Hz * * * For further information regarding the MPU-60X0's power modes, please refer to @@ -3371,25 +3371,27 @@ void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){ resetDMP(); } -void MPU6050_Base::PrintActiveOffsets() { - uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77; - int16_t Data[3]; - //Serial.print(F("Offset Register 0x")); - //Serial.print(AOffsetRegister>>4,HEX);Serial.print(AOffsetRegister&0x0F,HEX); - Serial.print(F("\n// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro\n// OFFSETS ")); - if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data, I2Cdev::readTimeout, wireObj); - else { - I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data, I2Cdev::readTimeout, wireObj); - I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1, I2Cdev::readTimeout, wireObj); - I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2, I2Cdev::readTimeout, wireObj); - } - // A_OFFSET_H_READ_A_OFFS(Data); - Serial.print((float)Data[0], 5); Serial.print(", "); - Serial.print((float)Data[1], 5); Serial.print(", "); - Serial.print((float)Data[2], 5); Serial.print(", "); - I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data, I2Cdev::readTimeout, wireObj); - // XG_OFFSET_H_READ_OFFS_USR(Data); - Serial.print((float)Data[0], 5); Serial.print(", "); - Serial.print((float)Data[1], 5); Serial.print(", "); - Serial.print((float)Data[2], 5); Serial.print("\n"); +int16_t * MPU6050_Base::GetActiveOffsets() { + uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77; + if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)offsets, I2Cdev::readTimeout, wireObj); + else { + I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)offsets, I2Cdev::readTimeout, wireObj); + I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)(offsets+1), I2Cdev::readTimeout, wireObj); + I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)(offsets+2), I2Cdev::readTimeout, wireObj); + } + I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)(offsets+3), I2Cdev::readTimeout, wireObj); + return offsets; } + +void MPU6050_Base::PrintActiveOffsets() { + GetActiveOffsets(); + // A_OFFSET_H_READ_A_OFFS(Data); + Serial.print((float)offsets[0], 5); Serial.print(",\t"); + Serial.print((float)offsets[1], 5); Serial.print(",\t"); + Serial.print((float)offsets[2], 5); Serial.print(",\t"); + + // XG_OFFSET_H_READ_OFFS_USR(Data); + Serial.print((float)offsets[3], 5); Serial.print(",\t"); + Serial.print((float)offsets[4], 5); Serial.print(",\t"); + Serial.print((float)offsets[5], 5); Serial.print("\n\n"); +} \ No newline at end of file diff --git a/lib/mpu6050/MPU6050.h b/lib/mpu6050/MPU6050.h index 888d4db..041963e 100644 --- a/lib/mpu6050/MPU6050.h +++ b/lib/mpu6050/MPU6050.h @@ -832,12 +832,16 @@ class MPU6050_Base { void CalibrateAccel(uint8_t Loops = 15);// Fine tune after setting offsets with less Loops. void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops); // Does the math void PrintActiveOffsets(); // See the results of the Calibration + int16_t * GetActiveOffsets(); protected: uint8_t devAddr; void *wireObj; uint8_t buffer[14]; uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT; + + private: + int16_t offsets[6]; }; #ifndef I2CDEVLIB_MPU6050_TYPEDEF @@ -845,4 +849,4 @@ class MPU6050_Base { typedef MPU6050_Base MPU6050; #endif -#endif /* _MPU6050_H_ */ +#endif /* _MPU6050_H_ */ \ No newline at end of file diff --git a/src/gyro.cpp b/src/gyro.cpp index 86354bd..4feb598 100644 --- a/src/gyro.cpp +++ b/src/gyro.cpp @@ -43,7 +43,9 @@ bool GyroSensor::setup() { Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having // compilation difficulties - if (!accelgyro.testConnection()) { + uint8_t id = accelgyro.getDeviceID(); + + if (id != 0x34 && id != 0x38) { // Allow both MPU6050 and MPU6000 ErrorFileLog errLog; errLog.addEntry(F("GYRO: Failed to connect to gyro, is it connected?")); _sensorConnected = false;