From 70e1fd4264482f3e8dbe208ea49414163089eed6 Mon Sep 17 00:00:00 2001
From: Andy CA6JAU <juribeparada@gmail.com>
Date: Mon, 28 Aug 2017 00:43:24 -0300
Subject: [PATCH] Adding LEDs selftest at start-up

---
 IO.cpp       | 32 ++++++++++++++++++++++++++++++++
 IO.h         |  4 ++++
 IODue.cpp    |  5 +++++
 IOSTM.cpp    | 19 +++++++++++++++++++
 IOTeensy.cpp |  7 ++++++-
 5 files changed, 66 insertions(+), 1 deletion(-)

diff --git a/IO.cpp b/IO.cpp
index d7a6ec9..16d46fe 100644
--- a/IO.cpp
+++ b/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()
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..8082cfb 100644
--- a/IOSTM.cpp
+++ b/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
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