@@ -2352,89 +2352,93 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
2352
2352
2353
2353
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1 .0f - sin_theta_d2);
2354
2354
2355
- // For small moves with >135° junction (octagon) find speed for approximate arc
2356
- if (block->millimeters < 1 && junction_cos_theta < -0 .7071067812f ) {
2357
-
2358
- #if ENABLED(JD_USE_MATH_ACOS)
2359
-
2360
- #error "TODO: Inline maths with the MCU / FPU."
2361
-
2362
- #elif ENABLED(JD_USE_LOOKUP_TABLE)
2363
-
2364
- // Fast acos approximation (max. error +-0.01 rads)
2365
- // Based on LUT table and linear interpolation
2366
-
2367
- /* *
2368
- * // Generate the JD Lookup Table
2369
- * constexpr float c = 1.00751495f; // Correction factor to center error around 0
2370
- * for (int i = 0; i < jd_lut_count - 1; ++i) {
2371
- * const float x0 = (sq(i) - 1) / sq(i),
2372
- * y0 = acos(x0) * (i == 0 ? 1 : c),
2373
- * x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f,
2374
- * y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1);
2375
- * jd_lut_k[i] = (y0 - y1) / (x0 - x1);
2376
- * jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
2377
- * }
2378
- *
2379
- * // Compute correction factor (Set c to 1.0f first!)
2380
- * float min = INFINITY, max = -min;
2381
- * for (float t = 0; t <= 1; t += 0.0003f) {
2382
- * const float e = acos(t) / approx(t);
2383
- * if (isfinite(e)) {
2384
- * if (e < min) min = e;
2385
- * if (e > max) max = e;
2386
- * }
2387
- * }
2388
- * fprintf(stderr, "%.9gf, ", (min + max) / 2);
2389
- */
2390
- static constexpr int16_t jd_lut_count = 16 ;
2391
- static constexpr uint16_t jd_lut_tll = _BV (jd_lut_count - 1 );
2392
- static constexpr int16_t jd_lut_tll0 = __builtin_clz (jd_lut_tll) + 1 ; // i.e., 16 - jd_lut_count + 1
2393
- static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
2394
- -1 .03145837f , -1 .30760646f , -1 .75205851f , -2 .41705704f ,
2395
- -3 .37769222f , -4 .74888992f , -6 .69649887f , -9 .45661736f ,
2396
- -13 .3640480f , -18 .8928222f , -26 .7136841f , -37 .7754593f ,
2397
- -53 .4201813f , -75 .5458374f , -106 .836761f , -218 .532821f };
2398
- static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
2399
- 1 .57079637f , 1 .70887053f , 2 .04220939f , 2 .62408352f ,
2400
- 3 .52467871f , 4 .85302639f , 6 .77020454f , 9 .50875854f ,
2401
- 13 .4009285f , 18 .9188995f , 26 .7321243f , 37 .7885055f ,
2402
- 53 .4293975f , 75 .5523529f , 106 .841369f , 218 .534011f };
2403
-
2404
- const float neg = junction_cos_theta < 0 ? -1 : 1 ,
2405
- t = neg * junction_cos_theta;
2406
-
2407
- const int16_t idx = (t < 0 .00000003f ) ? 0 : __builtin_clz (uint16_t ((1 .0f - t) * jd_lut_tll)) - jd_lut_tll0;
2408
-
2409
- float junction_theta = t * pgm_read_float (&jd_lut_k[idx]) + pgm_read_float (&jd_lut_b[idx]);
2410
- if (neg > 0 ) junction_theta = RADIANS (180 ) - junction_theta; // acos(-t)
2355
+ #if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
2356
+
2357
+ // For small moves with >135° junction (octagon) find speed for approximate arc
2358
+ if (block->millimeters < 1 && junction_cos_theta < -0 .7071067812f ) {
2359
+
2360
+ #if ENABLED(JD_USE_MATH_ACOS)
2361
+
2362
+ #error "TODO: Inline maths with the MCU / FPU."
2363
+
2364
+ #elif ENABLED(JD_USE_LOOKUP_TABLE)
2365
+
2366
+ // Fast acos approximation (max. error +-0.01 rads)
2367
+ // Based on LUT table and linear interpolation
2368
+
2369
+ /* *
2370
+ * // Generate the JD Lookup Table
2371
+ * constexpr float c = 1.00751495f; // Correction factor to center error around 0
2372
+ * for (int i = 0; i < jd_lut_count - 1; ++i) {
2373
+ * const float x0 = (sq(i) - 1) / sq(i),
2374
+ * y0 = acos(x0) * (i == 0 ? 1 : c),
2375
+ * x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f,
2376
+ * y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1);
2377
+ * jd_lut_k[i] = (y0 - y1) / (x0 - x1);
2378
+ * jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
2379
+ * }
2380
+ *
2381
+ * // Compute correction factor (Set c to 1.0f first!)
2382
+ * float min = INFINITY, max = -min;
2383
+ * for (float t = 0; t <= 1; t += 0.0003f) {
2384
+ * const float e = acos(t) / approx(t);
2385
+ * if (isfinite(e)) {
2386
+ * if (e < min) min = e;
2387
+ * if (e > max) max = e;
2388
+ * }
2389
+ * }
2390
+ * fprintf(stderr, "%.9gf, ", (min + max) / 2);
2391
+ */
2392
+ static constexpr int16_t jd_lut_count = 16 ;
2393
+ static constexpr uint16_t jd_lut_tll = _BV (jd_lut_count - 1 );
2394
+ static constexpr int16_t jd_lut_tll0 = __builtin_clz (jd_lut_tll) + 1 ; // i.e., 16 - jd_lut_count + 1
2395
+ static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
2396
+ -1 .03145837f , -1 .30760646f , -1 .75205851f , -2 .41705704f ,
2397
+ -3 .37769222f , -4 .74888992f , -6 .69649887f , -9 .45661736f ,
2398
+ -13 .3640480f , -18 .8928222f , -26 .7136841f , -37 .7754593f ,
2399
+ -53 .4201813f , -75 .5458374f , -106 .836761f , -218 .532821f };
2400
+ static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
2401
+ 1 .57079637f , 1 .70887053f , 2 .04220939f , 2 .62408352f ,
2402
+ 3 .52467871f , 4 .85302639f , 6 .77020454f , 9 .50875854f ,
2403
+ 13 .4009285f , 18 .9188995f , 26 .7321243f , 37 .7885055f ,
2404
+ 53 .4293975f , 75 .5523529f , 106 .841369f , 218 .534011f };
2405
+
2406
+ const float neg = junction_cos_theta < 0 ? -1 : 1 ,
2407
+ t = neg * junction_cos_theta;
2408
+
2409
+ const int16_t idx = (t < 0 .00000003f ) ? 0 : __builtin_clz (uint16_t ((1 .0f - t) * jd_lut_tll)) - jd_lut_tll0;
2410
+
2411
+ float junction_theta = t * pgm_read_float (&jd_lut_k[idx]) + pgm_read_float (&jd_lut_b[idx]);
2412
+ if (neg > 0 ) junction_theta = RADIANS (180 ) - junction_theta; // acos(-t)
2413
+
2414
+ #else
2415
+
2416
+ // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
2417
+ // Based on MinMax polynomial published by W. Randolph Franklin, see
2418
+ // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
2419
+ // acos( t) = pi / 2 - asin(x)
2420
+ // acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
2421
+
2422
+ const float neg = junction_cos_theta < 0 ? -1 : 1 ,
2423
+ t = neg * junction_cos_theta,
2424
+ asinx = 0 .032843707f
2425
+ + t * (-1 .451838349f
2426
+ + t * ( 29 .66153956f
2427
+ + t * (-131 .1123477f
2428
+ + t * ( 262 .8130562f
2429
+ + t * (-242 .7199627f
2430
+ + t * ( 84 .31466202f ) ))))),
2431
+ junction_theta = RADIANS (90 ) + neg * asinx; // acos(-t)
2432
+
2433
+ // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
2411
2434
2412
- #else
2413
-
2414
- // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
2415
- // Based on MinMax polynomial published by W. Randolph Franklin, see
2416
- // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
2417
- // acos( t) = pi / 2 - asin(x)
2418
- // acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
2419
-
2420
- const float neg = junction_cos_theta < 0 ? -1 : 1 ,
2421
- t = neg * junction_cos_theta,
2422
- asinx = 0 .032843707f
2423
- + t * (-1 .451838349f
2424
- + t * ( 29 .66153956f
2425
- + t * (-131 .1123477f
2426
- + t * ( 262 .8130562f
2427
- + t * (-242 .7199627f
2428
- + t * ( 84 .31466202f ) ))))),
2429
- junction_theta = RADIANS (90 ) + neg * asinx; // acos(-t)
2430
-
2431
- // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
2435
+ #endif
2432
2436
2433
- #endif
2437
+ const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
2438
+ NOMORE (vmax_junction_sqr, limit_sqr);
2439
+ }
2434
2440
2435
- const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
2436
- NOMORE (vmax_junction_sqr, limit_sqr);
2437
- }
2441
+ #endif // JD_HANDLE_SMALL_SEGMENTS
2438
2442
}
2439
2443
2440
2444
// Get the lowest speed
0 commit comments