File content as of revision 17:8c76a8b1f46f:

#include "mbed.h"
#include "functions.h"

#define PID_REQUEST         0x7DF
//#define PID_REQUEST         0x18DB33F1
#define PID_REPLY           0x7E8
//#define PID_REPLY           0x18DAF110

#define CANSPEED_500      500000
#define CANSPEED_250      250000
#define CANSPEED_125      125000
#define sendDebugMessage  1
#define pushDebugData 1

const int canSpeeds[3]= {CANSPEED_125,CANSPEED_500,CANSPEED_250};
//Serial serialPorts[2] = {Serial(P0_11,P0_13),Serial(P0_15,P0_14)};
Serial *serialPorts[2] = {NULL,NULL};
Serial *kLineUART=NULL;
DigitalOut *klineWakeup=NULL;
DigitalOut *canDisableKey=NULL;
CAN *canBus=NULL;

Ticker timeCounter;
Timer OBDTimer;
CANMessage can_MsgRx;
CANMessage can_MsgTx;
char kLineCmd[7]= {'\0','\0','\0','\0','\0','\0'};

void clearOBDCommand();
void flushBuffer();
void processUserCommand(char *_incomingDataInInterFace);
void sendCommandToVehicle(char *_cmd);
void setKLineMsg(char *_cmd);
void flushBuffer();
void kLinekeepalive();
char tolower(unsigned char ch);
int findStringLength(char *_cmd);
int checkCarStatus();
void initPins();

int _powerSaveFlag=0;
int isKeepAliveCommand=0;
int isPoweredupNow=1;
int interFace=1;
int debug=1;
int currentOBDProtocol=0;
const int protCan=1;
const int protkLine9141=2;
const int protkLine14230=3;
int canFrequency;
int cmd;
char can_msg[8];
char *incomingDataInInterFace;
const int incomingDataInInterFaceSize=256;
int incomingDataInInterFaceCounter=0;
char * incomingDataInOBD;
const int incomingDataInOBDSize=256;
int incomingDataInOBDCounter=0;
const int waitTimeForOBDComm=2000;
const int sleepTime=10000;
const int powerSaveTime=30;
int OBDCmdReceived=0;
int interfaceCmdReceived=0;
int lastOBDCmdRcvdTime=0;
int lastInterfaceCmdRcvdTime=0;
int KlineReadCount=0;
int isVehicleCommandProcessing=0;
int kLineCommandCount=0;
char kByte=0;

void flip()
{
//    led = !led;
}

void readInterface()
{
    char c;
    while(serialPorts[interFace]->readable()) {
        lastInterfaceCmdRcvdTime=0;
        c = serialPorts[interFace]->getc();

        serialPorts[debug]->putc(c);

        if(incomingDataInInterFaceCounter<incomingDataInInterFaceSize && c!='\r' && c!='\n') {
            incomingDataInInterFace[incomingDataInInterFaceCounter] = c;
        }
        incomingDataInInterFaceCounter++;

        if(c=='\r') {
            interfaceCmdReceived=1;
            serialPorts[debug]->puts("nl\r\n");
        }

    }
}

void canReader()
{
//    serialPorts[debug]->puts("a");
   // serialPorts[debug]->printf("can Reader\r\n");
    if (canBus->read(can_MsgRx)) {
       // serialPorts[debug]->printf("one : %s\r\n",can_MsgRx);
        lastOBDCmdRcvdTime=0;
        if (can_MsgRx.id == PID_REPLY) {
            for (int i = 0; i < (int)can_MsgRx.len && currentOBDProtocol!=0; i++) {
                char c;
//                sprintf(c, "%02X",can_MsgRx.data[i]);
//                serialPorts[interFace].puts(c);
                if(incomingDataInOBDCounter< incomingDataInOBDSize) {
                    incomingDataInOBD[incomingDataInOBDCounter]=can_MsgRx.data[i];
                    incomingDataInOBDCounter++;
                }
            }
            OBDCmdReceived=1;
        }
    }
}

void readKLine()
{
    char c;
    while(kLineUART->readable()) {
        c=kLineUART->getc();
        serialPorts[debug]->printf("%02X",c);
        lastOBDCmdRcvdTime=0;
        if(incomingDataInOBDCounter< incomingDataInOBDSize) {
            incomingDataInOBD[incomingDataInOBDCounter]=c;
            incomingDataInOBDCounter++;
        }
    }
}


