Diff: main.cpp

Revision:

6:d766e602c318

Parent:

5:ba77f2da6fc5

--- a/main.cpp	Fri Dec 09 06:51:47 2016 +0000
+++ b/main.cpp	Mon Dec 12 05:00:34 2016 +0000
@@ -1,85 +1,84 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "TextLCD.h"
-
+ 
 LocalFileSystem local("local");
-
+ 
 InterruptIn vpace(p5);
 InterruptIn apace(p6);
 DigitalOut vsignal(p7);
 DigitalOut asignal(p8);
-
+ 
 DigitalOut asignal_led(LED1);
 DigitalOut vsignal_led(LED2);
 DigitalOut apace_led(LED3);
 DigitalOut vpace_led(LED4);
-
+ 
 Thread *heartmodeThread;
 osThreadId heartmodeTid;
 osThreadId beats;
 osThreadId displayTid;
 osThreadId HeartSenseTid;
+osThreadId ledTid;
 Timer vClock;
-
+ 
 Queue<char,256> mode_q;
 Queue<char,256> signal_q;
 Queue<char,256> obsint_q;
-
+ 
 FILE * testresults;
-
+ 
 TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2);
 RawSerial pc(USBTX, USBRX);
 char key = 'n';
 int manual_mode;
-
+ 
 volatile char c;
 volatile int mm = 0;
-
+ 
 volatile int om = 0;
 int lri = 1000;
 int uri = 700;
 int avi = 150;
 int pvarp = 300;
 int vrp = 200;
-int arp = 50;
-
-
+int arp = 25;
+ 
+ 
 int twait = 10;
 int thrsh = 1000;
 int heartmode = 0;
-int v_flag = 0;
+int v_led = 0;
+int vp_led = 0;
 int observation_interval = 10000; // In miliseconds
 int tmm = 0; //testmode flag
-
+ 
 int heart_beats = 0; // Heart-Beats (sensed or paced) since the last observation interval
 Mutex hr_mutex; //hr_mutex.lock()/unlock()
-
+ 
 void HeartSense(void const *args)
 {
     while(1) {
         osEvent ext_signal = osSignalWait(0, osWaitForever);
         int evt2 = ext_signal.value.signals;
-
+ 
         switch(evt2) {
             case(0x1):
-                vpace_led = 1;
-                v_flag = 1;
-                Thread::wait(twait);
-                vpace_led = 0;
-                v_flag = 0;
+//                v_flag = 1;
+//                v_flag = 0;
+                osSignalSet(ledTid, 0xD);
                 break;
-
+ 
             case(0x10):
-                apace_led = 1;
-                Thread::wait(twait);
-                apace_led = 0;
+                osSignalSet(ledTid, 0xC);
                 break;
         }
     }
 }
-
+ 
 void vpace_irq()
 {
+    //lcd.printf("In vpace IRQ/n");
     //heartmodeThread->signal_set(0x1);
     if(tmm)
     {
@@ -87,19 +86,20 @@
     }
     osSignalSet(HeartSenseTid, 0x1);
 }
-
-
+ 
+ 
 void apace_irq()
 {
     //heartmodeThread->signal_set(0x10);
+    //lcd.printf("In apace IRQ/n");
     if(tmm)
     {
         osSignalSet(heartmodeTid, 0x10);
     }
     osSignalSet(HeartSenseTid, 0x10);
 }
-
-
+ 
+ 
 void Rx_interrupt()
 {
     while(pc.readable()) {
@@ -123,107 +123,135 @@
         }
     }
 }
-
+ 
+void ledThread(void const *args)
+{
+    while (1)
+    {   
+        osEvent ext_signal = osSignalWait(0, osWaitForever);
+        int evt = ext_signal.value.signals;
+        
+        if (evt == 0xA)
+        {
+            asignal_led = 1;
+            Thread::wait(twait);
+            asignal_led = 0;
+        }
+        else if (evt == 0xB)
+        {
+            vsignal_led = 1;
+            v_led = 1;
+            
+            Thread::wait(twait);
+            vsignal_led = 0;
+        } 
+        else if (evt == 0xC)
+        {
+            apace_led = 1;
+            Thread::wait(twait);
+            apace_led = 0; 
+        }
+        else if (evt == 0xD)
+        {
+            vpace_led = 1;
+            vp_led = 1;
+            
+            Thread::wait(twait);
+            vpace_led = 0;
+        }    
+    }
+}
+ 
+ 
 void displayThread(void const *args)
 {
-
+ 
     while (1) {
         Thread::wait(observation_interval);
         lcd.cls();
-
+ 
         hr_mutex.lock();
         int hr = (heart_beats*60) / (observation_interval / 1000);
         heart_beats = 0;
         hr_mutex.unlock();
-
+ 
         lcd.printf("%s%d%s","HR: ", hr, " bpm");
     }
 }
