File content as of revision 3:77efff091ef1:

#include "mbed.h"
#include "rtos.h"
#include "TextLCD.h"
#include <stdio.h>

InterruptIn vsignal(p7);
InterruptIn asignal(p8);
DigitalOut Vpace(p5);
DigitalOut Apace(p6);

DigitalOut asense_led(LED1);
DigitalOut vsense_led(LED2);
DigitalOut apace_led(LED3);
DigitalOut vpace_led(LED4);

Thread *pacemodeThread;

osThreadId signalTid;
osThreadId senseTid;
osThreadId displayTid;
osThreadId pacemodeTid;
osThreadId alarmTid;

TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2);
RawSerial pc(USBTX, USBRX);

Timer vClock;
Timer aClock;               //PaceSignal model
Timer arpClock;

//RtosTimer *apace_timer;
//RtosTimer *vpace_timer;
//RtosTimer *vpace_timer2;

double LRI = 1000;
double URI = 700;
double VRP = 200;                // V noise interval
double ARP = 50;                // A noise interval
double AVI = 500;          // A-V max interval
double PVARP = 300;      // V-A max interval
double ratio;
int wait_period = 10;       // 3a requirement

int observation_interval = 10000; // In miliseconds
int upperBound; //for mode changes
int lowerBound; //for mode changes
int heart_beats = 0; // Heart-Beats (sensed or paced) since the last observation interval
char mode = 'n';
char key = 'n';
char newObsInt[8];
int manual_mode = 0;
Mutex hr_mutex; //hr_mutex.lock()/unlock()

Queue<char,256> mode_q;
Queue<char,256> signal_q;
Queue<char,256> obsint_q;

volatile char c;
volatile int mm = 0;
volatile int om = 0;
int mm_flag = 0;

void initialize_intervals()
{
    LRI = 1000;
    URI = 700;
//    VRP = 200;
//    ARP = 50;
//    AVI = 150;
//    PVARP = 300;
}

void Rx_interrupt()
{
    while(pc.readable()) {
        c = pc.getc();
        if(c == 'm' && om != 1) {
            mode_q.put((char*)c);
            mm = 1;
        } else if(c == 'n' || c == 'e' || c == 's' && 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);
        }
    }
}

// Function to toggle the LEDs 1,2,3,4
void toggleLed(int led)
{
    switch (led) {
        case (1):
            asense_led = 1;
            Thread::wait(wait_period);
            asense_led = 0;
            break;
        case (2):
            vsense_led = 1;
            Thread::wait(wait_period);
            vsense_led = 0;
            break;
        case (3):
            apace_led = 1;
            Thread::wait(wait_period);
            apace_led = 0;
            break;
        case (4):
            vpace_led = 1;
            Thread::wait(wait_period);
            vpace_led = 0;
            break;
    }
}

void alarmThread(void const *args)
{
    while (1) {
        osEvent ext_signal = osSignalWait(0, osWaitForever);
        int evt = ext_signal.value.signals;

        if (evt == 0xb) {
            lcd.printf("%s", "\nALARM HIGH");
        } else if (evt == 0xc) {
            lcd.printf("%s", "\nALARM LOW");
        }
    }
}

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

        if (hr > upperBound) {
            osSignalSet(alarmTid, 0xb);
        } else if (hr < lowerBound) {
            osSignalSet(alarmTid, 0xc);
        }
    }
}



// Incoming signal from the heart
void asignal_irq()
{
    osSignalSet(signalTid, 0x1);
}

// Incoming signal from the heart
void vsignal_irq()
{
    osSignalSet(signalTid, 0x2);
}


// Timer-driven function to pace the Atrial
void a_pace(void const*)
{
    Apace = 1;
    aClock.reset();
//    apace_timer->stop();
    Apace = 0;
    osSignalSet(signalTid, 0x3);
    
    toggleLed(3);
}

