File content as of revision 19:1d4d31c23953:

#include "stm32f103c8t6.h"
#include "mbed.h"
#include "DS1820.h"
#include "MLX90614.h"
#include "config.h"
#include "ATCmdParser.h"
#include "UARTSerial.h"
#include "Watchdog.h"
 
Watchdog wd;
DigitalOut  myled(PC_13);
DigitalOut  simPWR(PA_8);  // (active low level) PWRKEY pin to power on or off  module
 
//SIM7000
UARTSerial *_serial;
ATCmdParser *_parser;
int rssiDB,rxQual;
 
bool from_sleep = false;
 
 
// ID
 
char device_id[] = {'M','I','E','M','H','S','E','-','2','0','2','1','\0'};
 
//Termometers
OneWire oneWire(PIN_ONEWIRE);
const int SENSORS_COUNT = 10;
DS1820* ds1820[SENSORS_COUNT];
int sensors_found = 0;
const char sensorsOrder[]={4,2,5,8,0,3,6,1,9,7};
float stickTemperatures[SENSORS_COUNT];
 
int i=0;
 
//IR termometer
I2C i2c(PIN_SDA, PIN_SCL);   //sda,scl
MLX90614 thermometer(&i2c);
float IRtemp;
 
// GPS
int Fix_st;
 
float B_l = 0;
float L_l = 0;
float Alt = 0;
 
bool cold_start  = 0;
int step_p = 0;
#if DEBUG_PC
Serial pc(PIN_TX, PIN_RX); // TX, RX
#endif
 
int index;
char bufferString[2048];
 
int h_time, m_time, s_time;
int fq, nst, fix, date;                                     // fix quality, Number of satellites being tracked, 3D fix
//float latitude, longitude, timefix, speed, altitude;
 
char state=STATE_INIT;
int sleepTimer=0;
int fixTries=0;
float a = 66;

 
 
void parseTime (float timeval)
{
    //format utc time to beijing time,add 8 time zone
                float time = timeval + 80000.00f;
                h_time = int(time) / 10000;
                m_time = (int(time) % 10000) / 100;
                s_time = int(time) % 100;
}
 
 
bool checkIfOk() {
    if(_parser->recv("OK")) 
    {
        #if DEBUG_PC
        pc.printf("Done\r\n");
        #endif
        
        return true;
    } else 
    {
        #if DEBUG_PC
        pc.printf("Fail\r\n");
        #endif
        
        return false;
    }       
}
void blink_fast()
{
    for(int b = 0;b < 3;b++)
    {
        myled = 1;
        wait(0.2);
        myled = 0;
        wait(0.2);
        myled = 1;
    }
}
void blink_slow()
{
    for(int b = 0;b < 3;b++)
    {
        myled = 1;
        wait(1);
        myled = 0;
        wait(1);
        myled = 1;
        wd.Service();
    }
}
 
 
 
bool enableGPS(bool powerUp) {
    #if DEBUG_PC
    pc.printf("Powering GPS %s:\r\n",powerUp?"up":"down");
    #endif
    if(powerUp){
        _parser->send("AT+CGNSPWR=1"); //GPS power on
    } else {
        _parser->send("AT+CGNSPWR=0"); //GPS power off
    }
    wait(0.5);
    return checkIfOk();
}
bool configure()
{
    _parser->send("AT+CNMP=38"); 
    if (!checkIfOk())
    {
       return false;
    }
 
    _parser->send("AT+CMNB=2");
    if (!checkIfOk())
    {
       return false;
    }
    _parser->send("AT+CAPNMODE=1");
    if (!checkIfOk())
    {
       return false;
    }
    _parser->send("AT+CAPNMODE=1");
    if (!checkIfOk())
    {
       return false;
    }
    _parser->send("AT+CBANDCFG=\"NB-IOT\",8");
    if (!checkIfOk())
    {
       return false;
    }
    _parser->send("AT+CNDS=2");
    if (!checkIfOk())
    {
       return false;
    }
    _parser->send("ATE0");
    if (!checkIfOk())
    {
       return false;
    }
 
    return true;
}
 
