interpolator algorithm modification

- interpolator algorithm modification to correctly emulate acceleration based on riemman integrator

uCNC/cnc_hal_config.h

@@ -274,8 +274,6 @@ extern "C"
  * **/
 // assign the tools from 1 to 16
 #if (TOOL_COUNT >= 1)
-// to allow build on virtual emulator
-extern const tool_t spindle_pwm;
 #define TOOL1 spindle_pwm
 #endif
 #if (TOOL_COUNT >= 2)

uCNC/src/core/interpolator.c

@@ -337,8 +337,7 @@ void itp_run(void)
 	static float junction_speed = 0;
 	static float feed_convert = 0;
 	static float partial_distance = 0;
-	static float t_acc_integrator = 0;
-	static float t_deac_integrator = 0;
+
 #if S_CURVE_ACCELERATION_LEVEL != 0
 	static float acc_step = 0;
 	static float acc_step_acum = 0;
@@ -452,23 +451,6 @@ void itp_run(void)
 				float accel_dist = ABS(junction_speed_sqr - itp_cur_plan_block->entry_feed_sqr) * accel_inv;
 				accel_dist = fast_flt_div2(accel_dist);
 				accel_until -= floorf(accel_dist);
-				float t = ABS(junction_speed - current_speed);
-#if S_CURVE_ACCELERATION_LEVEL != 0
-				acc_scale = t;
-				acc_step_acum = 0;
-				acc_init_speed = current_speed;
-#endif
-				t *= accel_inv;
-				// slice up time in an integral number of periods (half with positive jerk and half with negative)
-				float slices_inv = fast_flt_inv(ceilf(INTERPOLATOR_FREQ * t));
-				t_acc_integrator = t * slices_inv;
-#if S_CURVE_ACCELERATION_LEVEL != 0
-				acc_step = slices_inv;
-#endif
-				if ((junction_speed_sqr < itp_cur_plan_block->entry_feed_sqr))
-				{
-					t_acc_integrator = -t_acc_integrator;
-				}
 			}
 
 			// if entry speed already a junction speed updates it.
@@ -483,24 +465,10 @@ void itp_run(void)
 				float deaccel_dist = (junction_speed_sqr - exit_speed_sqr) * accel_inv;
 				deaccel_dist = fast_flt_div2(deaccel_dist);
 				deaccel_from = floorf(deaccel_dist);
-				// same as before t can be calculated using the normal ramp equation
-				float t = ABS(junction_speed - fast_flt_sqrt(exit_speed_sqr));
-#if S_CURVE_ACCELERATION_LEVEL != 0
-				deac_scale = t;
-				deac_step_acum = 0;
-#endif
-				t *= accel_inv;
-				// slice up time in an integral number of periods (half with positive jerk and half with negative)
-				float slices_inv = fast_flt_inv(ceilf(INTERPOLATOR_FREQ * t));
-				t_deac_integrator = t * slices_inv;
-
-#if S_CURVE_ACCELERATION_LEVEL != 0
-				deac_step = slices_inv;
-#endif
 			}
 		}
 
-		float speed_change;
+		float speed_change, accel;
 		float profile_steps_limit;
 		float integrator;
 		// acceleration profile
@@ -513,18 +481,18 @@ void itp_run(void)
 				to the same distance traveled at a constant speed given that
 				constant_speed = 0.5 * (final_speed - initial_speed) + initial_speed
 				where
-				(final_speed - initial_speed) = acceleration * INTERPOLATOR_DELTA_T;
+				speed_change = final_speed - initial_speed = acceleration * INTERPOLATOR_DELTA_T;
+				distance travelled = final_position - initial_position = (initial_speed + 0.5*speed_change) * INTERPOLATOR_DELTA_T
 			*/
-			integrator = t_acc_integrator;
+			accel = itp_cur_plan_block->acceleration;
+			speed_change = accel * INTERPOLATOR_DELTA_T;
 #if S_CURVE_ACCELERATION_LEVEL != 0
 			float acum = acc_step_acum;
 			acum += acc_step;
 			acc_step_acum = MIN(acum, 0.999f);
 			float new_speed = acc_scale * s_curve_function(acum) + acc_init_speed;
 			new_speed = (t_acc_integrator >= 0) ? (new_speed + acc_init_speed) : (acc_init_speed - new_speed);
 			speed_change = new_speed - current_speed;
-#else
-			speed_change = integrator * itp_cur_plan_block->acceleration;
 #endif
 
 			profile_steps_limit = accel_until;
