diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index 434d86c..4d21f4f 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -822,7 +822,7 @@ static void module_reset()
 		}
 	}
 }
-/*
+#ifndef STM32_board
 int16_t map( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
 {
 //  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
@@ -833,7 +833,7 @@ int16_t map( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t
 	x = y / (in_max - in_min) ;
 	return x  + out_min ;
 }
-*/
+#endif
 // Channel value is converted to 8bit values full scale
 uint8_t convert_channel_8b(uint8_t num)
 {
@@ -1050,6 +1050,118 @@ static void set_rx_tx_addr(uint32_t id)
 	
 #endif
 
+
+#ifndef STM32_board
+/************************************/
+/**  Arduino replacement routines  **/
+/************************************/
+// replacement millis() and micros()
+// These work polled, no interrupts
+// micros() MUST be called at least once every 32 milliseconds
+uint16_t MillisPrecount ;
+uint16_t lastTimerValue ;
+uint32_t TotalMicros ;
+uint32_t TotalMillis ;
+uint8_t Correction ;
+
+uint32_t micros()
+{
+   uint16_t elapsed ;
+   uint8_t millisToAdd ;
+   uint8_t oldSREG = SREG ;
+   cli() ;
+   uint16_t time = TCNT1 ;   // Read timer 1
+   SREG = oldSREG ;
+
+   elapsed = time - lastTimerValue ;
+   elapsed += Correction ;
+   Correction = elapsed & 0x01 ;
+   elapsed >>= 1 ;
+   
+   uint32_t ltime = TotalMicros ;
+   ltime += elapsed ;
+   cli() ;
+   TotalMicros = ltime ;   // Done this way for RPM to work correctly
+   lastTimerValue = time ;
+   SREG = oldSREG ;   // Still valid from above
+   
+   elapsed += MillisPrecount;
+   millisToAdd = 0 ;
+   
+   if ( elapsed  > 15999 )
+   {
+      millisToAdd = 16 ;
+      elapsed -= 16000 ;
+   }
+   if ( elapsed  > 7999 )
+   {
+      millisToAdd += 8 ;
+      elapsed -= 8000 ;
+   }
+   if ( elapsed  > 3999 )
+   {
+      millisToAdd += 4 ;      
+      elapsed -= 4000 ;
+   }
+   if ( elapsed  > 1999 )
+   {
+      millisToAdd += 2 ;
+      elapsed -= 2000 ;
+   }
+   if ( elapsed  > 999 )
+   {
+      millisToAdd += 1 ;
+      elapsed -= 1000 ;
+   }
+   TotalMillis += millisToAdd ;
+   MillisPrecount = elapsed ;
+   return TotalMicros ;
+}
+
+uint32_t millis()
+{
+   micros() ;
+   return TotalMillis ;
+}
+
+void delayMilliseconds(unsigned long ms)
+{
+   uint16_t start = (uint16_t)micros();
+   uint16_t lms = ms ;
+
+   while (lms > 0) {
+      if (((uint16_t)micros() - start) >= 1000) {
+         lms--;
+         start += 1000;
+      }
+   }
+}
+
+/* Important notes:
+	- Max value is 16000µs
+	- delay is not accurate due to interrupts happening */
+void delayMicroseconds(unsigned int us)
+{
+   if (--us == 0)
+      return;
+   us <<= 2;	// * 4
+   us -= 2;		// - 2
+   __asm__ __volatile__ (
+      "1: sbiw %0,1" "\n\t" // 2 cycles
+      "brne 1b" : "=w" (us) : "0" (us) // 2 cycles
+   );
+}
+
+void init()
+{
+   // this needs to be called before setup() or some functions won't work there
+   sei();
+}
+
+#endif
+
+
+
 /**************************/
 /**************************/
 /**  Interrupt routines  **/