Adding LEDs selftest at start-up

IO.cpp

@@ -94,6 +94,38 @@ m_lockout(false)
   m_boxcarFilter.pCoeffs = BOXCAR_FILTER;
   
   initInt();
+  
+  selfTest();
+}
+
+void CIO::selfTest()
+{
+  bool ledValue = false;
+  uint32_t ledCount = 0;
+  uint32_t blinks = 0;
+
+  while(true) {
+    ledCount++;
+    delayInt(100);
+
+    if(ledCount >= 2U) {
+      ledCount = 0U;
+      ledValue = !ledValue;
+
+      setLEDInt(ledValue);
+      setPTTInt(ledValue);
+      setDStarInt(ledValue);
+      setDMRInt(ledValue);
+      setYSFInt(ledValue);
+      setP25Int(ledValue);
+      setCOSInt(ledValue);
+
+      blinks++;
+
+      if(blinks > 5)
+        break;
+    }
+  }
 }
 
 void CIO::start()

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

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

IOSTM.cpp

@@ -764,4 +764,23 @@ void CIO::setP25Int(bool on)
 #endif
 }
 
+void CIO::delayInt(unsigned int dly)
+{
+  unsigned int loopsPerMillisecond = (SystemCoreClock/1000) / 3; //3 clock cycles per loop
+  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

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