diff --git a/IO.cpp b/IO.cpp
index d7a6ec9..068a2e1 100644
--- a/IO.cpp
+++ b/IO.cpp
@@ -94,6 +94,84 @@ m_lockout(false)
   m_boxcarFilter.pCoeffs = BOXCAR_FILTER;
   
   initInt();
+  
+  selfTest();
+}
+
+void CIO::selfTest()
+{
+  bool ledValue = false;
+
+  for (uint8_t i = 0; i < 6; i++) {
+    ledValue = !ledValue;
+
+    // We exclude PTT to avoid trigger the transmitter
+    setLEDInt(ledValue);
+    setCOSInt(ledValue);
+#if defined(ARDUINO_MODE_PINS)
+    setDStarInt(ledValue);
+    setDMRInt(ledValue);
+    setYSFInt(ledValue);
+    setP25Int(ledValue);
+#endif
+    delayInt(250);
+  }
+
+#if defined(ARDUINO_MODE_PINS)
+  setDStarInt(true);
+  setDMRInt(false);
+  setYSFInt(false);
+  setP25Int(false);
+
+  delayInt(250);
+
+  setDStarInt(true);
+  setDMRInt(true);
+  setYSFInt(false);
+  setP25Int(false);
+
+  delayInt(250);
+
+  setDStarInt(true);
+  setDMRInt(true);
+  setYSFInt(true);
+  setP25Int(false);
+  
+  delayInt(250);
+
+  setDStarInt(true);
+  setDMRInt(true);
+  setYSFInt(true);
+  setP25Int(true);
+
+  delayInt(250);
+  
+  setDStarInt(true);
+  setDMRInt(true);
+  setYSFInt(true);
+  setP25Int(false);
+
+  delayInt(250);
+
+  setDStarInt(true);
+  setDMRInt(true);
+  setYSFInt(false);
+  setP25Int(false);
+
+  delayInt(250);
+  
+  setDStarInt(true);
+  setDMRInt(false);
+  setYSFInt(false);
+  setP25Int(false);
+
+  delayInt(250);
+
+  setDStarInt(false);
+  setDMRInt(false);
+  setYSFInt(false);
+  setP25Int(false);
+#endif
 }
 
 void CIO::start()
diff --git a/IO.h b/IO.h
index 6c2fe7d..64fb1d9 100644
--- a/IO.h
+++ b/IO.h
@@ -53,6 +53,8 @@ public:
 
   void resetWatchdog();
   uint32_t getWatchdog();
+  
+  void selfTest();
 
 private:
   bool                 m_started;
@@ -107,6 +109,8 @@ private:
   void setDMRInt(bool on);
   void setYSFInt(bool on);
   void setP25Int(bool on);
+  
+  void delayInt(unsigned int dly);
 };
 
 #endif
diff --git a/IODue.cpp b/IODue.cpp
index 8d1e034..6eeeb82 100644
--- a/IODue.cpp
+++ b/IODue.cpp
@@ -226,4 +226,9 @@ void CIO::setP25Int(bool on)
   digitalWrite(PIN_P25, on ? HIGH : LOW);
 }
 
+void CIO::delayInt(unsigned int dly)
+{
+  delay(dly);
+}
+
 #endif
diff --git a/IOSTM.cpp b/IOSTM.cpp
index 3abf820..e2f9b00 100644
--- a/IOSTM.cpp
+++ b/IOSTM.cpp
@@ -764,4 +764,30 @@ void CIO::setP25Int(bool on)
 #endif
 }
 
+// Simple delay function for STM32
+// Example from: http://thehackerworkshop.com/?p=1209
+void CIO::delayInt(unsigned int dly)
+{
+#if defined(STM32F7_NUCLEO)
+  unsigned int loopsPerMillisecond = (SystemCoreClock/1000);
+#else
+  unsigned int loopsPerMillisecond = (SystemCoreClock/1000) / 3;
+#endif
+
+  for (; dly > 0; dly--)
+  {
+    asm volatile //this routine waits (approximately) one millisecond
+    (
+      "mov r3, %[loopsPerMillisecond] \n\t" //load the initial loop counter
+      "loop: \n\t"
+        "subs r3, #1 \n\t"
+        "bne loop \n\t"
+
+      : //empty output list
+      : [loopsPerMillisecond] "r" (loopsPerMillisecond) //input to the asm routine
+      : "r3", "cc" //clobber list
+    );
+  }
+}
+
 #endif
diff --git a/IOTeensy.cpp b/IOTeensy.cpp
index 3f4d9b9..de0651a 100644
--- a/IOTeensy.cpp
+++ b/IOTeensy.cpp
@@ -210,4 +210,9 @@ void CIO::setP25Int(bool on)
   digitalWrite(PIN_P25, on ? HIGH : LOW);
 }
 
-#endif
+void CIO::delayInt(unsigned int dly)
+{
+  delay(dly);
+}
+
+#endif