Merge pull request #673 from Paciente8159/fix-interpolation-float-aprox

prevent floating point round errors in speed
Paciente8159 · Mar 29, 2024 · 61197e2 · 61197e2
2 parents 17a08e9 + 9cfbde7
commit 61197e2
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 34 deletions.
diff --git a/makefiles/virtual/mcu_virtual.cpp b/makefiles/virtual/mcu_virtual.cpp
@@ -342,16 +342,16 @@ extern "C"
 			BOOL fConnected = FALSE;
 
 			hPipe = CreateNamedPipe(
-				lpszPipename,				// pipe name
-				PIPE_ACCESS_DUPLEX,			// read/write access
-				PIPE_TYPE_MESSAGE |			// message type pipe
-					PIPE_READMODE_MESSAGE | // message-read mode
-					PIPE_WAIT,				// blocking mode
-				PIPE_UNLIMITED_INSTANCES,	// max. instances
-				sizeof(VIRTUAL_MAP),		// output buffer size
-				sizeof(VIRTUAL_MAP),		// input buffer size
-				0,							// client time-out
-				NULL);						// no template file
+					lpszPipename,								// pipe name
+					PIPE_ACCESS_DUPLEX,					// read/write access
+					PIPE_TYPE_MESSAGE |					// message type pipe
+							PIPE_READMODE_MESSAGE | // message-read mode
+							PIPE_WAIT,							// blocking mode
+					PIPE_UNLIMITED_INSTANCES,		// max. instances
+					sizeof(VIRTUAL_MAP),				// output buffer size
+					sizeof(VIRTUAL_MAP),				// input buffer size
+					0,													// client time-out
+					NULL);											// no template file
 
 			if (hPipe == INVALID_HANDLE_VALUE)
 			{
@@ -376,11 +376,11 @@ extern "C"
 					memcpy(lpvMessage, (void *)&virtualmap, sizeof(VIRTUAL_MAP));
 
 					fSuccess = WriteFile(
-						hPipe,		// pipe handle
-						lpvMessage, // message
-						cbToWrite,	// message length
-						&cbWritten, // bytes written
-						NULL);		// not overlapped
+							hPipe,			// pipe handle
+							lpvMessage, // message
+							cbToWrite,	// message length
+							&cbWritten, // bytes written
+							NULL);			// not overlapped
 
 					if (!fSuccess)
 					{
@@ -391,11 +391,11 @@ extern "C"
 					// Read from the pipe.
 
 					fSuccess = ReadFile(
-						hPipe,		// pipe handle
-						lpvMessage, // buffer to receive reply
-						cbToWrite,	// size of buffer
-						&cbRead,	// number of bytes read
-						NULL);		// not overlapped
+							hPipe,			// pipe handle
+							lpvMessage, // buffer to receive reply
+							cbToWrite,	// size of buffer
+							&cbRead,		// number of bytes read
+							NULL);			// not overlapped
 
 					if (!fSuccess && GetLastError() != ERROR_MORE_DATA)
 						break;
@@ -623,7 +623,7 @@ extern "C"
 	 *
 	 * **/
 
-	#ifndef ITP_SAMPLE_RATE
+#ifndef ITP_SAMPLE_RATE
 #define ITP_SAMPLE_RATE (F_STEP_MAX * 2)
 #endif
 
@@ -873,15 +873,15 @@ extern "C"
 
 	void ticksimul(void)
 	{
-//		long t = stopCycleCounter();
-//		printf("Elapsed %dus\n\r", (int)((double)t / cyclesPerMicrosecond));
+		//		long t = stopCycleCounter();
+		//		printf("Elapsed %dus\n\r", (int)((double)t / cyclesPerMicrosecond));
 		for (int i = 0; i < (int)ceil(20 * ITP_SAMPLE_RATE / 1000); i++)
 		{
 			mcu_gen_step();
 		}
 
 		mcu_rtc_cb(mcu_millis());
-//		startCycleCounter();
+		//		startCycleCounter();
 	}
 
 	/**
@@ -901,6 +901,10 @@ extern "C"
 		mcu_enable_global_isr();
 	}
 
+	void mcu_io_reset(void)
+	{
+	}
+
 	int main(int argc, char **argv)
 	{
 		cnc_init();
@@ -910,8 +914,8 @@ extern "C"
 		}
 		return 0;
 	}
-	
-	uint8_t itp_set_step_mode(uint8_t mode) {return 0;}
+
+	uint8_t itp_set_step_mode(uint8_t mode) { return 0; }
 
 	uint32_t mcu_free_micros(void)
 	{

diff --git a/uCNC/src/core/interpolator.c b/uCNC/src/core/interpolator.c
@@ -459,10 +459,11 @@ void itp_run(void)
 				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(floorf(INTERPOLATOR_FREQ * t));
-				if (slices_inv)
+
+				if (t > INTERPOLATOR_DELTA_T)
 				{
+					// slice up time in an integral number of periods (half with positive jerk and half with negative)
+					float slices_inv = fast_flt_inv(floorf(INTERPOLATOR_FREQ * t));
 					t_acc_integrator = t * slices_inv;
 #if S_CURVE_ACCELERATION_LEVEL != 0
 					acc_step = slices_inv;
@@ -497,10 +498,11 @@ void itp_run(void)
 				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(floorf(INTERPOLATOR_FREQ * t));
-				if (slices_inv)
+
+				if (t > INTERPOLATOR_DELTA_T)
 				{
+					// slice up time in an integral number of periods (half with positive jerk and half with negative)
+					float slices_inv = fast_flt_inv(floorf(INTERPOLATOR_FREQ * t));
 					t_deac_integrator = t * slices_inv;
 					if (t_deac_integrator < 0.00001f)
 					{
@@ -1127,7 +1129,7 @@ MCU_CALLBACK void mcu_step_cb(void)
 		else
 		{
 			cnc_clear_exec_state(EXEC_RUN); // this naturally clears the RUN flag. Any other ISR stop does not clear the flag.
-			itp_stop();						// the buffer is empty. The ISR can stop
+			itp_stop();											// the buffer is empty. The ISR can stop
 			return;
 		}
 	}

diff --git a/uCNC/src/core/planner.c b/uCNC/src/core/planner.c
@@ -363,7 +363,7 @@ float planner_get_block_top_speed(float exit_speed_sqr)
 	// calculates the difference between the entry speed and the exit speed
 	uint8_t index = planner_data_read;
 	float speed_delta = exit_speed_sqr - planner_data[index].entry_feed_sqr;
-	// caclculates the speed increase/decrease for the given distance
+	// calculates the speed increase/decrease for the given distance
 	float junction_speed_sqr = planner_data[index].acceleration * (float)(planner_data[index].steps[planner_data[index].main_stepper]);
 	junction_speed_sqr = fast_flt_mul2(junction_speed_sqr);
 	// if there is enough space to accelerate computes the junction speed