From fc0b55b213340e9d51a2165303672e9042e3fe42 Mon Sep 17 00:00:00 2001
From: Robert Pelnar <robert.pelnar@seznam.cz>
Date: Sun, 27 Jan 2019 17:23:53 +0100
Subject: [PATCH] Timing functions micros2 and delay2 + fix of
 delay_keep_alive().

---
 Firmware/Marlin.h        |  1 +
 Firmware/Marlin_main.cpp |  4 ++--
 Firmware/temperature.cpp |  5 -----
 Firmware/timer02.c       | 39 +++++++++++++++++++++++++++++++++++++--
 Firmware/timer02.h       | 31 +++++++++++++++++++++++++++++++
 5 files changed, 71 insertions(+), 9 deletions(-)
 create mode 100644 Firmware/timer02.h

diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 6d965a99..4bb8e644 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -17,6 +17,7 @@
 #include <avr/eeprom.h>
 #include <avr/interrupt.h>
 
+#include "timer02.h"
 
 #include "fastio.h"
 #include "Configuration.h"
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 869db774..e855db81 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -7809,10 +7809,10 @@ void delay_keep_alive(unsigned int ms)
         if (ms == 0)
             break;
         else if (ms >= 50) {
-            delay(50);
+            delay2(50);
             ms -= 50;
         } else {
-            delay(ms);
+			delay2(ms);
             ms = 0;
         }
     }
diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index ddffcfac..e8ee38e9 100644
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -45,11 +45,6 @@
 #include "Configuration_prusa.h"
 
 
-extern "C" {
-extern void timer02_init(void);
-extern void timer02_set_pwm0(uint8_t pwm0);
-}
-
 
 //===========================================================================
 //=============================public variables============================
diff --git a/Firmware/timer02.c b/Firmware/timer02.c
index 8db3cac7..921aaed3 100644
--- a/Firmware/timer02.c
+++ b/Firmware/timer02.c
@@ -87,7 +87,6 @@ ISR(TIMER2_OVF_vect)
 	// (volatile variables must be read from memory on every access)
 	unsigned long m = timer0_millis;
 	unsigned char f = timer0_fract;
-
 	m += MILLIS_INC;
 	f += FRACT_INC;
 	if (f >= FRACT_MAX)
@@ -95,9 +94,45 @@ ISR(TIMER2_OVF_vect)
 		f -= FRACT_MAX;
 		m += 1;
 	}
-
 	timer0_fract = f;
 	timer0_millis = m;
 	timer0_overflow_count++;
 }
 
+unsigned long micros2()
+{
+	unsigned long m;
+	uint8_t oldSREG = SREG, t;
+	cli();
+	m = timer0_overflow_count;
+#if defined(TCNT2)
+	t = TCNT2;
+#elif defined(TCNT2L)
+	t = TCNT2L;
+#else
+	#error TIMER 2 not defined
+#endif
+#ifdef TIFR2
+	if ((TIFR2 & _BV(TOV2)) && (t < 255))
+		m++;
+#else
+	if ((TIFR & _BV(TOV2)) && (t < 255))
+		m++;
+#endif
+	SREG = oldSREG;	
+	return ((m << 8) + t) * (64 / clockCyclesPerMicrosecond());
+}
+
+void delay2(unsigned long ms)
+{
+	uint32_t start = micros2();
+	while (ms > 0)
+	{
+		yield();
+		while ( ms > 0 && (micros2() - start) >= 1000)
+		{
+			ms--;
+			start += 1000;
+		}
+	}
+}
diff --git a/Firmware/timer02.h b/Firmware/timer02.h
new file mode 100644
index 00000000..fd9570d9
--- /dev/null
+++ b/Firmware/timer02.h
@@ -0,0 +1,31 @@
+//timer02.h
+// use atmega timer2 as main system timer instead of timer0
+// timer0 is used for fast pwm (OC0B output)
+// original OVF handler is disabled
+#ifndef TIMER02_H
+#define TIMER02_H
+
+#include <inttypes.h>
+
+
+#if defined(__cplusplus)
+extern "C" {
+#endif //defined(__cplusplus)
+
+
+extern uint8_t timer02_pwm0;
+
+extern void timer02_set_pwm0(uint8_t pwm0);
+
+extern void timer02_init(void);
+
+extern unsigned long micros2(void);
+
+extern void delay2(unsigned long ms);
+
+
+#if defined(__cplusplus)
+}
+#endif //defined(__cplusplus)
+
+#endif //TIMER02_H