Adding LEDs selftest blink

IO.cpp

@@ -58,9 +58,41 @@ m_watchdog(0U)
   SDATA_pin(LOW);
   SLE_pin(LOW);
 
+  selfTest();
+
   m_modeTimerCnt = 0;
 }
 
+void CIO::selfTest()
+{
+  bool ledValue = false;
+  uint32_t ledCount = 0;
+  uint32_t blinks = 0;
+
+  while(true) {
+    ledCount++;
+    delay_us(1000);
+
+    if(ledCount >= 125U) {
+      ledCount = 0U;
+      ledValue = !ledValue;
+
+      LED_pin(!ledValue);
+      PTT_pin(ledValue);
+      DSTAR_pin(ledValue);
+      DMR_pin(ledValue);
+      YSF_pin(ledValue);
+      P25_pin(ledValue);
+      COS_pin(ledValue);
+
+      blinks++;
+
+      if(blinks > 5)
+        break;
+    }
+  }
+}
+
 void CIO::process()
 {
   uint8_t bit;

IO.h

@@ -80,8 +80,6 @@ public:
 #if defined(DUPLEX)
   void      interrupt2(void);
 #endif
-  void     resetWatchdog(void);
-  uint32_t getWatchdog(void);
   
 #if defined(BIDIR_DATA_PIN)
   void      Data_dir_out(bool dir);
@@ -97,6 +95,9 @@ public:
   void      setMode(MMDVM_STATE modemState);
   void      setDecode(bool dcd);
   void      setLoDevYSF(bool ysfLoDev);
+  void      resetWatchdog(void);
+  uint32_t  getWatchdog(void);
+  void      selfTest(void);
 
   // RF interface API
   void      setTX(void);