File content as of revision 6:53fa082961d2:

#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;
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 = 25;


int twait = 10;
int thrsh = 1000;
int heartmode = 0;
int v_flag = 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;
                break;

            case(0x10):
                apace_led = 1;
                Thread::wait(twait);
                apace_led = 0;
                break;
        }
    }
}

void vpace_irq()
{
    //lcd.printf("In vpace IRQ/n");
    //heartmodeThread->signal_set(0x1);
    if(tmm)
    {
        osSignalSet(heartmodeTid, 0x1);
    }
    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()) {
        c = pc.getc();
        if(c == 'm' && om != 1) {
            mode_q.put((char*)c);
            mm = 1;
        } else if((c == 'r' || c == 't') && om != 1) {
            mode_q.put((char*)c);
            mm = 0;
        } else if((c == 'a' || c == 'v') && mm) {
            signal_q.put((char*)c);
        } else if(c == 'o' && om != 1) {
            mode_q.put((char*)c);
            om = 1;
        } else if (c == '\r' && om) {
            obsint_q.put((char*)c);
            om = 0;
        } else if ((int)c > 47 && (int)c < 58 && om) {
            obsint_q.put((char*)c);
        }
    }
}

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_flag == 1) { //osSignalWait(0x1, osWaitForever); when vpace happens, v_flag is set and beats++
            hr_mutex.lock();
            heart_beats++;
            hr_mutex.unlock();
            vClock.reset();
            v_flag = 0;
        }
    }
}

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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    v_flag = 1;
    vpace_led = 1;
    Thread::wait(twait);
    v_flag = 0;
    vpace_led = 0;


    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;


    Thread::wait(vrpwait);
    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;


    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;


    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;


    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)
{
    int pvarpwait = pvarp/2;
    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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    vpace_led = 1;
    v_flag = 1;
    Thread::wait(twait);
    vpace_led = 0;
    v_flag = 0;


    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;


    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;


    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;


    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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    vpace_led = 1;
    v_flag = 1;
    Thread::wait(twait);
    vpace_led = 0;
    v_flag = 0;


    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;


    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;


    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;


    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;


    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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    vpace_led = 1;
    v_flag = 1;
    Thread::wait(twait);
    vpace_led = 0;
    v_flag = 0;


    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;


    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;


    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;


    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;


    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;


    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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    vpace_led = 1;
    v_flag = 1;
    Thread::wait(twait);
    vpace_led = 0;
    v_flag = 0;


    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;


    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;


    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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    vpace_led = 1;
    v_flag = 1;
    Thread::wait(twait);
    vpace_led = 0;
    v_flag = 0;

    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;


    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;


    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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    vpace_led = 1;
    v_flag = 1;
    Thread::wait(twait);
    vpace_led = 0;
    v_flag = 0;


    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;


    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;


    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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    vpace_led = 1;
    v_flag = 1;
    Thread::wait(twait);
    vpace_led = 0;
    v_flag = 0;


    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;


    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;


    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;
    osSignalWait(0x1, osWaitForever);
    ti->reset();
    vpace_led = 1;
    v_flag = 1;
    Thread::wait(twait);
    vpace_led = 0;
    v_flag = 0;


    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;


        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;
        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;


    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;


    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);
    test_pvarp_arp_aav(testresults, t);
    test_vpace_after_a(testresults, t);
    test_apace_vpace_av(testresults, t);
    test_avi_a(testresults, t);
    test_uri_a(testresults, t);
    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);
    //osSignalWait(0, osWaitForever);
    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;
                    break;
                } else {
                    vsignal = 1;
                    vsignal_led = 1;
                    Thread::wait(twait);
                    vsignal_led = 0;
                    vsignal = 0;
                    break;
                }
//            case(0x1):
//                vpace_led = 1;
//                v_flag = 1;
//                Thread::wait(twait);
//                vpace_led = 0;
//                v_flag = 0;
//                break;
//
//
//            case(0x10):
//                apace_led = 1;
//                Thread::wait(twait);
//                apace_led = 0;
//                break;


//            case(0x100):
//
        }
    }
}


void manualmode(void const *args)
{
    while(1) {
        osEvent evt = signal_q.get();
        if(evt.status == osEventMessage) {
            if((char)evt.value.p == 'a') {
                asignal = 1;
                asignal_led = 1;
                Thread::wait(twait);
                asignal_led = 0;
                asignal = 0;
            } else if((char)evt.value.p == 'v') {
                vsignal = 1;
                vsignal_led = 1;
                Thread::wait(twait);
                vsignal = 0;
                vsignal_led = 0;
            }
        }
    }
}

void obsinterval()
{
    char newObsInt[8];
    int isChangingObsInt = 1;
    int i = 0;
    while(isChangingObsInt) {
        osEvent evt = obsint_q.get();
        if(evt.status == osEventMessage) {
            key = (char)evt.value.p;
            if(key != '\r' && i < 7 ) {
                newObsInt[i] = key;
                i++;
            } else if((key == '\r') && (i > 0)) {
                hr_mutex.lock();
                heart_beats = 0;
                hr_mutex.unlock();
                int obsint;
                newObsInt[i] = '\0';
                sscanf(newObsInt, "%d", &obsint);
                hr_mutex.lock();
                observation_interval = (obsint > 0 ) ? obsint: 1;
                hr_mutex.unlock();
                isChangingObsInt = 0;
                //lcd.printf("%d", observation_interval);
            }
        }
    }
}
osThreadDef(randommode, osPriorityNormal, DEFAULT_STACK_SIZE);
osThreadDef(testmode, osPriorityNormal, DEFAULT_STACK_SIZE);
osThreadDef(manualmode, osPriorityNormal, DEFAULT_STACK_SIZE);
osThreadDef(h_beats, osPriorityNormal, DEFAULT_STACK_SIZE);
osThreadDef(displayThread, osPriorityNormal, DEFAULT_STACK_SIZE);
osThreadDef(HeartSense, 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);
//    heartmodeTid = osThreadCreate(osThread(testmode), NULL);
//    heartmodeTid = osThreadCreate(osThread(manualmode), NULL);

    while(true) {
        osEvent evt = mode_q.get();
        if(evt.status == osEventMessage) {
            switch((char)evt.value.p) {
                    //pvarp = 0.3s
                case('r'):
                    tmm = 0;
                    lcd.printf("R");
                    osThreadTerminate (heartmodeTid);
                    osThreadTerminate (displayTid);
                    if (testresults != NULL) {
                        fclose(testresults);
                    }
                    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);
                    break;
                case('t'):
                    
                    tmm = 1;
                    lcd.printf("T");
                    osThreadTerminate (heartmodeTid);
                    osThreadTerminate (displayTid);
                    asignal_led = 0;
                    vsignal_led = 0;
                    apace_led = 0;
                    vpace_led = 0;
                    osSignalClear (heartmodeTid,0);
                    displayTid = osThreadCreate(osThread(displayThread), NULL);
                    heartmodeTid = osThreadCreate(osThread(testmode), NULL);
                    
                    if (testresults != NULL) {
                        fclose(testresults);
                    }

                    break;
                case('m'):
                    tmm = 0;
                    lcd.printf("M");
                    osThreadTerminate (heartmodeTid);
                    osThreadTerminate (displayTid);
                    if (testresults != NULL) {
                        fclose(testresults);
                    }
                    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;
                    lcd.printf("modeO");
                    obsinterval();
                    osThreadTerminate (displayTid);
                    displayTid = osThreadCreate(osThread(displayThread), NULL);
                    break;
            }
        }
    }
}