void timeCounterFunction()
{
    lastInterfaceCmdRcvdTime++;
    lastOBDCmdRcvdTime++;
    /*KlineReadCount++;
    if(currentOBDProtocol==protkLine14230 && KlineReadCount>=3) {
        KlineReadCount=0;
        if(incomingDataInInterFaceCounter) {
            return;
        }
        kLinekeepalive();
    }*/

}

int main()
{
    //Serial serialPorts[2] = {Serial(P0_11,P0_13),Serial(P0_15,P0_14)};

    initPins();
    serialPorts[debug]->puts("f2120\r\n");
    incomingDataInInterFace=(char *)malloc(incomingDataInInterFaceSize*sizeof(char));
    incomingDataInOBD=(char *)malloc(incomingDataInOBDSize*sizeof(char));

    *klineWakeup=0;
    clearMemoryLocation(incomingDataInInterFace,incomingDataInInterFaceSize);
    clearMemoryLocation(incomingDataInOBD,incomingDataInOBDSize);
    wait_ms(800);
    timeCounter.attach(&timeCounterFunction, 1.0);
    serialPorts[interFace]->attach(&readInterface);
    serialPorts[interFace]->puts("+TU211 v1.0\r");
    serialPorts[interFace]->puts("OK\r\n");
    wait_ms(300); //????? suren :  have to remove wait
    while(1) {
        if(interfaceCmdReceived && checkForLocalInterfaceCommand()) {
            serialPorts[debug]->puts("Going to clear UART data\r\n");
            clearInterfaceCommand();
            serialPorts[debug]->puts("UART data cleared\r\n");
        }
        if(currentOBDProtocol==0/* && isPoweredupNow*/) {
//            isPoweredupNow=0;
#if sendDebugMessage
            serialPorts[debug]->puts("Going to figure protocol\r\n");
            serialPorts[debug]->puts("Searching...\r\n");
#endif
            checkCommunicationProtocol();
            if(!currentOBDProtocol) {
#if sendDebugMessage
                serialPorts[debug]->puts("!Unknownprotocol\r\n");
#endif
            } else if(currentOBDProtocol==protCan) {
                *canDisableKey=0;
#if sendDebugMessage
                serialPorts[debug]->puts("can Protocol\r\n");
#endif
            } else if(currentOBDProtocol==protkLine14230) {
                *canDisableKey=1;
#if sendDebugMessage
                serialPorts[debug]->puts("Kline Protocol\r\n");
#endif
            }
        }
        if(interfaceCmdReceived && checkForLocalInterfaceCommand()) {
            serialPorts[debug]->puts("Going to clear UART data\r\n");
            clearInterfaceCommand();
            serialPorts[debug]->puts("UART data cleared\r\n");
        }
        if(interfaceCmdReceived && currentOBDProtocol) {
            serialPorts[debug]->puts("Going to process user command\r\n");
            clearOBDCommand();
            if(currentOBDProtocol==protCan) {
                while(isVehicleCommandProcessing) {}
                processUserCommand(incomingDataInInterFace);
            } else if(currentOBDProtocol==protkLine14230) {
                while(isVehicleCommandProcessing) {}
                flushBuffer();
                /*kLineUART->attach(NULL);
                kLineUART->attach(&readKLine);*/
                sendCommandToVehicle(incomingDataInInterFace);
                wait_ms(500);
                serialPorts[debug]->printf("\r\n");
            } else {
                clearInterfaceCommand();
                serialPorts[interFace]->puts("?\r\n");
#if sendDebugMessage
                serialPorts[debug]->puts("?\r\n");
#endif
            }
        } else if(interfaceCmdReceived) {
            clearInterfaceCommand();
            serialPorts[interFace]->puts("?\r\n");
#if sendDebugMessage
            serialPorts[debug]->puts("?\r\n");
#endif
        }
//        sleepProcess();
        if(lastOBDCmdRcvdTime> powerSaveTime /*&& lastInterfaceCmdRcvdTime> powerSaveTime */) {
//            powerSaveMode();
        } else {
            sleepProcess();
        }
        /*if(lastOBDCmdRcvdTime> powerSaveTime && lastInterfaceCmdRcvdTime> powerSaveTime ) {
            powerSaveMode();
        } else {
            sleepProcess();
        }*/
    }
}
void checkCommunicationProtocol()
{
    /* #if sendDebugMessage
     serialPorts[debug].puts("check Communication protocal ENT\r\n");
    #endif*/
    int i=0;
    do {

#if sendDebugMessage
        serialPorts[debug]->puts("CanBusCheck\r\n");
#endif
        canBus->reset();
        canBus->frequency(canSpeeds[i]);
        canBus->attach(NULL);
        canBus->attach(&canReader);
        OBDTimer.reset();
        OBDTimer.start();
        OBDCmdReceived=0;
        
      //  can_MsgTx.format = CANStandard;//CANStandard;// or  CANExtended;  // standard or extended ID (can be skipped for standard)
       // can_MsgTx.id = PID_REQUEST;
//        can_MsgTx.len = 8;//length in bytes (1 to 8);
//        can_MsgTx.data[0]=0x42;
        setCanMessage("010D");
        
        if (canBus->write(CANMessage(PID_REQUEST, can_msg, 8))) {
     //   if (canBus->write(can_MsgTx)) {
            while(OBDTimer.read_ms() < waitTimeForOBDComm) {
                if(OBDCmdReceived) {
                    currentOBDProtocol=protCan;
                    canFrequency=i;
                    break;
                }
            }
        }
        wait_ms(200);
        i++;
    } while(i<3 && canFrequency==0);
    OBDTimer.stop();
    canBus->attach(NULL);
    if(!currentOBDProtocol) {
        //Kline interface check
        initializeKLine();
    }
#if sendDebugMessage
    serialPorts[debug]->puts("check Communication protocal EXT\r\n");
#endif
}
void clearMemoryLocation( char *base, int _size)
{
    for(int forCount=0; forCount<_size; forCount++) {
        base[forCount]='\0';
    }
}
void setCanMessage(char* _cmd)
{
    serialPorts[debug]->puts("user Command :");
    serialPorts[debug]->puts(_cmd);
    serialPorts[debug]->puts("\r\n");
    int len=findStringLength(_cmd)/2;
    char _mode[2]="";
    for(int i=0; i<2; i++) {
        _mode[i]=_cmd[i];
    }
    int mode=strtol(_mode,NULL, 16);
    cmd=strtol((_cmd+2),NULL, 16);
    can_msg[0] = len;
    can_msg[1] = mode;
    can_msg[2] = cmd;
    can_msg[3] = 0;
    can_msg[4] = 0;
    can_msg[5] = 0;
    can_msg[6] = 0;
    can_msg[7] = 0;
    
    /*can_MsgTx.len = len;
    can_MsgTx.data[0] = mode;
    can_MsgTx.data[1] = cmd;
    can_MsgTx.data[3] = 0;
    can_MsgTx.data[4] = 0;
    can_MsgTx.data[5] = 0;
    can_MsgTx.data[6] = 0;
    can_MsgTx.data[7] = 0;*/
}
void processUserCommand(char *_incomingDataInInterFace)
{
    isVehicleCommandProcessing=1;
    OBDTimer.reset();
    setCanMessage(_incomingDataInInterFace);
    OBDTimer.start();
    OBDCmdReceived=0;
    clearOBDCommand();
    canBus->attach(NULL);
    canBus->attach(&canReader);
    if (canBus->write(CANMessage(PID_REQUEST, can_msg, 8))) {
        while(OBDTimer.read_ms() < waitTimeForOBDComm) {
            if(OBDCmdReceived) {
                clearInterfaceCommand();
                if(_powerSaveFlag) {
                    serialPorts[interFace]->printf("+TUAWAKE\r\n");
                    _powerSaveFlag=0;
                    break;
                }
                int len=incomingDataInOBD[0];
                for(int i=1; i<=len; i++) {
                    if(incomingDataInOBD[i]!=0x00) {
                        serialPorts[interFace]->printf("%02X",incomingDataInOBD[i]);
                    } else {
                        serialPorts[interFace]->printf("%02X",00);
                    }
                }
                break;
            }
        }
    }
    OBDTimer.stop();
    if(!OBDCmdReceived) {
        clearInterfaceCommand();
        if(!_powerSaveFlag) {
            serialPorts[interFace]->puts("?");
        }
    }
    clearOBDCommand();
    if(!_powerSaveFlag) {
        serialPorts[interFace]->puts("\r\n");
    }
    isVehicleCommandProcessing=0;
}
void sleepProcess()
{

#if pushDebugData
    serialPorts[debug]->puts("SPe\r\n");
#endif
    OBDTimer.reset();
    OBDTimer.start();
    while(OBDTimer.read_ms()<sleepTime && !interfaceCmdReceived) {
        wait_ms(50);
        KlineReadCount=KlineReadCount+50;
        if(currentOBDProtocol==protkLine14230 && KlineReadCount>=3000) {
            KlineReadCount=0;
            if(isVehicleCommandProcessing) {
                continue;
            }
            kLinekeepalive();
        }
    }
    OBDTimer.stop();
}

