From d5033895b26c82ba11aed09105dc9af3adfd99de Mon Sep 17 00:00:00 2001
From: Markus Amsler <markus.amsler@oribi.org>
Date: Tue, 8 Feb 2011 11:10:11 +0100
Subject: [PATCH] Fix timer for high speeds.

---
 timer.c | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/timer.c b/timer.c
index dd2bbbf..44556a9 100644
--- a/timer.c
+++ b/timer.c
@@ -100,9 +100,14 @@ void setTimer(uint32_t delay)
 			timer1_compa_isr();
 		}
 		else if (next_step_time <= TICK_TIME) {
+			// next step occurs before overflow, set comparator here
 			OCR1A = next_step_time & 0xFFFF;
 			TIMSK1 |= MASK(OCIE1A);
 		}
+		else {
+			// adjust next_step_time so overflow interrupt sets the correct timeout
+			next_step_time -= TICK_TIME;
+		}
 	}
 	else {
 		next_step_time = 0;