// Timer-driven function to pace the ventrical
void v_pace(void const*)
{
    Vpace = 1;

    vClock.reset();
//    vpace_timer->start(LRI);
//    apace_timer->start(LRI-AVI);
    Vpace = 0;

    hr_mutex.lock();
    osSignalSet(signalTid, 0x4);
    heart_beats++;
    hr_mutex.unlock();

    toggleLed(4);
}



void PaceSignal(void const *args)
{
    int pFlag1 = 0;
    int pFlag2 = 0;
    vClock.start();
    aClock.start();
    arpClock.start();
//    if(!mm_flag) {
//        vpace_timer->start(LRI);
//        apace_timer->start(LRI-AVI);
//    }
//    vpace_timer2->start(AVI);
    while(1) {
        while (!pFlag1) {
            osEvent ext_signal = osSignalWait(0, osWaitForever);
            int evt = ext_signal.value.signals;

            //lcd.printf("%d",evt); // 4(Vpace), 3(Apace), 2(Vsignal), 1(Asignal)

            if (evt == 0x1 && vClock.read_ms() >= PVARP) { //aSense
                osSignalSet(senseTid, 0x1);
                aClock.reset();
                arpClock.reset();
                pFlag1 = 1;
            } else if(evt == 0x2 && vClock.read_ms() >= VRP) { //vSignal
                osSignalSet(senseTid, 0x2);
                vClock.reset();
//                if(!mm_flag) {
//                    vpace_timer->start(LRI);
//                    apace_timer->start(LRI-AVI);
//                }
            } else if (evt == 0x3) { //aPace
                pFlag1 = 1;
            }
        }
        pFlag1 = 0;
        lcd.cls();
        lcd.printf("PS1");
        while(!pFlag2) {

            osEvent ext_signal = osSignalWait(0, osWaitForever);
            int evt = ext_signal.value.signals;

            //lcd.printf("%d",evt);

            if (evt == 0x1 && arpClock.read_ms() >= ARP) { //aSignal
                osSignalSet(senseTid, 0x1);
                arpClock.reset();
//                aClock.reset();
            } else if(evt == 0x2) { //vSense
                hr_mutex.lock();
                osSignalSet(senseTid, 0x2);
                heart_beats++;
                vClock.reset();
                hr_mutex.unlock();
//                if(!mm_flag) {
//                    vpace_timer->start(LRI);
//                    apace_timer->start(LRI-AVI);
//                }
                pFlag2 = 1;
            } else if (evt == 0x4) { //vPace
                pFlag2 = 1;
            }
        }
        pFlag2 = 0;
        lcd.cls();
        lcd.printf("PS2");
    }
}


void PaceSense(void const *args)
{
    int interval;
    int pFlag1 = 0;
    int pFlag2 = 0;
    while(1) {
        while (!pFlag1) {
            osEvent ext_signal = osSignalWait(0, (LRI-AVI));
            int evt = ext_signal.value.signals;

            //lcd.printf("%d",evt);
            if (evt == 0x3) { //aPace 0x0
                Apace = 1;
                Thread::wait(1);
                Apace = 0;
                osSignalSet(signalTid, 0x3);
                aClock.reset();
                arpClock.reset();
                
                toggleLed(3);
                pFlag1 = 1;
            }
            if (evt == 0x1) { //aSense
                if(!mm_flag) {
//                    vpace_timer->stop();
                    interval = (vClock.read_ms() + AVI >= URI) ? AVI : URI - vClock.read_ms();
                }
//                if(mm_flag)
                toggleLed(evt);
                pFlag1 = 1;
            } else if(evt == 0x2) { //vSense
                toggleLed(evt);
            }
//            } else if (evt == 0x3) { //aPace
//                aClock.reset();
//                arpClock.reset();
//                pFlag1 = 1;
//            }
        }
        pFlag1 = 0;
        lcd.cls();
        lcd.printf("PS3");
        while(!pFlag2) {
            lcd.printf("%d", interval);
            osEvent ext_signal = osSignalWait(0, interval);
            
            int evt = ext_signal.value.signals;
            
            //lcd.printf("%d",evt); // 4096, 256, 16, 1
            if (evt == 0x4) { //vPace 0x0
                hr_mutex.lock();
                Vpace = 1;
                vClock.reset();
                Thread::wait(1);
                Vpace = 0;
                osSignalSet(signalTid, 0x4);
                heart_beats++;
                hr_mutex.unlock();

                toggleLed(4);
                pFlag2 = 1;
            } else if (evt == 0x1) { //aSense
                toggleLed(evt);
            } else if(evt == 0x2) { //vSense
                toggleLed(evt);
                pFlag2 = 1;
            }
        }
        pFlag2 = 0;
        lcd.cls();
        lcd.printf("PS4");
    }
}

