Diff: main.cpp

Revision:

11:57fa27cb533e

Parent:

10:51960145754a

Child:

12:7fe416cdac08

--- a/main.cpp	Wed Sep 30 16:40:29 2020 +0000
+++ b/main.cpp	Fri Jan 08 12:20:48 2021 +0000
@@ -8,14 +8,20 @@
 #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','-','T','E','S','T','\0'};
 
 //Termometers
 OneWire oneWire(PIN_ONEWIRE);
@@ -32,6 +38,15 @@
 MLX90614 thermometer(&i2c);
 float IRtemp;
 
+// GPS
+int Fix_st;
+
+float B_l;
+float L_l;
+float Alt;
+
+bool cold_start  = 0;
+int step_p = 0;
 #if DEBUG_PC
 Serial pc(PIN_TX, PIN_RX); // TX, RX
 #endif
@@ -41,11 +56,28 @@
 
 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;
+//float latitude, longitude, timefix, speed, altitude;
 
 char state=STATE_INIT;
 int sleepTimer=0;
 int fixTries=0;
+float a = 66;
+void waste_time(int x)
+{ 
+    
+    for(int i; i < 10000000; i++)
+    {
+        for(int k; k < 10000000 * x * 5; k++)
+        {
+            a = a / 33;
+            a = a * 33;
+        }
+    }
+
+
+
+}
+
 
 void parseTime (float timeval)
 {
@@ -56,93 +88,6 @@
                 s_time = int(time) % 100;
 }
 
