Brewery/gravitymon
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Updated mpu libraries

Browse Source
This commit is contained in:
Magnus Persson 2022-07-05 17:50:33 +02:00
parent c503ad88a9
commit 50257e2805
5 changed files with 38 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
lib/mpu6050/I2Cdev.cpp
View File

@ -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

2
lib/mpu6050/I2Cdev.h
View File

@ -308,4 +308,4 @@ class I2Cdev {
#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
#endif /* _I2CDEV_H_ */
#endif /* _I2CDEV_H_ */

50
lib/mpu6050/MPU6050.cpp
View File

@ -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
* </pre>
*
* 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");
}

6
lib/mpu6050/MPU6050.h
View File

@ -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_ */

4
src/gyro.cpp
View File

@ -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;