void powerSaveMode()
{
#if pushDebugData
    serialPorts[debug]->puts("Ignition test\r\n");
#endif
    clearInterfaceCommand();
    clearOBDCommand();
    strcpy(incomingDataInInterFace,"010D");
    _powerSaveFlag=1;
    if(currentOBDProtocol==protCan) {
        while(isVehicleCommandProcessing) {}
        processUserCommand(incomingDataInInterFace);
    } else if(currentOBDProtocol==protkLine14230) {
        while(isVehicleCommandProcessing);
        flushBuffer();
        sendCommandToVehicle(incomingDataInInterFace);
        wait_ms(500);
        serialPorts[debug]->printf("\r\n");
    }
#if pushDebugData
    serialPorts[debug]->puts("Ignition test done\r\n");
#endif
    if(!_powerSaveFlag) {
#if pushDebugData
        serialPorts[debug]->puts("Vehicle is in Power mode\r\n");
#endif
        return;
    }
#if pushDebugData
    serialPorts[debug]->puts("Power save On\r\n");
#endif
    if(currentOBDProtocol==protkLine14230) {
        kLineUART->attach(NULL);
    }
    serialPorts[debug]->attach(NULL);
    serialPorts[interFace]->attach(NULL);
    timeCounter.detach();
    *canDisableKey=1;

//    LPC_SYSCON->SYSAHBCLKCTRL1 |= (1<<7);
    LPC_SWM->PINASSIGN0 = 0xffffffffUL;
    LPC_SWM->PINASSIGN1 = 0xffffffffUL;
    LPC_SWM->PINASSIGN2 = 0xffffffffUL;
    LPC_SWM->PINASSIGN3 = 0xffffffffUL;
    LPC_SWM->PINASSIGN4 = 0xffffffffUL;
    LPC_SWM->PINASSIGN5 = 0xffffffffUL;
    LPC_SWM->PINASSIGN8 = 0xffffffffUL;
//    LPC_SYSCON->SYSAHBCLKCTRL1 |= (0<<7);
    clearInterfaceCommand();
//    serialPorts[interFace]->attach(&readInterface); //  it will not work
    InterruptIn in2(P0_22);
    InterruptIn in3(P0_27);
    in2.rise(&flip);
    in3.rise(&flip);
    while(1) {
//        serialPorts[debug]->printf("Waiting for interupt..\r\n");
        __WFI();
        int waitTime=3000;
        while(waitTime>0) {
            if(interfaceCmdReceived) {
                break;
            }
            wait_ms(50);
            waitTime=(waitTime-50);
        }
        if(interfaceCmdReceived) {
//            serialPorts[debug]->printf("%s\r\n",incomingDataInInterFace);
            if(checkForLocalInterfaceCommand()) {
                clearInterfaceCommand();
            } else {
                serialPorts[interFace]->printf("?\r\n"); // here ahave to send "TTUF" // sleep command
            }
            clearInterfaceCommand();
        } else {
            break;
        }
    }
    NVIC_SystemReset();
#if pushDebugData
    serialPorts[debug]->puts("PowerSaveOff\r\n");
#endif
}
int checkForLocalInterfaceCommand()
{
    serialPorts[debug]->puts("CommandDETECTED\r\n");
    int i=0;
    while(incomingDataInInterFace[i]!='\0') {
        incomingDataInInterFace[i]=tolower(incomingDataInInterFace[i]);
        i++;
    }
    char temp[2]= {incomingDataInInterFace[0],incomingDataInInterFace[1]};
    if(strcmp(temp,"at")!=0) {
        serialPorts[debug]->puts("Not Local Command\r\n");
        return 0;
    }
    if(strcmp(incomingDataInInterFace,"atz")==0) {
        NVIC_SystemReset();
    }
    if(strcmp(incomingDataInInterFace,"atrv")==0) {
        serialPorts[interFace]->puts("12.1V\r\n");
        clearMemoryLocation(incomingDataInInterFace,incomingDataInInterFaceSize);
        return 1;
    }
    serialPorts[interFace]->puts("?\r\n");
    return 2;
}
void sendResponseToInterface()
{
    serialPorts[interFace]->puts(incomingDataInOBD);
    serialPorts[interFace]->puts("\r\n");
}