-/* 
- * NMEA sentences:  https://www.gpsinformation.org/dale/nmea.htm#nmea
- *                  http://navspark.mybigcommerce.com/content/NMEA_Format_v0.1.pdf
- */
-void nmea_parse(char *cmd)
-{   
-    char ns, ew, tf, status;
-    
-    
-    // Global Positioning System Fix Data
-    if(strncmp(cmd,"$GPGGA", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
-        #if DEBUG_PC
-        pc.printf("GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
-        #endif
-    }
-    
-    // Satellite status
-     else if(strncmp(cmd,"$GPGSA", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst);
-        #if DEBUG_PC
-        pc.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
-        #endif
-    }
-    
-    // Geographic position, Latitude and Longitude
-    else if(strncmp(cmd,"$GPGLL", 6) == 0) 
-    {
-        sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
-        #if DEBUG_PC
-        pc.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
-        #endif
-    }
-    
-    // Geographic position, Latitude and Longitude
-    else if(strncmp(cmd,"$GPRMC", 6) == 0) 
-    {
-        sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date);
-        #if DEBUG_PC
-        pc.printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n", timefix, status, latitude, ns, longitude, ew, speed, date);
-        #endif
-    }
-    
-    // 
-    else if(strncmp(cmd,"$GNVTG", 6) == 0) 
-    {
-     //   pc.printf("its a Vector Track message.\n");
-    }
-    
-    else if(strncmp(cmd,"$GNGGA", 6) == 0) 
-    {
-        sscanf(cmd, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude);
-        parseTime(timefix);
-        #if DEBUG_PC
-        pc.printf("GNGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
-        pc.printf("Time: %d:%d:%d\n", h_time, m_time, s_time);
-        #endif
-    }
-    
-    else if(strncmp(cmd,"$GNGSA", 6) == 0) 
-    {
-        sscanf(cmd, "$GNGSA,%c,%d,%d", &tf, &fix, &nst);
-        #if DEBUG_PC
-        pc.printf("GNGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
-        #endif
-    }
-    
-    else if(strncmp(cmd,"$GPGSV", 6) == 0) 
-    {
-     //   pc.printf("its a Satellite details message.\n");
-    }
-    
-    else if(strncmp(cmd,"$GNGLL", 6) == 0) 
-    {
-        sscanf(cmd, "$GNGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix);
-        #if DEBUG_PC
-        pc.printf("GNGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix);
-        #endif
-    }
-    
-    else
-    {
-   //     pc.printf("Unknown message type\n");
-    }
-}
 
 bool checkIfOk() {
     if(_parser->recv("OK")) {
@@ -159,35 +104,60 @@
         return false;
     }       
 }
+void blink_fast()
+{
+    for(i=0;i<5;i++)
+    {
+    myled = 1;
+    wait(0.5);
+    myled = 0;
+    wait(0.5);
+    myled = 1;
+    }
+}
+void blink_slow()
+{
+    for(i=0;i<5;i++)
+    {
+    myled = 1;
+    wait(1);
+    myled = 0;
+    wait(1);
+    myled = 1;
+    }
+}
+
+
 
 bool enableGPS(bool powerUp) {
     #if DEBUG_PC
-    pc.printf("Powering GPS %s:",powerUp?"up":"down");
+    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 enableRF(bool powerUp){
     #if DEBUG_PC
-    pc.printf("Powering RF %s:",powerUp?"up":"down");
+    pc.printf("Powering RF %s: \r\n",powerUp?"up":"down");
     #endif
     if(powerUp){
-        _parser->send("AT+CFUN=1,1"); 
+        _parser->send("AT+CFUN=1"); 
     } else {
         _parser->send("AT+CFUN=0"); //GPS power off
     }
-    
     return checkIfOk();    
 }
 
-bool setPowerSavingMode() {
-    _parser->send("AT+CPSMS=1");//power save
+bool setPowerSavingMode() 
+{
+    _parser->send("AT+CPSMS=1");//power save on 
+    wait(0.2);
     return checkIfOk(); 
 }
 
@@ -199,7 +169,8 @@
     return checkIfOk();    
 }
 
-bool getSignalQuality() {
+bool getSignalQuality() 
+{
     _parser->send("AT+CSQ");
     
     if(_parser->recv("+CSQ: %d,%d", &rssiDB,&rxQual) && _parser->recv("OK")) {
@@ -209,7 +180,7 @@
 }
 
 bool setAPN() {
-     _parser->send("AT+CNACT=1,\"iot\"");
+    _parser->send("AT+CNACT=1,\"iot\"");
     return checkIfOk();
 }
 
@@ -218,94 +189,280 @@
     return checkIfOk();
 }
 
-void initSIM(){
-    enableRF(1);
-    enableGPS(0);
-    setSatSystems();
-    wd.Service();
-    setPowerSavingMode();
-    setAPN();
+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 && !setSatSystems())
+    {
+        
+        wait(2);
+        wd.Service();
+        fixTries++;
+        if (fixTries >= COMD_EXE_TRIES)
+        {
+            return 0;
+        }
+    }
     wd.Service();    
+    
+ 
+
+    return true;
 }
 
 bool getGPS() {
     wd.Service();
-    _parser->send("AT+CGNSTST=1,1");
-    //wait(PARSER_TIMEOUT_S);
+    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");
+
+    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);
     
-    if(_parser->recv("OK")) {
-        int nmeaStrLen=_parser->read(bufferString, 2048);
-        
-        char *curLine = bufferString;
-        while(curLine)
+    
+
+    int GNSS_st = atof(GNSSrunstatus_c); 
+    Fix_st      = atof(Fixstatus_c); 
+    int sat_count = atof(satellitesinview_c); 
+
+    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 > 5 )
         {
-          char *nextLine = strchr(curLine, '\n');
-          if (nextLine) *nextLine = '\0';  // temporarily terminate the current line
-          if(curLine[0]=='$'){
-           nmea_parse(curLine);
-           #if DEBUG_PC
-           pc.printf("curLine=[%s]\n", curLine);
-           #endif
-          }
-          if (nextLine) *nextLine = '\n';  // then restore newline-char, just to be tidy    
-          curLine = nextLine ? (nextLine+1) : NULL;
+            return true;
         }
-        return fix>0;
-    } else{
-        int nmeaStrLen=_parser->read(bufferString, 2048);
-        pc.printf("curLine=[%s]\n", bufferString);
+        else
+        {
+            step_p++;
         }
+    }
     return false;
 }
 
-void terminateCOAPSession() {
+int terminateCOAPSession() {
+
+    for(int try_c = 0; try_c < 5; try_c++)
+    {
+    _parser->flush(); 
     _parser->send("AT+CCOAPTERM");
-    checkIfOk();
+    int r1 = checkIfOk();
     wait(0.5);
     _parser->send("AT+CNACT=0");
-    checkIfOk();
+    int r2 = checkIfOk();
+    if (r1 == r2 == true)
+    {
+        return true;
+    }
+    else
+    {
+        if(try_c >=5)
+        {
+            return false;
+        }
+    }
+    }
+
 }
 
-bool sendTelemetry() {
+
+
+bool sendTelemetry() 
+{
+    int check = 0;
     wd.Service();
+    int tr = 0;
+
     setAPN();
-    wait(1);
+    
+    wd.Service();
+    check = tr = 0;
+    wait(2);
+
+    _parser->flush();   
     _parser->send("AT+CCOAPINIT");
-    if(!checkIfOk()){
-        terminateCOAPSession();
-        return false;
-    }
+  
+
     wait(1);
+     _parser->flush(); 
     _parser->send("AT+CCOAPURL=\"" MTS_TELEMETRY_URL_STRING);
-    if(!checkIfOk()){ terminateCOAPSession();return false;}
+    checkIfOk();
+    //if(!checkIfOk()){ terminateCOAPSession();return false;}
     wait(1);
-    _parser->printf("AT+CCOAPPARA=code,2,token,0,\"%s\",payload,1,",xstr(MTS_COAP_TOKEN));
-    
+     _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,");
+     
     for (size_t i = 0; i < strlen(bufferString); i++) {
     _parser->printf("%x",bufferString[i]);
     }
     
     _parser->printf("\r\n");
-    if(!checkIfOk()){ terminateCOAPSession();return false;}
+    checkIfOk();
+    //if(!checkIfOk()){ terminateCOAPSession();return false;}
     wait(0.5);
+     _parser->flush(); 
     _parser->send("AT+CCOAPACTION");
-    if(!checkIfOk()){ terminateCOAPSession();return false;}
+    //checkIfOk();
+    if(!checkIfOk()){ return false;}
     wait(0.5);
+
     terminateCOAPSession();
     
     return true;
 }
-
 int main() {
     confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)  
-    for(i=0;i<10;i++){
-    myled = 0;
+
+    #if ENABLE_WATCHDOG
+    wd.Configure(WATCHDOG_INTERVAL_S);       // sets the timeout interval
+    #endif
+
+    simPWR = 0; 
+    wait(2);
+    simPWR = 1;
+
+    //blink_slow();
+    wd.Service();
+    for(i=0;i<7;i++)
+    {
+    //myled = 1;
     wait(0.5);
-    myled = 1;
+    //myled = 0;
     wait(0.5);
-    myled = 0;}
+    wd.Service();
+    //myled = 1;
+    }
+
+    //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");
@@ -313,32 +470,67 @@
         pc.printf("Watchdog caused reset.\r\n");
     }
     #endif
+
     
-    #if ENABLE_WATCHDOG
-    wd.Configure(WATCHDOG_INTERVAL_S);       // sets the timeout interval
-    #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 (PARSER_TIMEOUT_S);
-    wait(0.5);
-    
-    initSIM();
+    _parser->set_timeout(PARSER_TIMEOUT_S);
+    wait(2);
+
+    wd.Service();
+    wait(2);
+    wd.Service();
+    wait(2);
+
+    fixTries=0;
+
+    #if DEBUG_PC
+    pc.printf("INIT START\r\n");
+    #endif
+
     
-    #if DEBUG_SIM
-    _parser->send("AT+CMEE=2"); //report debug info
-    checkIfOk();
+
+
+    int res = initSIM();
+    if(!res)
+    {
+        #if DEBUG_PC
+        pc.printf("Init Failed\r\n");
+        #endif
+
+        wait(2);
+        wait(2);
+        wait(2);
+        if(!initSIM());
+        {
+            wait(WATCHDOG_INTERVAL_S+10); // reset by dog 
+        }
+    }
+
+    #if DEBUG_PC
+    pc.printf("INIT finished\r\n");
     #endif
-    _parser->send("AT+CPIN?");
-    if(_parser->recv("+CPIN:READY")) {
-        #if DEBUG_PC
-        pc.printf("SIM ready\r\n");
-        #endif
+
+    for(i=0;i<7;i++)
+    {
+    myled = 1;
+    wait(0.5);
+    myled = 0;
+    wait(0.5);
+    wd.Service();
+    myled = 1;
     }
     
+
+    wd.Service();
+    
     /*_parser->send("AT+CPSI?");
     checkIfOk();
     _parser->send("AT+CGNAPN");
@@ -372,47 +564,158 @@
     
     //Feed the watchdog
     wd.Service();
+
     state=STATE_STARTING_GPS;
     
+    int off = 0;
+
     while(1) {
         wd.Service();
-        if (state==STATE_STARTING_GPS){
+        if (state==STATE_STARTING_GPS)
+        {
+            wd.Service();
+            
+
             #if DEBUG_PC
             pc.printf("STATE=STARTING GPS\r\n");
             #endif
-            setSatSystems();
-            enableGPS(1);
+
+            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(5);
+                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(5);
+                    wd.Service();
+                }
+                
+                
+
+            }
+
+
+        
             state=STATE_WAITING_FIX;
             fixTries=0;
             wd.Service();
-            wait(FIX_CHECK_TIME_S);
-        } else if(state==STATE_WAITING_FIX) {
+             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
-            wd.Service();
-            if(getGPS()){
+
+            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();
-                enableGPS(0);
-                fixTries=0;
+                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 {
+            } 
+            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) {
+                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
-                    enableGPS(0);
+
+                    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 {
+                } 
+                else
+                 {
                     #if DEBUG_PC
                     pc.printf("Waiting %d sec\r\n",FIX_CHECK_TIME_S);
                     #endif
@@ -442,10 +745,10 @@
         
             IRtemp=thermometer.read_temp(1);
             
-            
+            float bat_v = 100;
             //Form JSON as {"tempIR":1,"temps":[1,...,10],"latitude":37,"longitude":51,"altitude":21,"validGeo":true}
-            snprintf(bufferString,2048,"{\"tempIR\":%f,\"latitude\":%f,\"longitude\":%f,\"altitude\":%f,\"validGeo\":%s,temps:[", IRtemp, latitude/10.f, longitude/10.f, altitude, fix ? "true" : "false");
-            
+             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 ? "true" : "false", bat_v, rssiDB);
+           // device_id
             index = strlen(bufferString);
             
             for(i=0;i<SENSORS_COUNT;i++){
@@ -455,22 +758,62 @@
             
             wd.Service();
             state=STATE_SENDING_TELEMETRY;
-        } else if(state==STATE_SENDING_TELEMETRY){
+        } 
+        else if(state==STATE_SENDING_TELEMETRY)
+        {
             #if DEBUG_PC
             pc.printf("STATE=SENDING TELEMETRY\r\n");
             pc.printf(bufferString);
             #endif
-            fixTries=0;
-            while(fixTries<FIX_MAX_TRIES && !sendTelemetry()){
-                disconnectNetwork();
-                initSIM();
-                fixTries++;
-                wd.Service();
-                wait(SLEEP_CHECK_TIME);
+         
+            for(int try_c = 0; try_c < COMD_EXE_TRIES; try_c ++)
+            {
+                if(getSignalQuality())
+                {
+                    break;
+                }
                 wd.Service();
             }
+            
+         
             fixTries=0;
+            
+            while(fixTries<FIX_MAX_TRIES && !sendTelemetry())
+            {
+                wait(1);
+                terminateCOAPSession();
+                wait(3);
+                wd.Service();
+                fixTries++;
+            }
+
+            //disconnectNetwork();
+            //initSIM();
+            //enableGPS(0); 
+            //setPowerSavingMode();
+            //fixTries++;
             wd.Service();
+            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;
         } else if(state==STATE_SLEEPING){
             #if DEBUG_PC
@@ -480,6 +823,59 @@
             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(3);
+                wd.Service();
+                wait(3);
+                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
+                for(i=0;i<7;i++)
+                {
+                    myled = 1;
+                    wait(1);
+                    myled = 0;
+                    wait(1);
+                    wd.Service();
+                    myled = 1;
+                }
+
+                int res = initSIM();
+                if(!res)
+                {
+                    #if DEBUG_PC
+                    pc.printf("Init Failed\r\n");
+                    #endif
+                    wait(5);
+                    if(!initSIM());
+                    {
+                        wait(WATCHDOG_INTERVAL_S+10); // reset by dog 
+                    }
+                    
+
+                   
+                }
+
                 state=STATE_STARTING_GPS;
                 sleepTimer=0;
             } else {
@@ -489,6 +885,8 @@
             #if DEBUG_PC
             pc.printf("After sleep\r\n");
             #endif
+            
+            //from_sleep = true;
         }
 
         //Feed the watchdog