Make the step timer fit for very short delays.

This isn't the magic patch which suddenly allows us to run at
arbitrary high step rates, but can deal with short bursts of
very short delays only. Typical for a 45 deg move when using
ACCELERATION_TEMPORAL.

The implementation simply extends the impossible small delay.
To keep overall timing promises regardless, the extra is stored
and compensated for when a longer delay request comes in.

Wrapped all this in #ifdef ACCELERATION_TEMPORAL, as this adds
about 300 bytes of binary size, so it likely slows down the
setTimer() code a bit and non-temporal algorithms are expected to
never request an unreasonable short delay.

timer.c

@@ -29,6 +29,11 @@
 /// time until next step, as output compare register is too small for long step times
 uint32_t	next_step_time;
 
+#ifdef ACCELERATION_TEMPORAL
+/// unwanted extra delays, ideally always zero
+uint32_t	step_extra_time = 0;
+#endif /* ACCELERATION_TEMPORAL */
+
 /// every time our clock fires, we increment this so we know when 10ms has elapsed
 uint8_t						clock_counter_10ms = 0;
 /// keep track of when 250ms has elapsed
@@ -134,28 +139,58 @@ void setTimer(uint32_t delay)
 	// save interrupt flag
 	uint8_t sreg = SREG;
 	uint16_t step_start = 0;
+	#ifdef ACCELERATION_TEMPORAL
+	uint16_t current_time;
+	uint32_t earliest_time, actual_time;
+	#endif /* ACCELERATION_TEMPORAL */
 
 	// re-enable clock interrupt in case we're recovering from emergency stop
 	TIMSK1 |= MASK(OCIE1B);
 
-	// if the delay is too small use a minimum delay so that there is time
-	// to set everything up before the timer expires.
-
-	if (delay < 17 )
-		delay = 17;
+	// An interrupt would make all our timing calculations invalid,
+	// so stop that here.
+	cli();
+	CLI_SEI_BUG_MEMORY_BARRIER();
 
 	// Assume all steps belong to one move. Within one move the delay is
 	// from one step to the next one, which should be more or less the same
 	// as from one step interrupt to the next one. The last step interrupt happend
 	// at OCR1A, so start delay from there.
-	#warning This can't work. If it took some time since the last setTimer() and the delay is very short, the timer will wait almost a full round (3,27 ms).
 	step_start = OCR1A;
 	if (next_step_time == 0) {
 		// new move, take current time as start value
 		step_start = TCNT1;
 	}
-
 	next_step_time = delay;
+
+	#ifdef ACCELERATION_TEMPORAL
+	// 300 = safe number of cpu cycles until the interrupt actually happens
+	current_time = TCNT1;
+	earliest_time = (uint32_t)current_time + 300;
+	if (current_time < step_start) // timer counter did overflow recently
+		earliest_time += 0x00010000;
+	actual_time = (uint32_t)step_start + next_step_time;
+
+	// Setting the interrupt earlier than it can happen obviously doesn't
+	// make sense. To keep the "belongs to one move" idea, add an extra,
+	// remember this extra and compensate the extra if a longer delay comes in.
+	if (earliest_time > actual_time) {
+		step_extra_time += (earliest_time - actual_time);
+		next_step_time = earliest_time - (uint32_t)step_start;
+	}
+	else if (step_extra_time) {
+		if (step_extra_time < actual_time - earliest_time) {
+			next_step_time -= step_extra_time;
+			step_extra_time = 0;
+		}
+		else {
+			step_extra_time -= (actual_time - earliest_time);
+			next_step_time -= (actual_time - earliest_time);
+		}
+	}
+	#endif /* ACCELERATION_TEMPORAL */
+
+	// Now we know how long we actually want to delay, so set the timer.
 	if (next_step_time < 65536) {
 		// set the comparator directly to the next real step
 		OCR1A = (next_step_time + step_start) & 0xFFFF;
@@ -172,11 +207,9 @@ void setTimer(uint32_t delay)
 	}
 
 	// Enable this interrupt, but only do it after disabling
-	// global interrupts. This will cause push any possible
+	// global interrupts (see above). This will cause push any possible
 	// timer1a interrupt to the far side of the return, protecting the 
 	// stack from recursively clobbering memory.
-	cli();
-	CLI_SEI_BUG_MEMORY_BARRIER();
 	TIMSK1 |= MASK(OCIE1A);
 
 	// restore interrupt flag