Browse Source

Improve JD acosx implementation (#17817)

Štěpán Dalecký 5 years ago
parent
commit
0c68794fa9
No account linked to committer's email address
2 changed files with 93 additions and 22 deletions
  1. 79
    21
      Marlin/src/module/planner.cpp
  2. 14
    1
      Marlin/src/module/planner.h

+ 79
- 21
Marlin/src/module/planner.cpp View File

@@ -2304,28 +2304,87 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
2304 2304
         const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),
2305 2305
                     sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
2306 2306
 
2307
-        vmax_junction_sqr = (junction_acceleration * junction_deviation_mm * sin_theta_d2) / (1.0f - sin_theta_d2);
2307
+        vmax_junction_sqr = JUNC_SQ(junction_acceleration, sin_theta_d2);
2308 2308
 
2309 2309
         if (block->millimeters < 1) {
2310
-          // Fast acos approximation (max. error +-0.033 rads)
2311
-          // Based on MinMax polynomial published by W. Randolph Franklin, see
2312
-          // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
2313
-          // (acos(x) = pi / 2 - asin(x))
2314
-
2315 2310
           const float neg = junction_cos_theta < 0 ? -1 : 1,
2316
-                      t = neg * junction_cos_theta,
2317
-                      asinx =       0.032843707f
2318
-                            + t * (-1.451838349f
2319
-                            + t * ( 29.66153956f
2320
-                            + t * (-131.1123477f
2321
-                            + t * ( 262.8130562f
2322
-                            + t * (-242.7199627f + t * 84.31466202f) )))),
2323
-                      junction_theta = RADIANS(90) - neg * asinx;
2311
+                      t = neg * junction_cos_theta;
2324 2312
 
2325 2313
           // If angle is greater than 135 degrees (octagon), find speed for approximate arc
2326
-          if (junction_theta > RADIANS(135)) {
2327
-            // NOTE: MinMax acos approximation and thereby also junction_theta top out at pi-0.033, which avoids division by 0
2328
-            const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;
2314
+          if (t < -0.7071067812f) {
2315
+
2316
+            #if ENABLED(JD_USE_MATH_ACOS)
2317
+
2318
+              #error "TODO: Inline maths with the MCU / FPU."
2319
+
2320
+            #elif ENABLED(JD_USE_LOOKUP_TABLE)
2321
+
2322
+              // Fast acos approximation (max. error +-0.01 rads)
2323
+              // Based on LUT table and linear interpolation
2324
+
2325
+              /**
2326
+               *  // Generate the JD Lookup Table
2327
+               *  constexpr float c = 1.00751317f; // Correction factor to center error around 0
2328
+               *  for (int i = 0; i < jd_lut_count - 1; ++i) {
2329
+               *    const float x0 = (sq(i) - 1) / sq(i),
2330
+               *                y0 = acos(x0) * (i ? c : 1),
2331
+               *                x1 = 0.5 * x0 + 0.5,
2332
+               *                y1 = acos(x1) * c;
2333
+               *    jd_lut_k[i] = (y0 - y1) / (x0 - x1);
2334
+               *    jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
2335
+               *  }
2336
+               *  jd_lut_k[jd_lut_count - 1] = jd_lut_b[jd_lut_count - 1] = 0;
2337
+               *
2338
+               *  // Compute correction factor (Set c to 1.0f first!)
2339
+               *  float min = INFINITY, max = -min;
2340
+               *  for (float t = 0; t <= 1; t += 0.0003f) {
2341
+               *    const float e = acos(t) / approx(t);
2342
+               *    if (isfinite(e)) {
2343
+               *      NOMORE(min, e);
2344
+               *      NOLESS(max, e);
2345
+               *    }
2346
+               *  }
2347
+               *  fprintf(stderr, "%.9gf, ", (min + max) / 2);
2348
+               */
2349
+              static constexpr int16_t  jd_lut_count = 15;
2350
+              static constexpr uint16_t jd_lut_tll   = 1 << jd_lut_count;
2351
+              static constexpr int16_t  jd_lut_tll0  = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count
2352
+              static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
2353
+                -1.03146219f, -1.30760407f, -1.75205469f, -2.41705418f, -3.37768555f,
2354
+                -4.74888229f, -6.69648552f, -9.45659828f, -13.3640289f, -18.8927879f,
2355
+                -26.7136307f, -37.7754059f, -53.4200745f, -75.5457306f,   0.0f };
2356
+              static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
2357
+                1.57079637f, 1.70886743f, 2.04220533f, 2.62408018f, 3.52467203f,
2358
+                4.85301876f, 6.77019119f, 9.50873947f, 13.4009094f, 18.9188652f,
2359
+                26.7320709f, 37.7884521f, 53.4292908f, 75.5522461f,  0.0f };
2360
+
2361
+              const int16_t idx = (t == 0.0f) ? 0 : __builtin_clz(int16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0;
2362
+
2363
+              float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]);
2364
+              if (neg > 0) junction_theta = RADIANS(180) - junction_theta;
2365
+
2366
+            #else
2367
+
2368
+              // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
2369
+              // Based on MinMax polynomial published by W. Randolph Franklin, see
2370
+              // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
2371
+              //  acos( t) = pi / 2 - asin(x)
2372
+              //  acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
2373
+
2374
+              const float asinx =       0.032843707f
2375
+                                + t * (-1.451838349f
2376
+                                + t * ( 29.66153956f
2377
+                                + t * (-131.1123477f
2378
+                                + t * ( 262.8130562f
2379
+                                + t * (-242.7199627f
2380
+                                + t * ( 84.31466202f ) ))))),
2381
+                          junction_theta = RADIANS(90) + neg * asinx; // acos(-t)
2382
+
2383
+              // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
2384
+
2385
+            #endif
2386
+
2387
+            const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
2329 2388
             NOMORE(vmax_junction_sqr, limit_sqr);
