diff --git a/src/ArduinoZeroTemplate.c b/src/ArduinoZeroTemplate.c
index 604c81da5e8524a88c345101b91a2ea5ad4cb8fa..c3ce3b2493496c279fd7968e19a5d243eda57164 100644
--- a/src/ArduinoZeroTemplate.c
+++ b/src/ArduinoZeroTemplate.c
@@ -23,10 +23,11 @@ struct spi_module spi0_instance_struct;
 uint64_t t1ms_timer = 0;
 static uint64_t	next_ms=0;			// To save the time of next ms. Value in ticks of the RTC clock. Global so it can be initialized.
 
+// Other variables for peripherals:
+volatile uint8_t usart5_rx_buffer[MAX_RX_BUFFER_LENGTH];
+uint8_t usart5_tx_buffer[5];
 
-/* Other variables for peripherals:
-volatile uint8_t rx_buffer[MAX_RX_BUFFER_LENGTH];
-
+/*
 // For I2C Master
 static uint8_t write_buffer[DATA_LENGTH];
 static uint8_t read_buffer[DATA_LENGTH];
@@ -124,7 +125,7 @@ void ArduinoZeroTemplateInit(void)
 		// USART 5, to/from PC
 //		port_get_config_defaults(&port_config_struct);
 //		port_config_struct.direction  = PORT_PIN_DIR_OUTPUT;
-//		port_pin_set_config(PIN_PB22, &port_config_struct);		// TX
+//		port_pin_set_config(PIN_PB22, &port_config_struct);		// TX	This should not bee needed but better check.
 
 		usart_get_config_defaults(&usart_config_struct);
 		usart_config_struct.generator_source = GCLK_GENERATOR_1;
@@ -139,16 +140,22 @@ void ArduinoZeroTemplateInit(void)
 		{
 		}
 		usart_enable(&usart5_instance_struct);
-		//			usart_enable_callback(&usart5_instance_struct, USART_CALLBACK_BUFFER_RECEIVED);
+		usart_enable_callback(&usart5_instance_struct, USART_CALLBACK_BUFFER_RECEIVED);
 
 		// How to use in callback mode:
 		//	ASF use all interrupt driven code as callbacks. That means that the address of the interrupt service routine
 		//	is not defined at compile time. Instead the address of the function that is to be called by the interrupt
-		//	must be registered at run-time. This method is defined as callback. It makes more	sense in OS driven systems.
+		//	must be registered at run-time. This method is defined as callback.
+		// It makes more sense in OS driven systems.
 			
 		//	Receive a specific nr of bytes: An interrupt is triggered on data receive which calls usart_read_buffer_job. 
 		//	Send a buffer: usart_write_buffer_wait. Holds until the buffer is sent.
 
+		// Test to send a string with length 5:
+		usart_write_buffer_wait(&usart5_instance_struct, usart5_tx_buffer, 5);
+
+callback routine for usart 5:
+....
 
 	/* USART if used for I2C
 		// USART 2, I2C
@@ -199,20 +206,21 @@ void ArduinoZeroTemplateInit(void)
 
 		// Output is probably enabled by dac_init or dac_enable but otherwise it must be done also.
 
+	Init1msClock();
 } // end init
 
 
 void Init1msClock(void)
 {
 	next_ms = rtc_count_get_count(&rtc_instance_struct)+1;		// 1 ms from now.
-
 }
 
 void Update1msClock(void)
 {
 	// Create a 1 ms clock (0.9765625 ms). Counts in 64 bit mode so it will last to the end of the world.
 	// To not create unnecessary large disturbance on the application this is not interrupt driven.
-	// The counter is supposed to count exact ms but the result from the crystal driven counter will meet exactly only once every second. (32768 = 2^15)
+	// Instead it will check the RTC timer and increment the ms counter several steps as needed if it misses because of longer delays.
+	// The counter is supposed to count exact ms but the result from the crystal driven counter will give exact match only once every second. (32768 = 2^15)
 	// This results in that the ms counter will count a little to slow.
 	// Therefore the ms counter is optionally error compensated once every second.
 	// Turning it on results in that 7 (false) ms is removed every second.
@@ -226,8 +234,10 @@ void Update1msClock(void)
 	#endif
 
 	rtc_64_bit_counter = rtc_count_get_count(&rtc_instance_struct);	// This is the time right now. Can lock 30.5 �s to synchronize the system clock to the slower RTC clock
-	// Check for wrap-around. (There is a small risk that a wrap around happens just between reading RTC value and the check).
-	if (RTC->MODE0.INTFLAG.bit.OVF != 0)
+	// Check for wrap-around of RTC clock. Since the RTC clock is 32 bits this will happen after a little more than 1 hour for a 1 ms timer.
+	// To avoid the (unlikely) situation that there is an overflow just after the	reading of the RTC counter, it also check if the value was 0xFFFFFFFF
+	// Any overflow will be solved next check.
+	if ( (rtc_64_bit_counter != 0xFFFFFFFF) &&  (RTC->MODE0.INTFLAG.bit.OVF != 0) )
 	{
 		rtc_64_bit_counter += 100000000;	// Add one to the 64 bit domain.
 		RTC->MODE0.INTFLAG.reg = 0x80;		// Clear the interrupt flag
@@ -237,12 +247,14 @@ void Update1msClock(void)
 	while(rtc_64_bit_counter >= next_ms)	// Have we reached next ms?
 	{
 		added_ms++;					// Count up one 1 ms
-		msErrorCounter++;		// Count up the error counter to catch exactly 1 second
-					if (msErrorCounter>=993)	// This is the value the ms counter will have then it should have 1000
-					{
-						msErrorCounter=0;
-						t1ms_timer = 1000;	// At exit any missed ms will also be added. This happens if the time of 1 second is missed and is OK.
-					}
+		#ifdef MS_COUNTER_ERROR_CORRECTION_ON
+			msErrorCounter++;						// Count up the error counter to catch exactly 1 second
+			if (msErrorCounter >= 993)	// This is the value the ms counter will have then it should have 1000
+			{
+				msErrorCounter=0;
+				t1ms_timer = 1000;		// At exit any missed ms will also be added. This happens if the time of 1 second is missed and is OK.
+			}
+		#endif
 		rtc_64_bit_counter -= 1;	// Remove count for another ms
 		port_pin_toggle_output_level(LED_0_PIN);		// Tick the LED to check time
 	}