-
+ 
 void h_beats(void const *args)
 {
     vClock.start();
     while(1) {
-        if(vsignal == 1 && vClock.read_ms() >= vrp) {
-            v_flag = 1;
-        }
+        if(v_led == 1) {
 
-        if(v_flag == 1) { //osSignalWait(0x1, osWaitForever); when vpace happens, v_flag is set and beats++
             hr_mutex.lock();
             heart_beats++;
             hr_mutex.unlock();
+            v_led = 0;
             vClock.reset();
-            v_flag = 0;
+        }
+ 
+        else if(vp_led == 1) { //osSignalWait(0x1, osWaitForever); when vpace happens, v_flag is set and beats++
+            hr_mutex.lock();
+            heart_beats++;
+            hr_mutex.unlock();
+            vp_led = 0;
+            vClock.reset();
+
         }
     }
 }
-
+ 
 void test_vrp_vv(FILE * results, Timer * ti)
 {
     int vrpwait = vrp/2;
     int adifu,vdifu,difl = 0;
     fprintf(results, "   Test VRP VV\n\n  Time    Action\n");
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    v_flag = 1;
-    vpace_led = 1;
-    Thread::wait(twait);
-    v_flag = 0;
-    vpace_led = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
+ 
     Thread::wait(vrpwait);
     vsignal = 1;
-    vsignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VS");
-    Thread::wait(twait);
-    vsignal_led = 0;
     vsignal = 0;
-
-
+    osSignalSet(ledTid, 0xB);
+ 
+ 
     Thread::wait(vrpwait);
     vsignal = 1;
-    difl = ti->read_us();
-    vsignal_led = 1;
+    difl = ti->read_us();  
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VS");
-    Thread::wait(twait);
     vsignal = 0;
-    vsignal_led = 0;
-
-
+    osSignalSet(ledTid, 0xB);
+ 
     osSignalWait(0x10, osWaitForever);
     adifu = ti->read_us();
-    apace_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AP");
-    Thread::wait(twait);
-    apace_led = 0;
-
-
+    osSignalSet(ledTid, 0xC);
+ 
     osSignalWait(0x1, osWaitForever);
-    vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
+    vdifu = ti->read_us(); 
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     int adif = adifu-difl;
     int vdif = vdifu-difl;
     const char * res = ((adif >= ((lri-avi)*1000 - thrsh)) &&
                         (adif < ((lri-avi)*1000 + thrsh)) &&
                         (vdif >= (lri*1000 - thrsh)) &&
                         (vdif < (lri*1000 + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d %d\n\n", res, adif, vdif);
-
-
+ 
+ 
 }
 void test_pvarp_aa(FILE * results, Timer * ti)
 {
@@ -231,460 +259,343 @@
     int vdifu = 0;
     fprintf(results, "  Test PVARP AA\n\n  Time    Action\n");
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    vpace_led = 1;
-    v_flag = 1;
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     Thread::wait(pvarpwait);
     asignal = 1;
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
     Thread::wait(pvarpwait);
     asignal = 1;
     //difl = ti->read_us();
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     osSignalWait(0x1, osWaitForever);
     vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     int vdif = vdifu;
     const char * res = ((vdif >= (uri*1000 - thrsh)) &&
                         (vdif < (uri*1000 + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d\n\n", res, vdif);
 }
-
-
+ 
+ 
 void test_pvarp_arp_aaa(FILE * results, Timer * ti)
 {
     int arpwait = arp/2;
     int vdifu = 0;
     fprintf(results, "Test PVARP ARP AAA\n\n  Time    Action\n");
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    vpace_led = 1;
-    v_flag = 1;
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     Thread::wait(pvarp);
     asignal = 1;
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     Thread::wait(arpwait);
     asignal = 1;
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     Thread::wait(arpwait);
     asignal = 1;
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     osSignalWait(0x1, osWaitForever);
     vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     int vdif = vdifu;
     const char * res = ((vdif >= (uri*1000 - thrsh)) &&
                         (vdif < (uri*1000 + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d\n\n", res, vdif);
 }
-
-
+ 
+ 
 void test_pvarp_arp_aav(FILE * results, Timer * ti)
 {
     int adifu,vdifu,difl = 0;
     fprintf(results, "Test PVARP ARP AAV\n\n  Time    Action\n");
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    vpace_led = 1;
-    v_flag = 1;
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     Thread::wait(pvarp);
     asignal = 1;
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     Thread::wait(arp);
     asignal = 1;
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     Thread::wait(20);
     vsignal = 1;
     difl = ti->read_us();
-    vsignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VS");
-    Thread::wait(twait);
     vsignal = 0;
-    vsignal_led = 0;
-
-
+    osSignalSet(ledTid, 0xB);
+ 
+ 
+ 
     osSignalWait(0x10, osWaitForever);
     adifu = ti->read_us();
-    apace_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AP");
-    Thread::wait(twait);
-    apace_led = 0;
-
-
+    osSignalSet(ledTid, 0xC);
+ 
+ 
     osSignalWait(0x1, osWaitForever);
     vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
     int adif = adifu-difl;
     int vdif = vdifu-difl;
     const char * res = ((adif >= ((lri-avi)*1000 - thrsh)) &&
                         (adif < ((lri-avi)*1000 + thrsh)) &&
                         (vdif >= (lri*1000 - thrsh)) &&
                         (vdif < (lri*1000 + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d %d\n\n", res, adif, vdif);
-
-
+ 
+ 
 }
-
-
+ 
+ 
 void test_vpace_after_a(FILE * results, Timer * ti)
 {
     int vdifu = 0;
     fprintf(results, "Test VPACE After A\n\n  Time    Action\n");
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    vpace_led = 1;
-    v_flag = 1;
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     Thread::wait(pvarp);
     asignal = 1;
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     osSignalWait(0x1, osWaitForever);
     vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
     int vdif = vdifu;
     const char * res = ((vdif >= (uri*1000 - thrsh)) &&
                         (vdif < (uri*1000 + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d\n\n", res, vdif);
 }
-
-
+ 
+ 
 void test_apace_vpace_av(FILE * results, Timer * ti)
 {
     fprintf(results, "Test APACE After V\n\n  Time    Action\n");
     int adifu,vdifu = 0;
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    vpace_led = 1;
-    v_flag = 1;
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
+    osSignalSet(ledTid, 0xD);
+ 
     osSignalWait(0x10, osWaitForever);
     adifu = ti->read_us();
-    apace_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AP");
-    Thread::wait(twait);
-    apace_led = 0;
-
-
+    osSignalSet(ledTid, 0xC);
+ 
     osSignalWait(0x1, osWaitForever);
     vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
     int adif = adifu;
     int vdif = vdifu;
     const char * res = ((adif >= ((lri-avi)*1000 - thrsh)) &&
                         (adif < ((lri-avi)*1000 + thrsh)) &&
                         (vdif >= (lri*1000 - thrsh)) &&
                         (vdif < (lri*1000 + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d %d\n\n", res, adif, vdif);
 }
-
-
+ 
+ 
 void test_avi_a(FILE * results, Timer * ti)
 {
     int vdifu,difl = 0;
     fprintf(results, "   Test AVI\n\n  Time    Action\n");
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    vpace_led = 1;
-    v_flag = 1;
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     Thread::wait(uri);
     asignal = 1;
     difl = ti->read_us();
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     osSignalWait(0x1, osWaitForever);
     vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     int vpc = (avi*1000) + difl;
     const char * res = ((vpc >= (vdifu - thrsh)) &&
                         (vpc < (vdifu + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d\n\n", res, vpc);
-
-
+ 
+ 
 }
-
-
+ 
+ 
 void test_uri_a(FILE * results, Timer * ti)
 {
     int vdifu = 0;
     fprintf(results, "   Test URI\n\n  Time    Action\n");
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    vpace_led = 1;
-    v_flag = 1;
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     Thread::wait(pvarp);
     asignal = 1;
-    asignal_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-    Thread::wait(twait);
-    asignal_led = 0;
     asignal = 0;
-
-
+    osSignalSet(ledTid, 0xA);
+ 
+ 
     osSignalWait(0x1, osWaitForever);
     vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
     int vdif = vdifu;
     const char * res = ((vdif >= (uri*1000 - thrsh)) &&
                         (vdif < (uri*1000 + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d\n\n", res, vdif);
-
-
+ 
+ 
 }
-
-
+ 
+ 
 void test_normal_10(FILE * results, Timer * ti)
 {
     fprintf(results, "  Test Normal x10\n\n  Time    Action\n");
     int adifu,vdifu,difl,i = 0;
     osSignalWait(0x10, osWaitForever);
-    apace_led = 1;
-    Thread::wait(twait);
-    apace_led = 0;
+    osSignalSet(ledTid, 0xC);
     osSignalWait(0x1, osWaitForever);
     ti->reset();
-    vpace_led = 1;
-    v_flag = 1;
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     while(i < 10) {
         Thread::wait(pvarp);
         asignal = 1;
-        asignal_led = 1;
         fprintf(results, "%6d%8s\n", ti->read_ms(), "AS");
-        Thread::wait(twait);
-        asignal_led = 0;
         asignal = 0;
-
-
+        osSignalSet(ledTid, 0xA);
+ 
+ 
         Thread::wait(75);
         vsignal = 1;
         difl = ti->read_us();
-        vsignal_led = 1;
         fprintf(results, "%6d%8s\n", ti->read_ms(), "VS");
-        Thread::wait(twait);
         vsignal = 0;
-        vsignal_led = 0;
+        osSignalSet(ledTid, 0xB);
+        
         i = i+1;
     }
-
-
+ 
+ 
     osSignalWait(0x10, osWaitForever);
     adifu = ti->read_us();
-    apace_led = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "AP");
-    Thread::wait(twait);
-    apace_led = 0;
-
-
+    osSignalSet(ledTid, 0xC);
+ 
     osSignalWait(0x1, osWaitForever);
     vdifu = ti->read_us();
-    vpace_led = 1;
-    v_flag = 1;
     fprintf(results, "%6d%8s\n", ti->read_ms(), "VP");
-    Thread::wait(twait);
-    vpace_led = 0;
-    v_flag = 0;
-
-
+    osSignalSet(ledTid, 0xD);
+ 
+ 
     int adif = adifu - difl;
     int vdif = vdifu - difl;
     const char * res = ((adif >= ((lri-avi)*1000 - thrsh)) &&
                         (adif < ((lri-avi)*1000 + thrsh)) &&
                         (vdif >= (lri*1000 - thrsh)) &&
                         (vdif < (lri*1000 + thrsh))) ? "Passed": "Failed";
-
-
+ 
+ 
     fprintf(results, "\nResult: %s %d %d\n\n", res, adif, vdif);
 }
-
-
+ 
+ 
 void testmode(void const *args)
 {
     Timer *t;
     testresults = fopen("/local/test.txt", "w");  // Open "out.txt" on the local file system for writing
     t = new Timer();
     t->start();
-
-
+ 
+ 
     test_vrp_vv(testresults, t);
     test_pvarp_aa(testresults, t);
     test_pvarp_arp_aaa(testresults, t);
@@ -696,14 +607,14 @@
     test_normal_10(testresults, t);
     fclose(testresults);
 }
-
-
-
-
+ 
+ 
+ 
+ 
 void randommode(void const *args)
 {
     int aorv,sig = 0;
-
+ 
     //osEvent ext_signal = osSignalWait(0, 1);
     //int evt = ext_signal.value.signals;
     //osSignalSet(heartmodeTid, 0x1);
@@ -711,50 +622,42 @@
     while(heartmode == 0) {
         aorv = (float)(rand()%2);
         sig = aorv ? rand()%(lri - avi) : rand()%(lri);
-
+ 
         osEvent ext_signal = osSignalWait(0, sig);
         int evt = ext_signal.value.signals;
-
+ 
         switch(evt) {
             case(0x0):
                 if(aorv) {
                     asignal = 1;
-                    asignal_led = 1;
-                    Thread::wait(twait);
-                    asignal_led = 0;
                     asignal = 0;
+                    osSignalSet(ledTid, 0xA);
                     break;
                 } else {
                     vsignal = 1;
-                    vsignal_led = 1;
-                    Thread::wait(twait);
-                    vsignal_led = 0;
                     vsignal = 0;
+                    osSignalSet(ledTid, 0xB);
                     break;
                 }
 //            case(0x1):
-//                vpace_led = 1;
+//                osSignalSet(ledTid, 0xD);
 //                v_flag = 1;
-//                Thread::wait(twait);
-//                vpace_led = 0;
 //                v_flag = 0;
 //                break;
 //
 //
 //            case(0x10):
-//                apace_led = 1;
-//                Thread::wait(twait);
-//                apace_led = 0;
+//                osSignalSet(ledTid, 0xC);
 //                break;
-
-
+ 
+ 
 //            case(0x100):
 //
         }
     }
 }
-
-
+ 
+ 
 void manualmode(void const *args)
 {
     while(1) {
@@ -762,21 +665,17 @@
         if(evt.status == osEventMessage) {
             if((char)evt.value.p == 'a') {
                 asignal = 1;
-                asignal_led = 1;
-                Thread::wait(twait);
-                asignal_led = 0;
                 asignal = 0;
+                osSignalSet(ledTid, 0xA);
             } else if((char)evt.value.p == 'v') {
                 vsignal = 1;
-                vsignal_led = 1;
-                Thread::wait(twait);
                 vsignal = 0;
-                vsignal_led = 0;
+                osSignalSet(ledTid, 0xB);
             }
         }
     }
 }
-
+ 
 void obsinterval()
 {
     char newObsInt[8];
@@ -811,25 +710,27 @@
 osThreadDef(h_beats, osPriorityNormal, DEFAULT_STACK_SIZE);
 osThreadDef(displayThread, osPriorityNormal, DEFAULT_STACK_SIZE);
 osThreadDef(HeartSense, osPriorityNormal, DEFAULT_STACK_SIZE);
-
+osThreadDef(ledThread, osPriorityNormal, DEFAULT_STACK_SIZE);
+ 
 int main()
 {
     vpace.rise(&vpace_irq);
     apace.rise(&apace_irq);
-
+ 
     pc.attach(&Rx_interrupt, RawSerial::RxIrq);
-
+ 
     //heartmodeThread = new Thread();
-
+ 
 //    Callback<void()> testmodeTask((void*)NULL,(void (*)(void *))&testmode);
-
+ 
     HeartSenseTid = osThreadCreate(osThread(HeartSense), NULL);
     heartmodeTid = osThreadCreate(osThread(randommode), NULL);
     beats = osThreadCreate(osThread(h_beats), NULL);
     displayTid = osThreadCreate(osThread(displayThread), NULL);
+    ledTid = osThreadCreate(osThread(ledThread), NULL);
 //    heartmodeTid = osThreadCreate(osThread(testmode), NULL);
 //    heartmodeTid = osThreadCreate(osThread(manualmode), NULL);
-
+ 
     while(true) {
         osEvent evt = mode_q.get();
         if(evt.status == osEventMessage) {
@@ -843,11 +744,11 @@
                     if (testresults != NULL) {
                         fclose(testresults);
                     }
-                    asignal_led = 0;
-                    vsignal_led = 0;
-                    apace_led = 0;
-                    vpace_led = 0;
-
+//                    asignal_led = 0;
+//                    vsignal_led = 0;
+//                    apace_led = 0;
+//                    vpace_led = 0;
+ 
                     displayTid = osThreadCreate(osThread(displayThread), NULL);
                     heartmodeTid = osThreadCreate(osThread(randommode), NULL);
 //                    Thread::wait(100);
@@ -858,18 +759,18 @@
                     lcd.printf("T");
                     osThreadTerminate (heartmodeTid);
                     osThreadTerminate (displayTid);
-                    asignal_led = 0;
-                    vsignal_led = 0;
-                    apace_led = 0;
-                    vpace_led = 0;
-                    osSignalClear (HeartSenseTid,0);
+//                    asignal_led = 0;
+//                    vsignal_led = 0;
+//                    apace_led = 0;
+//                    vpace_led = 0;
+                    osSignalClear (heartmodeTid,0);
                     displayTid = osThreadCreate(osThread(displayThread), NULL);
-                    HeartSenseTid = osThreadCreate(osThread(testmode), NULL);
+                    heartmodeTid = osThreadCreate(osThread(testmode), NULL);
                     
                     if (testresults != NULL) {
                         fclose(testresults);
                     }
-
+ 
                     break;
                 case('m'):
                     tmm = 0;
@@ -879,16 +780,16 @@
                     if (testresults != NULL) {
                         fclose(testresults);
                     }
-                    asignal_led = 0;
-                    vsignal_led = 0;
-                    apace_led = 0;
-                    vpace_led = 0;
-
+//                    asignal_led = 0;
+//                    vsignal_led = 0;
+//                    apace_led = 0;
+//                    vpace_led = 0;
+ 
                     displayTid = osThreadCreate(osThread(displayThread), NULL);
                     heartmodeTid = osThreadCreate(osThread(manualmode), NULL);
                     manual_mode = 1;
-
-
+ 
+ 
                     break;
                 case('o'):
                     tmm = 0;