2330 2389
           }
2331 2390
         }
@@ -2363,11 +2422,10 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
2363 2422
     // Start with a safe speed (from which the machine may halt to stop immediately).
2364 2423
     float safe_speed = nominal_speed;
2365 2424
 
2366
-    #ifdef TRAVEL_EXTRA_XYJERK
2367
-      const float extra_xyjerk = (de <= 0) ? TRAVEL_EXTRA_XYJERK : 0;
2368
-    #else
2369
-      constexpr float extra_xyjerk = 0;
2425
+    #ifndef TRAVEL_EXTRA_XYJERK
2426
+      #define TRAVEL_EXTRA_XYJERK 0
2370 2427
     #endif
2428
+    const float extra_xyjerk = (de <= 0) ? TRAVEL_EXTRA_XYJERK : 0;
2371 2429
 
2372 2430
     uint8_t limited = 0;
2373 2431
     TERN(HAS_LINEAR_E_JERK, LOOP_XYZ, LOOP_XYZE)(i) {

+ 14
- 1
Marlin/src/module/planner.h View File

@@ -32,6 +32,17 @@
32 32
 
33 33
 #include "../MarlinCore.h"
34 34
 
35
+#if HAS_JUNCTION_DEVIATION
36
+  // Enable this option for perfect accuracy but maximum
37
+  // computation. Should be fine on ARM processors.
38
+  //#define JD_USE_MATH_ACOS
39
+
40
+  // Disable this option to save 120 bytes of PROGMEM,
41
+  // but incur increased computation and a reduction
42
+  // in accuracy.
43
+  #define JD_USE_LOOKUP_TABLE
44
+#endif
45
+
35 46
 #include "motion.h"
36 47
 #include "../gcode/queue.h"
37 48
 
@@ -827,9 +838,11 @@ class Planner {
827 838
       static void autotemp_update();
828 839
     #endif
829 840
 
841
+    #define JUNC_SQ(N,ST) (junction_deviation_mm * (N) * (ST) / (1.0f - (ST)))
842
+
830 843
     #if HAS_LINEAR_E_JERK
831 844
       FORCE_INLINE static void recalculate_max_e_jerk() {
832
-        #define GET_MAX_E_JERK(N) SQRT(SQRT(0.5) * junction_deviation_mm * (N) * RECIPROCAL(1.0 - SQRT(0.5)))
845
+        #define GET_MAX_E_JERK(N) SQRT(JUNC_SQ(N,SQRT(0.5)))
833 846
         #if ENABLED(DISTINCT_E_FACTORS)
834 847
           LOOP_L_N(i, EXTRUDERS)
835 848
             max_e_jerk[i] = GET_MAX_E_JERK(settings.max_acceleration_mm_per_s2[E_AXIS_N(i)]);

Loading…
Cancel
Save