void normalmode(void const *args)
{
    initialize_intervals();
    mode = 'n';
    lcd.printf("N");
    upperBound = 100; //beats per msecond
    lowerBound = 40; //beats per msecond
    //reset obs interval
    hr_mutex.lock();
    heart_beats = 0;
    hr_mutex.unlock();

    vClock.reset();
    aClock.reset();
//    vpace_timer->start(LRI);
//    apace_timer->start(LRI-AVI);
}

void exercisemode(void const *args)
{
    initialize_intervals();
    mode = 'e';
    lcd.printf("E");
    upperBound = 175; //beats per msecond
    lowerBound = 100; //beats per msecond
    ratio = (175.00/100.00 + 100.00/40.00) / 2.00;
    LRI /= ratio;
    URI /= ratio;
    //reset obs interval
    hr_mutex.lock();
    heart_beats = 0;
    hr_mutex.unlock();

    vClock.reset();
    aClock.reset();
//    vpace_timer->start(LRI);
//    apace_timer->start(LRI-AVI);
}

void sleepmode(void const *args)
{
    initialize_intervals();
    mode = 's';
    lcd.printf("S");
    upperBound = 60; //beats per msecond
    lowerBound = 30; //beats per msecond v-v 0.5s
    ratio = (60.00/100.00 + 30.00/40.00) / 2.00;
    LRI /= ratio;
    URI /= ratio;
    //reset obs interval
    hr_mutex.lock();
    heart_beats = 0;
    hr_mutex.unlock();

    vClock.reset();
    aClock.reset();
//    vpace_timer->start(LRI);
//    apace_timer->start(LRI-AVI);
}

void m_vpace()
{
    vClock.reset();
    Vpace = 1;
    Thread::wait(1);
    Vpace = 0;
    osSignalSet(signalTid, 0x4);
    hr_mutex.lock();
    heart_beats++;
    hr_mutex.unlock();
    
    toggleLed(4);
}

void m_apace()
{
    aClock.reset();
    Apace = 1;
    Thread::wait(1);
    Apace = 0;
    osSignalSet(signalTid, 0x3);
    
    toggleLed(3);
}

void manualmode(void const *args)
{
    upperBound = 175; //beats per msecond
    lowerBound = 30; //beats per msecond
    lcd.printf("M");
    mode = 'm';
    LRI = 2125; // max V-V (LRI) based on exercise mode
    URI = 675; // min V-V (URI) based on sleep mode

    while(1) {
        osEvent evt = signal_q.get();
        if(evt.status == osEventMessage) {
            if((char)evt.value.p == 'v') {
                m_vpace();
            } else if((char)evt.value.p == 'a') {
                m_apace();
            }
        }
    }
}

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)) {
                heart_beats = 0;
                int obsint;
                newObsInt[i] = '\0';
                sscanf(newObsInt, "%d", &obsint);
                observation_interval = (obsint > 0 ) ? obsint: 1;
                isChangingObsInt = 0;
                lcd.printf("%d", observation_interval);
            }
        }
    }
}

