Clean up i2c encoder, sanitize serial

MarlinFirmware · Feb 1, 2020 · e64b7a3 · e64b7a3
1 parent 1af0579
commit e64b7a3
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 20 deletions.
diff --git a/Marlin/src/feature/I2CPositionEncoder.cpp b/Marlin/src/feature/I2CPositionEncoder.cpp
@@ -162,15 +162,17 @@ void I2CPositionEncoder::update() {
       if (errIdx == 0) {
         // In order to correct for "error" but avoid correcting for noise and non-skips
         // it must be > threshold and have a difference average of < 10 and be < 2000 steps
-        if (ABS(error) > threshold * planner.settings.axis_steps_per_mm[encoderAxis] &&
-            diffSum < 10 * (I2CPE_ERR_ARRAY_SIZE - 1) && ABS(error) < 2000) { // Check for persistent error (skip)
+        if (ABS(error) > threshold * planner.settings.axis_steps_per_mm[encoderAxis]
+            && diffSum < 10 * (I2CPE_ERR_ARRAY_SIZE - 1)
+            && ABS(error) < 2000
+        ) {                              // Check for persistent error (skip)
           errPrst[errPrstIdx++] = error; // Error must persist for I2CPE_ERR_PRST_ARRAY_SIZE error cycles. This also serves to improve the average accuracy
           if (errPrstIdx >= I2CPE_ERR_PRST_ARRAY_SIZE) {
             float sumP = 0;
             LOOP_L_N(i, I2CPE_ERR_PRST_ARRAY_SIZE) sumP += errPrst[i];
             const int32_t errorP = int32_t(sumP * RECIPROCAL(I2CPE_ERR_PRST_ARRAY_SIZE));
             SERIAL_ECHO(axis_codes[encoderAxis]);
-            SERIAL_ECHOLNPAIR(" - err detected: ", errorP * planner.steps_to_mm[encoderAxis], "mm; correcting!");
+            SERIAL_ECHOLNPAIR(" : CORRECT ERR ", errorP * planner.steps_to_mm[encoderAxis], "mm");
             babystep.add_steps(encoderAxis, -LROUND(errorP));
             errPrstIdx = 0;
           }
@@ -189,7 +191,8 @@ void I2CPositionEncoder::update() {
     if (ABS(error) > I2CPE_ERR_CNT_THRESH * planner.settings.axis_steps_per_mm[encoderAxis]) {
       const millis_t ms = millis();
       if (ELAPSED(ms, nextErrorCountTime)) {
-        SERIAL_ECHOLNPAIR("Large error on ", axis_codes[encoderAxis], " axis. error: ", (int)error, "; diffSum: ", diffSum);
+        SERIAL_ECHO(axis_codes[encoderAxis]);
+        SERIAL_ECHOLNPAIR(" : LARGE ERR ", int(error), "; diffSum=", diffSum);
         errorCount++;
         nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS;
       }
@@ -243,17 +246,14 @@ bool I2CPositionEncoder::passes_test(const bool report) {
 }
 
 float I2CPositionEncoder::get_axis_error_mm(const bool report) {
-  float target, actual, error;
-
-  target = planner.get_axis_position_mm(encoderAxis);
-  actual = mm_from_count(position);
-  error = actual - target;
-
-  if (ABS(error) > 10000) error = 0; // ?
+  const float target = planner.get_axis_position_mm(encoderAxis),
+              actual = mm_from_count(position),
+              diff = actual - target,
+              error = ABS(diff) > 10000 ? 0 : diff; // Huge error is a bad reading
 
   if (report) {
     SERIAL_ECHO(axis_codes[encoderAxis]);
-    SERIAL_ECHOLNPAIR(" axis target: ", target, ", actual: ", actual, ", error : ",error);
+    SERIAL_ECHOLNPAIR(" axis target=", target, "mm; actual=", actual, "mm; err=", error, "mm");
   }
 
   return error;
@@ -278,21 +278,25 @@ int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) {
   //convert both 'ticks' into same units / base
   encoderCountInStepperTicksScaled = LROUND((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit);
 
-  int32_t target = stepper.position(encoderAxis),
-          error = (encoderCountInStepperTicksScaled - target);
+  const int32_t target = stepper.position(encoderAxis);
+  int32_t error = encoderCountInStepperTicksScaled - target;
 
   //suppress discontinuities (might be caused by bad I2C readings...?)
   const bool suppressOutput = (ABS(error - errorPrev) > 100);
 
+  errorPrev = error;
+
   if (report) {
     SERIAL_ECHO(axis_codes[encoderAxis]);
-    SERIAL_ECHOLNPAIR(" axis target: ", target, ", actual: ", encoderCountInStepperTicksScaled, ", error : ", error);
-    if (suppressOutput) SERIAL_ECHOLNPGM("Discontinuity detected, suppressing error.");
+    SERIAL_ECHOLNPAIR(" axis target=", target, "; actual=", encoderCountInStepperTicksScaled, "; err=", error);
   }
 
-  errorPrev = error;
+  if (suppressOutput) {
+    if (report) SERIAL_ECHOLNPGM("!Discontinuity. Suppressing error.");
+    error = 0;
+  }
 
-  return (suppressOutput ? 0 : error);
+  return error;
 }
 
 int32_t I2CPositionEncoder::get_raw_count() {

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
@@ -1629,7 +1629,7 @@
           auto debug_pt = [](PGM_P const pre, const xy_pos_t &pos, const float &zadd) {
             d_from(); serialprintPGM(pre);
             DEBUG_ECHO_F(normed(pos, zadd), 6);
-            DEBUG_ECHOLNPAIR_F("   Z error: ", zadd - get_z_correction(pos), 6);
+            DEBUG_ECHOLNPAIR_F("   Z error = ", zadd - get_z_correction(pos), 6);
           };
           debug_pt(PSTR("1st point: "), probe_pt[0], normal.z * z1);
           debug_pt(PSTR("2nd point: "), probe_pt[1], normal.z * z2);
@@ -1638,7 +1638,7 @@
           DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_homing_xy, 0), 6);
           d_from(); DEBUG_ECHOPGM("safe home with Z=");
           DEBUG_ECHOLNPAIR_F("mesh value ", normed(safe_homing_xy, get_z_correction(safe_homing_xy)), 6);
-          DEBUG_ECHOPAIR("   Z error: (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT);
+          DEBUG_ECHOPAIR("   Z error = (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT);
           DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(safe_homing_xy), 6);
         #endif
       } // DEBUGGING(LEVELING)