void clearInterfaceCommand()
{
    incomingDataInInterFaceCounter=0;
    interfaceCmdReceived=0;
    clearMemoryLocation(incomingDataInInterFace,incomingDataInInterFaceSize);
}

void clearOBDCommand()
{
    clearMemoryLocation(incomingDataInOBD,incomingDataInOBDSize);
    incomingDataInOBDCounter=0;
    OBDCmdReceived=0;
}
int findStringLength(char *_cmd)
{
    int i=0;
    for(i=0; _cmd[i]!='\0'; ++i);
    return i;
}
int initializeKLine()
{
#if sendDebugMessage
    serialPorts[debug]->puts("K-Line Check\r\n");
#endif
    LPC_SWM->PINASSIGN2 = 0xffffffffUL;
    *klineWakeup=1;
    wait_ms(25);
    *klineWakeup=0;
    wait_ms(25);
    *klineWakeup=1;
    wait_ms(25);
    //delete klineWakeup;
    LPC_SWM->PINASSIGN2 = 0x1B1CFFFFUL;
#if sendDebugMessage
    serialPorts[debug]->puts("Bitbang done\r\n");
    serialPorts[debug]->printf("%X",LPC_SWM->PINASSIGN2 );
#endif
   // klineWakeup = new DigitalOut(P0_28);
   
    clearOBDCommand();
    kLineUART->baud(10400);
    kLineUART->attach(&readKLine);
    uint8_t crc=0;
    crc=crc+0xC1;
    crc=crc+0x33;
    crc=crc+0xF1;
    crc=crc+0x81;
    kLineUART->putc(0xC1);
    kLineUART->putc(0x33);
    kLineUART->putc(0xF1);
    kLineUART->putc(0x81);
    kLineUART->putc(crc);
    wait(2);
    if(incomingDataInOBD[5]!='\0') {
        currentOBDProtocol=protkLine14230;
    }
  
#if sendDebugMessage
    serialPorts[debug]->puts("K-Line Check2\r\n");
#endif
   kLineUART->attach(NULL);
   /*if(currentOBDProtocol!=protkLine14230){
           delete kLineUART;
           kLineUART=NULL;
           LPC_SWM->PINASSIGN2 = 0xffffffffUL;
          klineWakeup = new DigitalOut(P0_28);
    }*/
    return 0;
}

