Skip to content

Commit 20fd62b

Browse files
XDA-BamHairingX
XDA-Bam
authored and
HairingX
committed
Add JD_HANDLE_SMALL_SEGMENTS option (MarlinFirmware#18316)
1 parent 68f211a commit 20fd62b

File tree

3 files changed

+87
-81
lines changed

3 files changed

+87
-81
lines changed

‎Marlin/Configuration.h

+2
Original file line numberDiff line numberDiff line change
@@ -815,6 +815,8 @@
815815
*/
816816
#if DISABLED(CLASSIC_JERK)
817817
#define JUNCTION_DEVIATION_MM 0.013 // (mm) Distance from real junction edge
818+
#define JD_HANDLE_SMALL_SEGMENTS // Use curvature estimation instead of just the junction angle
819+
// for small segments (< 1mm) with large junction angles (> 135°).
818820
#endif
819821

820822
/**

‎Marlin/src/module/planner.cpp

+84-80
Original file line numberDiff line numberDiff line change
@@ -2352,89 +2352,93 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
23522352

23532353
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2);
23542354

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.
24112434

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
24322436

2433-
#endif
2437+
const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
2438+
NOMORE(vmax_junction_sqr, limit_sqr);
2439+
}
24342440

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
24382442
}
24392443

24402444
// Get the lowest speed

‎Marlin/src/module/planner.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@
3232

3333
#include "../MarlinCore.h"
3434

35-
#if HAS_JUNCTION_DEVIATION
35+
#if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
3636
// Enable this option for perfect accuracy but maximum
3737
// computation. Should be fine on ARM processors.
3838
//#define JD_USE_MATH_ACOS

0 commit comments

Comments
 (0)