Update MPU6050.cpp

If the acceleration reading is [0 0 0] at the beginning, it causes the unit vector to return [nan nan nan nan], which causes the ITerm always be nan, leading to unconvergence Signed-off-by: Zhaosn <[email protected]>
ElectronicCats · Feb 1, 2024 · fbb3de4 · fbb3de4
1 parent a19a401
commit fbb3de4
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/src/MPU6050.cpp b/src/MPU6050.cpp
@@ -3436,6 +3436,7 @@ void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 				}
 			}
 			for (int i = 0; i < 3; i++) {
+				if (isnan(Reading[i])) {Reading[i] = 0;}
 				Error = -Reading[i];
 				eSum += abs(Reading[i]);
 				PTerm = kP * Error;
@@ -3492,4 +3493,4 @@ void MPU6050_Base::PrintActiveOffsets() {
     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");
-}
+}