void setKLineMsg(char *_cmd)
{
    for(int i=3; i<7; i++) {
        kLineCmd[i]='\0';
    }
    int len=findStringLength(_cmd)/2;
    char _mode[2]="";
    uint8_t crc=0;
    if(len==1) {
        crc=crc+0xC1;
        kLineCmd[0]=0xC1;
        crc=crc+0x33;
        kLineCmd[1]=(0x33);
        crc=crc+0xF1;
        kLineCmd[2]=(0xF1);
        crc=crc+strtol((_cmd),NULL, 16);
        kLineCmd[3]=(strtol((_cmd),NULL, 16));
        kLineCmd[4]=(crc);
    } else if(len==2) {
        crc=crc+0xC2;
        kLineCmd[0]=(0xC2);
        crc=crc+0x33;
        kLineCmd[1]=(0x33);
        crc=crc+0xF1;
        kLineCmd[2]=(0xF1);
        for(int i=0; i<2; i++) {
            _mode[i]=_cmd[i];
        }
        crc=crc+strtol(_mode,NULL, 16);
        kLineCmd[3]=(strtol(_mode,NULL, 16));
        if(_cmd[2]==0&&_cmd[3]==0) {
            crc=crc+0;
            kLineCmd[4]=0;
        } else {
            crc=crc+(strtol((_cmd+2),NULL, 16));
            kLineCmd[4]=(strtol((_cmd+2),NULL, 16));
        }
        kLineCmd[5]=(crc);
    }else if(len==3) {
        char _cmd1[2]="";
        crc=crc+0xC3;
        kLineCmd[0]=(0xC3);
        crc=crc+0x33;
        kLineCmd[1]=(0x33);
        crc=crc+0xF1;
        kLineCmd[2]=(0xF1);
        for(int i=0; i<2; i++) {
            _mode[i]=_cmd[i];
        }
        crc=crc+strtol(_mode,NULL, 16);
        kLineCmd[3]=(strtol(_mode,NULL, 16));
        for(int i=2,j=0; i<4,j<2; i++,j++) {
            _cmd1[j]=_cmd[i];
        }
        crc=crc+strtol(_cmd1,NULL, 16);
        kLineCmd[4]=(strtol(_cmd1,NULL, 16));
        crc=crc+(strtol((_cmd+2),NULL, 16));
        kLineCmd[5]=(strtol((_cmd+2),NULL, 16));
        /*if(_cmd[2]==0&&_cmd[3]==0) {
            crc=crc+0;
            kLineCmd[4]=0;
        } else {
            crc=crc+(strtol((_cmd+2),NULL, 16));
            kLineCmd[4]=(strtol((_cmd+2),NULL, 16));
        }*/
        kLineCmd[6]=(crc);
    }
}