osThreadDef(PaceSignal, osPriorityNormal, DEFAULT_STACK_SIZE);
osThreadDef(PaceSense, osPriorityNormal, DEFAULT_STACK_SIZE);
osThreadDef(alarmThread, osPriorityBelowNormal, DEFAULT_STACK_SIZE); //priority BelowNormal

osThreadDef(displayThread, osPriorityLow, DEFAULT_STACK_SIZE); //priority Low
osThreadDef(manualmode, osPriorityNormal, DEFAULT_STACK_SIZE);
//osThreadDef(normalmode, osPriorityNormal, DEFAULT_STACK_SIZE);
//osThreadDef(exercisemode, osPriorityNormal, DEFAULT_STACK_SIZE);
//osThreadDef(sleepmode, osPriorityNormal, DEFAULT_STACK_SIZE);


////////////////////////////////////////////
////////////////////////////////////////////
////////////////////////////////////////////

int main()
{
    alarmTid = osThreadCreate(osThread(alarmThread), NULL);
    senseTid = osThreadCreate(osThread(PaceSense), NULL);
    signalTid = osThreadCreate(osThread(PaceSignal), NULL);
    displayTid = osThreadCreate(osThread(displayThread), NULL);
//    pacemodeTid = osThreadCreate(osThread(normalmode), NULL);
    normalmode(NULL);
    
    vsignal.rise(&vsignal_irq); //rising edge of timer
    asignal.rise(&asignal_irq);

//    Callback<void()> apaceTimerTask((void*)NULL, (void (*)(void*))&a_pace);
//    Callback<void()> vpaceTimerTask((void*)NULL, (void (*)(void*))&v_pace);
//    apace_timer = new RtosTimer(apaceTimerTask);
//    vpace_timer = new RtosTimer(vpaceTimerTask);

    lcd.cls();

    pc.attach(&Rx_interrupt, RawSerial::RxIrq);
    while(true) {
//        lcd.printf("test1");
        osEvent evt = mode_q.get();
        if(evt.status == osEventMessage) {
            switch((char)evt.value.p) {
                case('n'):
                    mm_flag = 0;
                    lcd.printf("paceN");
                    osThreadTerminate (pacemodeTid);
                    osThreadTerminate (displayTid);
//                    pacemodeTid = osThreadCreate(osThread(normalmode), NULL);
                    normalmode(NULL);
                    displayTid = osThreadCreate(osThread(displayThread), NULL);
                    lcd.printf("%d", observation_interval);
                    break;
                case('s'):
                    mm_flag = 0;
                    lcd.printf("paceS");
                    osThreadTerminate (pacemodeTid);
                    osThreadTerminate (displayTid);
//                    pacemodeTid = osThreadCreate(osThread(sleepmode), NULL);
                    sleepmode(NULL);
                    displayTid = osThreadCreate(osThread(displayThread), NULL);
                    break;
                case('e'):
                    mm_flag = 0;
                    lcd.printf("paceE");
                    osThreadTerminate (pacemodeTid);
                    osThreadTerminate (displayTid);
//                    pacemodeTid = osThreadCreate(osThread(exercisemode), NULL);
                    exercisemode(NULL);
                    displayTid = osThreadCreate(osThread(displayThread), NULL);
                    lcd.printf("%d", observation_interval);
                    break;
                case('m'):
                    mm_flag = 1;
                    osThreadTerminate (pacemodeTid);
                    //osThreadTerminate (displayTid);
//                    apace_timer->stop();
//                    vpace_timer->stop();
                    pacemodeTid = osThreadCreate(osThread(manualmode), NULL);
//                    displayTid = osThreadCreate(osThread(displayThread), NULL);
                    manual_mode = 1;
                    break;
                case('o'):
                    lcd.printf("modeO");
                    hr_mutex.lock();
                    obsinterval();
                    hr_mutex.unlock();
                    osThreadTerminate (displayTid);
                    displayTid = osThreadCreate(osThread(displayThread), NULL);
                    break;
            }
        }
    }
}