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

3
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->beginTransmission(devAddr);
useWire->write(regAddr); useWire->write(regAddr);
useWire->endTransmission(); 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)); useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) {

44
lib/mpu6050/MPU6050.cpp
View File

@ -66,7 +66,7 @@ void MPU6050_Base::initialize() {
* @return True if connection is valid, false otherwise * @return True if connection is valid, false otherwise
*/ */
bool MPU6050_Base::testConnection() { 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) // 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 * 0 | 1.25 Hz
* 1 | 2.5 Hz * 1 | 2.5 Hz
* 2 | 5 Hz * 2 | 20 Hz
* 3 | 10 Hz * 3 | 40 Hz
* </pre> * </pre>
* *
* For further information regarding the MPU-60X0's power modes, please refer to * 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(); resetDMP();
} }
void MPU6050_Base::PrintActiveOffsets() { int16_t * MPU6050_Base::GetActiveOffsets() {
uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77; uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77;
int16_t Data[3]; if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)offsets, I2Cdev::readTimeout, wireObj);
//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 { else {
I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data, I2Cdev::readTimeout, wireObj); I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)offsets, I2Cdev::readTimeout, wireObj);
I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1, I2Cdev::readTimeout, wireObj); I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)(offsets+1), I2Cdev::readTimeout, wireObj);
I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2, I2Cdev::readTimeout, wireObj); I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)(offsets+2), I2Cdev::readTimeout, wireObj);
} }
// A_OFFSET_H_READ_A_OFFS(Data); I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)(offsets+3), I2Cdev::readTimeout, wireObj);
Serial.print((float)Data[0], 5); Serial.print(", "); return offsets;
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); void MPU6050_Base::PrintActiveOffsets() {
// XG_OFFSET_H_READ_OFFS_USR(Data); GetActiveOffsets();
Serial.print((float)Data[0], 5); Serial.print(", "); // A_OFFSET_H_READ_A_OFFS(Data);
Serial.print((float)Data[1], 5); Serial.print(", "); Serial.print((float)offsets[0], 5); Serial.print(",\t");
Serial.print((float)Data[2], 5); Serial.print("\n"); 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");
} }

4
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 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 PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops); // Does the math
void PrintActiveOffsets(); // See the results of the Calibration void PrintActiveOffsets(); // See the results of the Calibration
int16_t * GetActiveOffsets();
protected: protected:
uint8_t devAddr; uint8_t devAddr;
void *wireObj; void *wireObj;
uint8_t buffer[14]; uint8_t buffer[14];
uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT; uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT;
private:
int16_t offsets[6];
}; };
#ifndef I2CDEVLIB_MPU6050_TYPEDEF #ifndef I2CDEVLIB_MPU6050_TYPEDEF

4
src/gyro.cpp
View File

@ -43,7 +43,9 @@ bool GyroSensor::setup() {
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having
// compilation difficulties // compilation difficulties
if (!accelgyro.testConnection()) { uint8_t id = accelgyro.getDeviceID();
if (id != 0x34 && id != 0x38) { // Allow both MPU6050 and MPU6000
ErrorFileLog errLog; ErrorFileLog errLog;
errLog.addEntry(F("GYRO: Failed to connect to gyro, is it connected?")); errLog.addEntry(F("GYRO: Failed to connect to gyro, is it connected?"));
_sensorConnected = false; _sensorConnected = false;