void sendCommandToVehicle(char *_cmd)
{
    isVehicleCommandProcessing=1;
    int cmdLen=((findStringLength(_cmd)/2)+4);
    setKLineMsg(_cmd);
    clearOBDCommand();
    flushBuffer();
    kLineUART->attach(&readKLine);
    for(kLineCommandCount=0; kLineCommandCount<cmdLen; kLineCommandCount++) {
        /*if(kLineCmd[kLineCommandCount]=='\0' && cmdLen) {
            break;
        }*/
        kLineUART->putc(kLineCmd[kLineCommandCount]);
    }
    wait_ms(1000);
    serialPorts[debug]->printf("\r\n");
    serialPorts[debug]->printf("Got Data\r\n");
    //have to check given and received commands are same.
    //if it is same we have to read the response else just flush buffer and send the command again
   /* for(int i=0; i<strlen(incomingDataInOBD); i++) {
        if(incomingDataInOBD[i]==0) {
            serialPorts[debug]->printf("00");
        } else {
            serialPorts[debug]->printf("%02X",incomingDataInOBD[i]);
        }
    }*/
    kLineUART->attach(NULL);
    /*   strcpy(incomingDataInOBD,(incomingDataInOBD+kLineCommandCount)); //cut request from hole response
        for(int i=0; i<strlen(incomingDataInOBD); i++) {
            serialPorts[debug]->printf("%02X",incomingDataInOBD[i]);
        }
        serialPorts[debug]->printf("\r\n");
        serialPorts[debug]->printf("%02X\r\n",incomingDataInOBD[0]);
        int len=(incomingDataInOBD[0]%16);//convert first char to integer and find kength of the response
        serialPorts[debug]->printf("Response Length : %d\r\n",len);
        serialPorts[debug]->printf("Response Length : %d\r\n",strlen(incomingDataInOBD));
//        strcpy(incomingDataInOBD,(incomingDataInOBD+3)); //again cut first three byte
        serialPorts[debug]->printf("Response Length : %d\r\n",strlen(incomingDataInOBD));
        int obdDataLength=strlen(incomingDataInOBD);
        int count=0;
        while(count<len) {
            if(_powerSaveFlag) {
                serialPorts[interFace]->printf("+TUAWAKE");
                _powerSaveFlag=0;
                break;
            }
            if(incomingDataInOBD[count]!=0){
             serialPorts[interFace]->printf("%02X",incomingDataInOBD[count]);
               // serialPorts[debug]->printf("%02X",incomingDataInOBD[count]);
             }else{
                 serialPorts[interFace]->printf("%02X",0x00);
               // serialPorts[debug]->printf("%02X",0x00);
             }
             
            if(obdDataLength>0) { // have to recheck this condition
                serialPorts[interFace]->printf("%02X",incomingDataInOBD[count]);
                serialPorts[debug]->printf("%02X",incomingDataInOBD[count]);
            } else {
                serialPorts[interFace]->printf("%02X",0x00);
                serialPorts[debug]->printf("%02X",0x00);
            }
            count++;
            obdDataLength--;
        }


        //????? suren: Have to recheck the following lines
        if(len==0) {
            if(!_powerSaveFlag) {
                serialPorts[interFace]->printf("?");
                serialPorts[debug]->printf("?");
            }
        }
        if(!_powerSaveFlag) {
            serialPorts[interFace]->printf("\r\n");
            serialPorts[debug]->printf("\r\n");
            serialPorts[debug]->printf("Done\r\n");
        } 
        */
    wait_ms(300);
    clearInterfaceCommand();
    clearOBDCommand();
    isVehicleCommandProcessing=0;
}

