diff --git a/dda.c b/dda.c index 7416461..63bd807 100644 --- a/dda.c +++ b/dda.c @@ -75,10 +75,10 @@ static const axes_uint32_t PROGMEM maximum_feedrate_P = { /// \brief Initialization constant for the ramping algorithm. Timer cycles for /// first step interval. static const axes_uint32_t PROGMEM c0_P = { - ((uint32_t)((double)F_CPU / SQRT((double)(STEPS_PER_M_X * ACCELERATION / 2000.)))) << 8, - ((uint32_t)((double)F_CPU / SQRT((double)(STEPS_PER_M_Y * ACCELERATION / 2000.)))) << 8, - ((uint32_t)((double)F_CPU / SQRT((double)(STEPS_PER_M_Z * ACCELERATION / 2000.)))) << 8, - ((uint32_t)((double)F_CPU / SQRT((double)(STEPS_PER_M_E * ACCELERATION / 2000.)))) << 8 + (uint32_t)((double)F_CPU / SQRT((double)(STEPS_PER_M_X * ACCELERATION / 2000.))), + (uint32_t)((double)F_CPU / SQRT((double)(STEPS_PER_M_Y * ACCELERATION / 2000.))), + (uint32_t)((double)F_CPU / SQRT((double)(STEPS_PER_M_Z * ACCELERATION / 2000.))), + (uint32_t)((double)F_CPU / SQRT((double)(STEPS_PER_M_E * ACCELERATION / 2000.))) }; /*! Set the direction of the 'n' axis @@ -395,15 +395,15 @@ void dda_create(DDA *dda, TARGET *target) { dda_join_moves(prev_dda, dda); dda->n = dda->start_steps; if (dda->n == 0) - dda->c = pgm_read_dword(&c0_P[dda->fast_axis]); + dda->c = pgm_read_dword(&c0_P[dda->fast_axis]) << 8; else - dda->c = ((pgm_read_dword(&c0_P[dda->fast_axis]) >> 8) * + dda->c = (pgm_read_dword(&c0_P[dda->fast_axis]) * int_inv_sqrt(dda->n)) >> 5; if (dda->c < dda->c_min) dda->c = dda->c_min; #else dda->n = 0; - dda->c = pgm_read_dword(&c0_P[dda->fast_axis]); + dda->c = pgm_read_dword(&c0_P[dda->fast_axis]) << 8; #endif #elif defined ACCELERATION_TEMPORAL @@ -866,11 +866,11 @@ void dda_clock() { } if (recalc_speed) { if (dda->n == 0) - move_c = pgm_read_dword(&c0_P[dda->fast_axis]); + move_c = pgm_read_dword(&c0_P[dda->fast_axis]) << 8; else // Explicit formula: c0 * (sqrt(n + 1) - sqrt(n)), // approximation here: c0 * (1 / (2 * sqrt(n))). - move_c = ((pgm_read_dword(&c0_P[dda->fast_axis]) >> 8) * + move_c = (pgm_read_dword(&c0_P[dda->fast_axis]) * int_inv_sqrt(dda->n)) >> 5; // TODO: most likely this whole check is obsolete. It was left as a