Browse Source

More accurate Junction Deviation fast-acos (#17575)

XDA-Bam 5 years ago
parent
commit
bbe2cb75ad
No account linked to committer's email address
1 changed files with 16 additions and 3 deletions
  1. 16
    3
      Marlin/src/module/planner.cpp

+ 16
- 3
Marlin/src/module/planner.cpp View File

@@ -2390,13 +2390,26 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
2390 2390
                     sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
2391 2391
 
2392 2392
         vmax_junction_sqr = (junction_acceleration * junction_deviation_mm * sin_theta_d2) / (1.0f - sin_theta_d2);
2393
-        if (block->millimeters < 1) {
2394 2393
 
2395
-          // Fast acos approximation, minus the error bar to be safe
2396
-          const float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18f;
2394
+        if (block->millimeters < 1) {
2395
+          // Fast acos approximation (max. error +-0.033 rads)
2396
+          // Based on MinMax polynomial published by W. Randolph Franklin, see
2397
+          // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
2398
+          // (acos(x) = pi / 2 - asin(x))
2399
+
2400
+          const float neg = junction_cos_theta < 0 ? -1 : 1,
2401
+                      t = neg * junction_cos_theta,
2402
+                      asinx =       0.032843707f
2403
+                            + t * (-1.451838349f
2404
+                            + t * ( 29.66153956f
2405
+                            + t * (-131.1123477f
2406
+                            + t * ( 262.8130562f
2407
+                            + t * (-242.7199627f + t * 84.31466202f) )))),
2408
+                      junction_theta = RADIANS(90) - neg * asinx;
2397 2409
 
2398 2410
           // If angle is greater than 135 degrees (octagon), find speed for approximate arc
2399 2411
           if (junction_theta > RADIANS(135)) {
2412
+            // NOTE: MinMax acos approximation and thereby also junction_theta top out at pi-0.033, which avoids division by 0
2400 2413
             const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;
2401 2414
             NOMORE(vmax_junction_sqr, limit_sqr);
2402 2415
           }

Loading…
Cancel
Save