bool enableRF(bool powerUp){
    #if DEBUG_PC
    pc.printf("Powering RF %s: \r\n",powerUp?"up":"down");
    #endif
    if(powerUp){
        _parser->send("AT+CFUN=1"); 
    } else {
        _parser->send("AT+CFUN=0"); //GPS power off
    }
    //wait(0.1);
    return checkIfOk();  
    //return true;  
}
bool enableRF_RESTART()
{
    #if DEBUG_PC
    pc.printf("Powering RF with restart\r\n");
    #endif
    _parser->send("AT+CFUN=1,1"); 
}


bool setPowerSavingMode() 
{
    _parser->send("AT+CPSMS=1");//power save on 
    wait(0.2);
    return checkIfOk(); 
}
 
bool setSatSystems(){
    #if DEBUG_PC
    pc.printf("Setting sats:");
    #endif
    _parser->send("AT+CGNSMOD=1,1,1,1");
    return checkIfOk();    
}
 
bool getSignalQuality() 
{
    _parser->send("AT+CSQ");
    
    if(_parser->recv("+CSQ: %d,%d", &rssiDB,&rxQual) && _parser->recv("OK")) {
        return true;
    }
    return false;
}
 
bool setAPN() {
    _parser->send("AT+CNACT=1,\"iot\"");
    return checkIfOk();
}
 
bool disconnectNetwork() {
     _parser->send("AT+CNACT=0");
    return checkIfOk();
}
 
int initSIM()
{
    fixTries=0;
    while(fixTries<COMD_EXE_TRIES && !enableRF(1))
    {
        
        wait(2);
        wd.Service();
        
        fixTries++;
        if (fixTries >= COMD_EXE_TRIES)
        {
            return 0;
        }
    }
  fixTries=0;
    while(fixTries<COMD_EXE_TRIES && !configure())
    {
        wait(1);
        wd.Service();
        fixTries++;
        if (fixTries >= COMD_EXE_TRIES)
        {
            return 0;
        }
    }
 
    fixTries=0;
    while(fixTries<COMD_EXE_TRIES && !setSatSystems())
    {
        
        wait(2);
        wd.Service();
        fixTries++;
        if (fixTries >= COMD_EXE_TRIES)
        {
            return 0;
        }
    }
    wd.Service();    
    
 
 
    return true;
}
 
