Merge pull request jrowberg#462 from ZHomeSlice/master

Undefined Variable Reading Fixed Improved Calibration Performance
hrenfroe · Jul 28, 2019 · cca5c3a · cca5c3a
2 parents 97c7039 + 56bd088
commit cca5c3a
Showing 1 changed file with 3 additions and 11 deletions.
diff --git a/Arduino/MPU6050/MPU6050.cpp b/Arduino/MPU6050/MPU6050.cpp
@@ -3218,9 +3218,7 @@ void MPU6050::setDMPConfig2(uint8_t config) {
 /**
   @brief      Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings
 */
-
-void MPU6050::CalibrateGyro(uint8_t Loops) {
-
+void MPU6050::CalibrateGyro(uint8_t Loops ) {
   double kP = 0.3;
   double kI = 90;
   float x;
@@ -3234,7 +3232,6 @@ void MPU6050::CalibrateGyro(uint8_t Loops) {
 /**
   @brief      Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings
 */
-
 void MPU6050::CalibrateAccel(uint8_t Loops ) {
 
 	float kP = 0.3;
@@ -3244,7 +3241,6 @@ void MPU6050::CalibrateAccel(uint8_t Loops ) {
 	kP *= x;
 	kI *= x;
 	PID( 0x3B, kP, kI,  Loops);
-
 }
 
 void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
@@ -3260,6 +3256,7 @@ void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 	Serial.write('>');
 	for (int i = 0; i < 3; i++) {
 		I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, &Data); // reads 1 or more 16 bit integers (Word)
+		Reading = Data;
 		if(SaveAddress != 0x13){
 			BitZero[i] = Data & 1;										 // Capture Bit Zero to properly handle Accelerometer calibration
 			ITerm[i] = ((float)Reading) * 8;
@@ -3272,7 +3269,6 @@ void MPU6050::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, &Data); // reads 1 or more 16 bit integers (Word)
 				Reading = Data;
 				if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384;	//remove Gravity
@@ -3289,7 +3285,6 @@ void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 			if((c == 99) && eSum > 1000){						// Error is still to great to continue 
 				c = 0;
 				Serial.write('*');
-
 			}
 			if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++;	// Successfully found offsets prepare to  advance
 			if((eSum < 100) && (c > 10) && (eSample >= 10)) break;		// Advance to next Loop
@@ -3311,7 +3306,6 @@ void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 }
 
 #define printfloatx(Name,Variable,Spaces,Precision,EndTxt)  Serial.print(F(Name)); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(F(EndTxt));//Name,Variable,Spaces,Precision,EndTxt
-
 void MPU6050::PrintActiveOffsets() {
 	uint8_t AOffsetRegister = (getDeviceID() < 0x39 )? MPU6050_RA_XA_OFFS_H:0x77;
 	int16_t Data[3];
@@ -3323,15 +3317,13 @@ void MPU6050::PrintActiveOffsets() {
 		I2Cdev::readWords(devAddr, AOffsetRegister, 1, Data);
 		I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, Data+1);
 		I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, Data+2);
-
 	}
 	//	A_OFFSET_H_READ_A_OFFS(Data);
 	printfloatx("", Data[0], 5, 0, ",  ");
 	printfloatx("", Data[1], 5, 0, ",  ");
 	printfloatx("", Data[2], 5, 0, ",  ");
-
 	I2Cdev::readWords(devAddr, 0x13, 3, Data);
-
+	//	XG_OFFSET_H_READ_OFFS_USR(Data);
 	printfloatx("", Data[0], 5, 0, ",  ");
 	printfloatx("", Data[1], 5, 0, ",  ");
 	printfloatx("", Data[2], 5, 0, "\n");