void flushBuffer()
{
    while(kLineUART->readable()) {
        kLineUART->getc();
    }
}
void kLinekeepalive()
{
    serialPorts[debug]->printf("KeepAlive ENT\r\n");
    kLineUART->putc(0xc1);
    kLineUART->putc(0x33);
    kLineUART->putc(0xf1);
    kLineUART->putc(0x3e);
    kLineUART->putc(0x23);
    serialPorts[debug]->printf("EXT\r\n");
}
char tolower(unsigned char ch)
{
    if (ch >= 'A' && ch <= 'Z')
        ch = 'a' + (ch - 'A');
    return ch;
}

int checkCarStatus()
{
//    LPC_SYSCON->SYSAHBCLKCTRL1 |= (1<<7);
    LPC_SWM->PINASSIGN0 = 0xffffffffUL;
    LPC_SWM->PINASSIGN1 = 0xffffffffUL;
    LPC_SWM->PINASSIGN2 = 0xffffffffUL;
    LPC_SWM->PINASSIGN3 = 0xffffffffUL;
    LPC_SWM->PINASSIGN4 = 0xffffffffUL;
    LPC_SWM->PINASSIGN5 = 0xffffffffUL;
    LPC_SWM->PINASSIGN8 = 0xffffffffUL;
//    LPC_SYSCON->SYSAHBCLKCTRL1 |= (0<<7);
    DigitalIn kLine(P0_27);
    DigitalIn canLine(P0_22);
    if(kLine.read()) {
        return 1;
    }
    if(canLine.read()) {
        return 2;
    }
    return 0;
}

void initPins()
{
//    LPC_SYSCON->SYSAHBCLKCTRL1 |= (1<<7);
    LPC_SWM->PINASSIGN0 = 0xffffffffUL;
    LPC_SWM->PINASSIGN1 = 0xffffffffUL;
    LPC_SWM->PINASSIGN2 = 0xffffffffUL;
    LPC_SWM->PINASSIGN3 = 0xffffffffUL;
    LPC_SWM->PINASSIGN4 = 0xffffffffUL;
    LPC_SWM->PINASSIGN5 = 0xffffffffUL;
    LPC_SWM->PINASSIGN8 = 0xffffffffUL;
//    LPC_SYSCON->SYSAHBCLKCTRL1 |= (0<<7);
    serialPorts[0] = new Serial(P0_11,P0_13);
    serialPorts[1] = new Serial(P0_15,P0_14);
   /* serialPorts[0] = new Serial(P0_16,P0_12);
    serialPorts[1] = new Serial(USBTX,USBRX);*/
    klineWakeup = new DigitalOut(P0_28);
    canBus = new CAN(P0_22, P0_23);
    canDisableKey=new DigitalOut(P0_0,0);
    kLineUART=new Serial(P0_28,P0_27);
    serialPorts[interFace]->baud(9600);
    serialPorts[debug]->baud(9600);
}