bool getGPS() {
    wd.Service();
    char GNSSrunstatus_c[2];
    char Fixstatus_c[2];
    char UTCdatetime_c[19];
    char latitude_c[11];
    char logitude_c[12];
    char altitude_c[9];
    char speedOTG_c[7];
    char course_c[7];
    char fixmode_c[2];
    char reserved_1[1];
    char HDOP_c[5];
    char PDOP_c[5];
    char VDOP_c[5];
    char reserved_2[1];
    char satellitesinview_c[3];
    char GNSSsatellitesused_c[3];
    char GLONASSsatellitesused_c[3];
    char reserved_3[1];
    char cn0max_c[3];
    char HPA_c[7];
    char VPA_c[7];
 
    const int numChars = 120; // spec says up to 94 characters
    char receivedChars[ numChars ];
    receivedChars[0] = 0;
 
 
 
    _parser->send("AT+CGNSINF");
    wait(0.2);
    int nmeaStrLen=_parser->read(receivedChars, 120);
    _parser->flush();
    char *curLine = receivedChars;
 
    char frame[150];
    for (int i = 0; i< 115; i++)
    {
        if(curLine[i]==':')
        { 
            i++;    
            int j = 0;
            while(i < 125)
            {
                if(curLine[i]=='O' && curLine[i+1]=='K')
                {
                    break;
                }
                else
                {
                    frame[j] = curLine[i];
                    #if DEBUG_PC
                    pc.printf("cahr %c\n", frame[j]);
                    #endif
                    i++;
                    j++;    
                }     
            }
        break;
        }
    }
    // Parses the string
    char * pch = strtok (frame,",");
    strcpy(GNSSrunstatus_c, pch);
    pch = strtok(NULL, ",");
    strcpy(Fixstatus_c, pch);
    pch = strtok(NULL, ",");
    strcpy(UTCdatetime_c, pch);
    pch = strtok(NULL, ",");
    strcpy(latitude_c, pch);
    pch = strtok(NULL, ",");
    strcpy(logitude_c, pch);
    pch = strtok(NULL, ",");
    strcpy(altitude_c, pch);
    pch = strtok(NULL, ",");
    strcpy(speedOTG_c, pch);
    pch = strtok(NULL, ",");
    strcpy(course_c, pch);
    pch = strtok(NULL, ",");
    strcpy(fixmode_c, pch);
    pch = strtok(NULL, ",");
    strcpy(reserved_1, pch);
    pch = strtok(NULL, ",");
    strcpy(HDOP_c, pch);
    pch = strtok(NULL, ",");
    strcpy(PDOP_c, pch);
    pch = strtok(NULL, ",");
    strcpy(VDOP_c, pch);
    pch = strtok(NULL, ",");
    strcpy(reserved_2, pch);
    pch = strtok(NULL, ",");
    strcpy(satellitesinview_c, pch);
    pch = strtok(NULL, ",");
    strcpy(GNSSsatellitesused_c, pch);
    pch = strtok(NULL, ",");
    strcpy(GLONASSsatellitesused_c, pch);
    pch = strtok(NULL, ",");
    strcpy(reserved_3, pch);
    pch = strtok(NULL, ",");
    strcpy(cn0max_c, pch);
    pch = strtok(NULL, ",");
    strcpy(HPA_c, pch);
    pch = strtok(NULL, ",");
    strcpy(VPA_c, pch);
    
    
 
    int GNSS_st = atof(GNSSrunstatus_c); 
    Fix_st      = atof(Fixstatus_c); 
    int sat_count = atof(satellitesinview_c); 
    if(Fix_st)
    {
        B_l = atof(latitude_c);
        L_l = atof(logitude_c);
        Alt = atof(altitude_c);
    }

 
    #if DEBUG_PC
    pc.printf("GNSS st %d, FIX %d,SAT  Count %d, Latitude: %f , Longitude: %f, Altitude: %f \n",GNSS_st, Fix_st, sat_count, B_l, L_l, Alt);
    #endif
 
    wd.Service();
 
    if(Fix_st != 0)
    {
        if (step_p >= 4 )
        {
            return true;
        }
        else
        {
            step_p++;
        }
    }
    //return true;
    return false;
}
 
int terminateCOAPSession() {
 
    //_parser->send("AT+CCOAPTERM");
    //wait(2);
    //wd.Service();

    _parser->send("AT+CNACT=0");
}
 
 
 