@@ -540,15 +508,14 @@ void itp_run(void)
 		}
 		else
 		{
-			integrator = t_deac_integrator;
+			accel = itp_cur_plan_block->acceleration;
+			speed_change = -accel * integrator;
 #if S_CURVE_ACCELERATION_LEVEL != 0
 			float acum = deac_step_acum;
 			acum += deac_step;
 			deac_step_acum = MIN(acum, 0.999f);
 			float new_speed = junction_speed - deac_scale * s_curve_function(acum);
 			speed_change = new_speed - current_speed;
-#else
-			speed_change = -(integrator * itp_cur_plan_block->acceleration);
 #endif
 			profile_steps_limit = 0;
 			sgm->flags = ITP_UPDATE_ISR | ITP_DEACCEL;
@@ -558,21 +525,19 @@ void itp_run(void)
 			common calculations for all three profiles (accel, constant and deaccel)
 		*/
 		uint16_t segm_steps;
-		speed_change = fast_flt_div2(speed_change);
-		current_speed += speed_change;
-
-		if (current_speed > 0)
+		float half_speed_change = fast_flt_div2(speed_change);
+		partial_distance += (current_speed + half_speed_change) * INTERPOLATOR_DELTA_T;
+		// computes how many steps it will perform at this speed and frame window
+		segm_steps = (uint16_t)truncf(partial_distance);
+		if (speed_change)
 		{
-			partial_distance += current_speed * integrator;
-			// computes how many steps it will perform at this speed and frame window
-			segm_steps = (uint16_t)floorf(partial_distance);
-			// update speed at the end of segment
-			if (speed_change)
-			{
-				itp_cur_plan_block->entry_feed_sqr = fast_flt_pow2(current_speed);
-			}
+			current_speed += speed_change;
+			itp_cur_plan_block->entry_feed_sqr = fast_flt_pow2(current_speed);
+			// continue itp calculation with the average speed increase
+			current_speed -= half_speed_change;
 		}
-		else
+
+		if (current_speed <= 0)
 		{
 			// speed can't be negative
 			itp_cur_plan_block->entry_feed_sqr = 0;
@@ -602,14 +567,24 @@ void itp_run(void)
 // This works in a similar way to Grbl's AMASS but has a modified implementation to minimize the processing penalty on the ISR and also take less static memory.
 // DSS never loads the step generating ISR with a frequency above half of the absolute maximum frequency
 #if (DSS_MAX_OVERSAMPLING != 0)
+
 		float dss_speed = current_speed;
 		uint8_t dss = 0;
-		while (dss_speed < DSS_CUTOFF_FREQ && dss < DSS_MAX_OVERSAMPLING && segm_steps)
+		switch (segm_steps)
 		{
-			dss_speed = fast_flt_mul2(dss_speed);
-			dss++;
+		case 0:
+		case 1:
+			dss_speed = INTERPOLATOR_FREQ; // one full bresenham sample
+			break;
+		default:
+			// not empty step block
+			while (dss_speed < DSS_CUTOFF_FREQ && dss < DSS_MAX_OVERSAMPLING && segm_steps)
+			{
+				dss_speed = fast_flt_mul2(dss_speed);
+				dss++;
+			}
+			break;
 		}
-
 		if (dss != prev_dss)
 		{
 			sgm->flags = ITP_UPDATE_ISR;
@@ -676,9 +651,13 @@ void itp_run(void)
 #endif
 		remaining_steps -= segm_steps;
 
-		if (remaining_steps == accel_until && !cnc_get_exec_state(EXEC_HOLD)) // resets float additions error
+		//		if (remaining_steps == accel_until && !cnc_get_exec_state(EXEC_HOLD)) // resets float additions error
+		//		{
+		//			itp_cur_plan_block->entry_feed_sqr = fast_flt_pow2(junction_speed);
+		//		}
+		if (!remaining_steps)
 		{
-			itp_cur_plan_block->entry_feed_sqr = fast_flt_pow2(junction_speed);
+			partial_distance = 0;
 		}
 
 		itp_cur_plan_block->steps[itp_cur_plan_block->main_stepper] = remaining_steps;

uCNC/src/hal/boards/esp8266/esp8266.ini

@@ -6,7 +6,7 @@
 platform = espressif8266
 framework = arduino
 board = d1
-; build_src_filter = +<*>-<src/tinyusb>
+build_src_filter = +<*>-<src/hal/mcus/virtual>
 ; lib_deps = 
 ;     https://github.com/tzapu/WiFiManager/archive/refs/heads/master.zip
 build_flags = -DBOARD=BOARD_WEMOS_D1 -DENABLE_WIFI

uCNC/src/hal/mcus/virtual/mcu_virtual.cpp

@@ -16,10 +16,16 @@
 	See the	GNU General Public License for more details.
 */
 
+#include "../../../../cnc_config.h"
+
+#if (BOARD == BOARD_VIRTUAL)
+#ifdef __cplusplus
 extern "C"
 {
+#endif
+
 #include "../../../cnc.h"
-#if (MCU == MCU_VIRTUAL_WIN)
+
 
 #include <stdio.h>
 #include <conio.h>
@@ -325,7 +331,7 @@ extern "C"
 
 	static volatile VIRTUAL_MAP virtualmap;
 
-	void* ioserver(void *args)
+	void *ioserver(void *args)
 	{
 		HANDLE hPipe;
 		TCHAR chBuf[sizeof(VIRTUAL_MAP)];
@@ -801,5 +807,7 @@ extern "C"
 		return 0;
 	}
 
-#endif
+#ifdef __cplusplus
 }
+#endif
+#endif

uCNC/src/hal/mcus/virtual/mcumap_virtual.h

@@ -420,6 +420,9 @@
 extern void virtual_delay_us(uint16_t delay);
 #define mcu_delay_us(X) virtual_delay_us(X)
 
+// to allow build on virtual emulator
+extern const tool_t spindle_pwm;
+
 #define asm __asm__
 
 #endif