Merge pull request #248 from mp-se/dev

Merge latest beta 3 changes

Author: mp-se

bin/custom-olimex_esp32c3_devkit_lipo.bin

@@ -3321,7 +3321,11 @@ void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 	if (ReadAddress == 0x3B) gravity = 16384 >> getFullScaleAccelRange();
 	Serial.write('>');
 	for (int i = 0; i < 3; i++) {
-		I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj); // reads 1 or more 16 bit integers (Word)
+		if (I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj) != 1) {
+			Serial.print(F("I2C read error at "));
+			Serial.println(i);
+			return;
+		}
 		Reading = Data;
 		if(SaveAddress != 0x13){
 			BitZero[i] = Data & 1;										 // Capture Bit Zero to properly handle Accelerometer calibration
@@ -3335,7 +3339,11 @@ void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 		for (int c = 0; c < 100; c++) {// 100 PI Calculations
 			eSum = 0;
 			for (int i = 0; i < 3; i++) {
-				I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj); // reads 1 or more 16 bit integers (Word)
+				if (I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj) != 1) {
+					Serial.print(F("I2C read error, PID, axis "));
+					Serial.println(i);
+					return;
+				}
 				Reading = Data;
 				if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= gravity;	//remove Gravity
 				Error = -Reading;
@@ -3346,7 +3354,11 @@ void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 					Data = round((PTerm + ITerm[i] ) / 8);		//Compute PID Output
 					Data = ((Data)&0xFFFE) |BitZero[i];			// Insert Bit0 Saved at beginning
 				} else Data = round((PTerm + ITerm[i] ) / 4);	//Compute PID Output
-				I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, wireObj);
+				if (I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, wireObj) != 1) {
+					Serial.print(F("I2C write error, PID, axis "));
+					Serial.println(i);
+					return;
+				}
 			}
 			if((c == 99) && eSum > 1000){						// Error is still to great to continue 
 				c = 0;
@@ -3371,27 +3383,37 @@ void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 	resetDMP();
 }
 
-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");
+    uint8_t deviceID = getDeviceID();
+    uint8_t AOffsetRegister = (deviceID < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77;
+    int16_t accelOffsets[3], gyroOffsets[3];
+
+    Serial.print(F("who: 0x"));
+    Serial.print(deviceID, HEX);
+
+    if(AOffsetRegister == 0x06) {
+        I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)accelOffsets, I2Cdev::readTimeout, wireObj);
+    } else {
+        I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)accelOffsets, I2Cdev::readTimeout, wireObj);
+        I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)(accelOffsets+1), I2Cdev::readTimeout, wireObj);
+        I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)(accelOffsets+2), I2Cdev::readTimeout, wireObj);
+    }
+
+    Serial.print(F(", accel: "));
+    Serial.print((float)accelOffsets[0], 5); 
+    Serial.print(F(", "));
+    Serial.print((float)accelOffsets[1], 5); 
+    Serial.print(F(", "));
+    Serial.print((float)accelOffsets[2], 5); 
+
+    I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)gyroOffsets, I2Cdev::readTimeout, wireObj);
+
+    Serial.print(F(", gyro: "));
+    Serial.print((float)gyroOffsets[0], 5); 
+    Serial.print(F(", "));
+    Serial.print((float)gyroOffsets[1], 5); 
+    Serial.print(F(", "));
+    Serial.print((float)gyroOffsets[2], 5); 
+    Serial.print("\n");
 }

bin/custom-tenstar_esp32c3_super_mini.bin

@@ -832,7 +832,6 @@ 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;
@@ -841,7 +840,6 @@ class MPU6050_Base {
         uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT;
 
     private:
-        int16_t offsets[6];
 };
 
 #ifndef I2CDEVLIB_MPU6050_TYPEDEF