bool sendTelemetry() 
{
    int check = 0;
    wd.Service();
    int tr = 0;
 
    setAPN();
    
    wd.Service();
    check = tr = 0;
    wait(2);
 
    _parser->flush();   

    _parser->send("AT+CCOAPINIT");
    wait(1);

    _parser->flush(); 
    //_parser->send("AT+CCOAPURL=\"" MTS_TELEMETRY_URL_STRING);
    
    _parser->send("AT+CCOAPURL=\"coap://193.227.232.26:5683/multisensors\"");
    checkIfOk();
    //if(!checkIfOk()){ terminateCOAPSession();return false;}
    wait(1);
     _parser->flush(); 

    //_parser->printf("AT+CCOAPPARA=code,2,token,0,\"%s\",payload,1,",xstr(MTS_COAP_TOKEN));
    //_parser->printf("AT+CCOAPPARA=code,2,payload,1,");
    
    pc.printf("\r\nAT+CCOAPPARA=\"CODE\",2,payload,1,");
      for (size_t i = 0; i < strlen(bufferString); i++) {
    pc.printf("%x",bufferString[i]);
    }
    pc.printf("\r\n");

    _parser->printf("AT+CCOAPPARA=\"CODE\",2,payload,1,");

    //_parser->send("AT+CCOAPPARA=\"CODE\",2,payload,1,\"7b224944223a20224d49454d4853452d54455354222c2274656d704952223a202d312e3636393938392c226c61746974756465223a202d36392e3337393135382c226c6f6e676974756465223a2037362e3338393339372c22616c746974756465223a20362e342c2276616c696447656f223a20747275652c2262617474657279223a203130302c226e65746c766c223a2033312c2274656d7073223a5b332e37352c312e3837352c332e352c332e333132352c332e343337352c342e32352c322e3632352c302e313837352c342e3132352c302e3837355d7d\"");
    //_parser->printf(bufferString);
   
    for (size_t i = 0; i < strlen(bufferString); i++) 
    {
    _parser->printf("%x",bufferString[i]);
    }
    _parser->printf("\r\n");

    checkIfOk();
    //if(!checkIfOk()){ terminateCOAPSession();return false;}
    wait(0.5);
     _parser->flush(); 
    _parser->send("AT+CCOAPACTION");
    //checkIfOk();
    if(!checkIfOk()){ return false;}
    wait(2);
    wd.Service();
    wait(2);
    wait(2);
    wd.Service();
    wait(2);
    return true;
}
int main() {
    confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)  
 
    #if ENABLE_WATCHDOG
    wd.Configure(WATCHDOG_INTERVAL_S);       // sets the timeout interval
    #endif
 
    //simPWR = 0; 
    //wait(2);
    //simPWR = 1;
 
    blink_slow();
   
   
    //wait(7);
    //toggle_sim_power();
    /*
    * Generally, the NET indicator will fast flash firstly (1 time per second), which means that the
    * module has not logged in the Network. After logging in, the indicator become to flash slowly (1
    * time every three seconds). 
    */
 
    //wait(WATCHDOG_INTERVAL_S-15);//Let SIM7000 wake up
    wd.Service();
    #if DEBUG_PC
    pc.baud(115200);
    pc.printf("Starting\r\n");
    if (wd.WatchdogCausedReset()) {
        pc.printf("Watchdog caused reset.\r\n");
    }
    #endif
 
    
 
    
 
    
    //Initiate SIM7000 SERIAL
    _serial = new UARTSerial(PIN_SIM_TX, PIN_SIM_RX, 115200);
    _parser = new ATCmdParser(_serial);
    _parser->debug_on( DEBUG_SIM );
    _parser->set_delimiter("\r\n");
    _parser->set_timeout(10);

    wait(2);
    wd.Service();
    wait(2);
    _parser->flush(); 
    enableRF_RESTART();
    
    wait(2);
    wd.Service();
    wait(2);
    wait(2);
    wd.Service();
    wait(2);
    wait(2);

    
    fixTries=0;
 
    #if DEBUG_PC
    pc.printf("INIT CONFIG\r\n");
    #endif
 
    int res = initSIM();
    if(!res)
    {
        #if DEBUG_PC
        pc.printf("Init Failed\r\n");
        #endif
        wd.Service();
        wait(2);
        wd.Service();
        wait(2);
        if(!initSIM());
        {
            wait(WATCHDOG_INTERVAL_S+10); // reset by dog 
        }
    }
 
    #if DEBUG_PC
    pc.printf("INIT finished\r\n");
    #endif
 
    blink_fast();
 
    wd.Service();
    

    getSignalQuality();

    #if DEBUG_PC
    pc.printf("Signal quality: %d\r\n",rssiDB);
    #endif
    
    wd.Service();
    //Initiate termal stick
    for(i = 0; i < SENSORS_COUNT; i++) {
          ds1820[i] = new DS1820(&oneWire);
          if(!ds1820[i]->begin()) {
              delete ds1820[i];
              break;
          }
    }
    
    sensors_found = i;
    #if DEBUG_PC
    pc.printf("Found %d sensors\r\n",sensors_found);
    if (sensors_found==0)
          pc.printf("No devices found");
    #endif
    
    //Feed the watchdog
    wd.Service();
 
    state=STATE_STARTING_GPS;
    //state = STATE_COLLECTING_TELEMETRY;
    
    int off = 0;
 
    while(1) 
    {
        wd.Service();
        if (state==STATE_STARTING_GPS)
        {
            wd.Service();
            
 
            #if DEBUG_PC
            pc.printf("STATE=STARTING GPS\r\n");
            #endif
 
            fixTries=0;
            while(fixTries<COMD_EXE_TRIES && !enableGPS(1))
            {
                #if DEBUG_PC
                pc.printf("GPS enable Fail\r\n");
                #endif
                wait(3);
                wd.Service();
                fixTries++;
                if (fixTries >= COMD_EXE_TRIES)
                {
                    wait(WATCHDOG_INTERVAL_S+5);
                }
            }
 
            
            for(int test = 0; test < FIX_MAX_TRIES; test ++)
            {
                #if DEBUG_PC
                pc.printf("WAITNG gps to turn on \r\n");
                #endif
                wd.Service();
                wait(2);  
                wait(2);  
                wait(1);
                wd.Service();
                _parser->send("AT+CGNSINF");
                if(checkIfOk())
                {
                    if(cold_start)
                    {
                        off = 0;
                        while(off<COMD_EXE_TRIES && !enableGPS(0))
                        {
                            wd.Service();
                            off++;
                        }
                        break;
                    }
                    else
                    {
                        break;
                    }    
                }
                else
                {
                    if (test > 2)
                    {
                        cold_start = true;
                    }
                    wd.Service();
                    wait(2);  
                    wait(2);  
                    wait(1);
                    wd.Service();
                }
                
                
 
            }
 
 
        
            state=STATE_WAITING_FIX;
            fixTries=0;
            wd.Service();
             step_p = 0;
            //wait(FIX_CHECK_TIME_S);
        
        }
        else if(state==STATE_WAITING_FIX) {
            
            #if DEBUG_PC
            pc.printf("STATE=WAITNG FIX\r\n");
            #endif
 
            off = 0;
            if (cold_start)
            {
                while(off<COMD_EXE_TRIES && !enableGPS(1))
                {
                    wd.Service();
                    off++;
                    if (off >= COMD_EXE_TRIES)
                    {
                    wait(WATCHDOG_INTERVAL_S+5);
                    }
                }
                cold_start = false;
            }
            
            
            if(getGPS())
            {
                wd.Service();
                off = 0;
                while(off<FIX_MAX_TRIES && !enableGPS(0))
                {
                    #if DEBUG_PC
                    pc.printf("GPS disable Fail\r\n");
                    #endif
                    wait(3);
                    wd.Service();
                    off++;  
                }
                
                state=STATE_COLLECTING_TELEMETRY; 
            } 
            else 
            {
                wd.Service();
                #if DEBUG_PC
                pc.printf("No fix(%d) at %d/%d try\r\n", fix, fixTries, FIX_MAX_TRIES);
                #endif
                fixTries++;
                if (fixTries>FIX_MAX_TRIES) 
                {
                    //fix not achieved in given tries, send as is
                    #if DEBUG_PC
                    pc.printf("No fix but continue\r\n");
                    #endif
 
                    off = 0;
                    while(off<FIX_MAX_TRIES && !enableGPS(0))
                    {
                        #if DEBUG_PC
                        pc.printf("GPS disable Fail\r\n");
                        #endif
                        wait(3);
                        wd.Service();
                        off++;
                    }
                    
                    
                    wd.Service();
                    state=STATE_COLLECTING_TELEMETRY;
                    fixTries=0;
                } 
                else
                 {
                    #if DEBUG_PC
                    pc.printf("Waiting %d sec\r\n",FIX_CHECK_TIME_S);
                    #endif
                    wd.Service();
                    wait(2);
                    wait(2);
                    wait(1);
                    wd.Service();
                }
            }
        } else if(state==STATE_COLLECTING_TELEMETRY) {
            #if DEBUG_PC
            pc.printf("STATE=COLLECTING TELEMETRY\r\n");
            #endif
            for(i=0;i<SENSORS_COUNT;i++){
                if(sensorsOrder[i]<sensors_found){
                    ds1820[sensorsOrder[i]]->startConversion();
                }    
            }
            
            wait(1);
            pc.printf("startConversion\r\n");
            for(i=0;i<SENSORS_COUNT;i++){
                if(sensorsOrder[i]<sensors_found){
                    if(ds1820[sensorsOrder[i]]->isPresent()){
                        stickTemperatures[i]=ds1820[i]->read();
                    } else {
                        stickTemperatures[i]=-273.f; // Sensor is offline
                    }
                }
            }
             pc.printf("startConversion\r\n");
           
            IRtemp=thermometer.read_temp(1);

            pc.printf("ik done\r\n");
            float bat_v = 3.2;

            for(int try_c = 0; try_c < COMD_EXE_TRIES; try_c ++)
            {
                if(getSignalQuality())
                {
                    break;
                }
                wd.Service();
            }

            if (rssiDB > 100)
            {
                rssiDB = 100;
            }
            else if (rssiDB < 0)
            {
                rssiDB = 0;
            }

            if (IRtemp > 100)
            {
                IRtemp = 100;
            }
            else if (IRtemp < -300)
            {
                IRtemp = -300;
            }
            
            if (B_l > 200)
            {
                B_l = 200;
            }
            else if (B_l < -200)
            {
                B_l = -200;
            }
            
            if (L_l > 200)
            {
                L_l = 200;
            }
            else if (L_l < -200)
            {
                L_l = -200;
            }
            
            if (Alt > 9000)
            {
                Alt = 9000;
            }
            else if (Alt < -200)
            {
                Alt = -200;
            }
            
 

            //Form JSON as {"tempIR":1,"temps":[1,...,10],"latitude":37,"longitude":51,"altitude":21,"validGeo":true}
             //snprintf(bufferString,2048,"{\"ID\":\"%s\",\"tempIR\":%f,\"latitude\":%f,\"longitude\":%f,\"altitude\":%f,\"validGeo\":%s,\"battery\":%f,\"netlvl\":%d,\"temps\":[",device_id, IRtemp, B_l, L_l, Alt, Fix_st ? "1" : "0", bat_v, rssiDB);
             snprintf(bufferString,2048,"{\"ID\":\"%s\",\"tempIR\":%4.2f,\"latitude\":%3.8f,\"longitude\":%3.8f,\"altitude\":%4.2f,\"validGeo\":%d,\"battery\":%4.2f,\"netlvl\":%d,\"temps\":[",device_id, IRtemp, B_l, L_l, Alt, Fix_st ? 1 : 0, bat_v, rssiDB);
  //  snprintf(bufferString,2048,"{\"ID\":\"%s\",\"tempIR\":%f,\"latitude\":%f,\"longitude\":%f,\"altitude\":%f,\"validGeo\":%s,\"battery\":%f,\"netlvl\":%d,\"temps\":[",device_id, IRtemp, B_l, L_l, Alt, Fix_st ? ( 1: 0) , bat_v
           // device_id

           
            index = strlen(bufferString);
            pc.printf("index = %d \r\n", index);

            for(i=0;i<SENSORS_COUNT;i++){
                //index += snprintf(&bufferString[index], 2048-index, i==0?"%f":",%f", stickTemperatures[i]);
                if (stickTemperatures[i] > 100)
                {
                    stickTemperatures[i] = 100;
                }
                else if (stickTemperatures[i] < -280)
                {
                    stickTemperatures[i] = -280;
                }
                index += snprintf(&bufferString[index], 2048-index, i==0?"%4.4f":",%4.4f", stickTemperatures[i]);
            }
            strcat (bufferString,"]}");

            index = strlen(bufferString);
            pc.printf("\r\n index = %d", index);
            
            for (size_t i = 0; i < strlen(bufferString); i++) 
            {
             pc.printf("%c",bufferString[i]);
            }
        
            wd.Service();
            state=STATE_SENDING_TELEMETRY;
        } 
        else if(state==STATE_SENDING_TELEMETRY)
        {
            #if DEBUG_PC
            pc.printf("\r\nSTATE=SENDING TELEMETRY\r\n");
            pc.printf(bufferString);
            #endif
         
          
         
            fixTries=0;
            
            if(!sendTelemetry())
            {
                terminateCOAPSession();
                wd.Service();
                wait(2);
                 wait(2);
                  wait(2);
                  wd.Service();
                   wait(2);
                sendTelemetry();
            }
            wait(1);
            terminateCOAPSession();
 
            //disconnectNetwork();
            //initSIM();
            //enableGPS(0); 
            //setPowerSavingMode();
            //fixTries++;
            wd.Service();
            enableRF(0);
 
            off = 0;
            while(off<COMD_EXE_TRIES && !setPowerSavingMode())
            {
                wd.Service();
                off++;
            }
             #if DEBUG_PC
            pc.printf("Sim 7000 Off\r\n");
 
            #endif
 
            simPWR = 0; 
            wait(2);
            simPWR = 1;
            _parser->flush();
            
 
            //wait(SLEEP_CHECK_TIME);
            wd.Service();
            
 
            state=STATE_SLEEPING;
            sleepTimer=0;
 
        } else if(state==STATE_SLEEPING){
            #if DEBUG_PC
            pc.printf("STATE=SLEEPING already for %d\r\n",sleepTimer);
            #endif
            wd.Service();
            sleepTimer+=SLEEP_CHECK_TIME;
            
            if(sleepTimer>SLEEP_TIME_S){
                simPWR = 0; 
                wait(3);
                simPWR = 1;
                _parser->flush();
                #if DEBUG_PC
                pc.printf("from sleep\r\n");
                #endif
                wait(2);
                wd.Service();
                wait(2);
                wd.Service();
                wait(2);
                wd.Service();
            
                _parser->flush();
                   
 
                for(i=0;i<7;i++)
                {
                    myled = 1;
                    wait(1);
                    myled = 0;
                    wait(1);
                    wd.Service();
                    myled = 1;
                }
                #if DEBUG_PC
                pc.printf("boot\r\n");
                #endif
                blink_fast();
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
                
                int res = initSIM();
                if(!res)
                {
                    #if DEBUG_PC
                    pc.printf("Init Failed\r\n");
                    #endif
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
    wd.Service();
    wait(2);
    wd.Service();
    wait(2); 
                    if(!initSIM());
                    {
                        wait(WATCHDOG_INTERVAL_S+10); // reset by dog 
                    }
                    
 
                   
                }
 
                state=STATE_STARTING_GPS;
                //state = STATE_COLLECTING_TELEMETRY;
                sleepTimer=0;
            } else {
               wait(2);  
               wait(2);  
               wait(1);
               // = 5 =   SLEEP_CHECK_TIME
            }
            wd.Service();
            #if DEBUG_PC
            pc.printf("After sleep\r\n");
            #endif
            
            //from_sleep = true;
        }
 
        //Feed the watchdog
        wd.Service();
        wait(0.5);
    }
}