Move GPS decoding routine to a separate thread

stanleyhuangyc · Feb 20, 2018 · 7869a7b · 7869a7b
1 parent 5e6f4c6
commit 7869a7b
Show file tree

Hide file tree

Showing 8 changed files with 166 additions and 160 deletions.
diff --git a/firmware_v5/datalogger/datalogger.ino b/firmware_v5/datalogger/datalogger.ino
@@ -87,16 +87,16 @@ public:
           Serial.print("VIN:");
           Serial.println(buffer);
         }
+        setState(STATE_OBD_READY);
       } else {
         Serial.println("NO");
-        reconnect();
+        standby();
+        return;
       }
-      setState(STATE_OBD_READY);
 #else
       SPI.begin();
 #endif
 
-#if USE_GPS
       if (!checkState(STATE_GPS_FOUND)) {
         Serial.print("GPS ");
         if (gpsInit(GPS_SERIAL_BAUDRATE)) {
@@ -107,7 +107,6 @@ public:
           Serial.println("NO");
         }
       }
-#endif
 
       if (!checkState(STATE_SD_READY)) {
         Serial.print("SD ");
@@ -124,12 +123,12 @@ public:
       startTime = millis();
       dataCount = 0;
     }
-#if USE_GPS
     void logGPSData()
     {
         // issue the command to get parsed GPS data
         GPS_DATA gd = {0};
-        if (checkState(STATE_GPS_FOUND) && gpsGetData(&gd)) {
+        int count = gpsGetData(&gd);
+        if (count) {
             dataTime = millis();
             if (gd.time && gd.time != UTC) {
               byte day = gd.date / 10000;
@@ -148,6 +147,15 @@ public:
               MMDD = (DDMM % 100) * 100 + (DDMM / 100);
               // set GPS ready flag
               setState(STATE_GPS_READY);
+              // output some GPS data
+              Serial.print("GPS DATA:");
+              Serial.print(count);              
+              Serial.print(" UTC:");
+              Serial.print(gd.time);
+              Serial.print(" LAT:");
+              Serial.print(gd.lat);
+              Serial.print(" LNG:");
+              Serial.println(gd.lng);
             }
         }
     }
@@ -171,7 +179,6 @@ public:
           }
         }
     }
-#endif
     uint16_t initSD()
     {
         pinMode(PIN_SD_CS, OUTPUT);
@@ -197,28 +204,17 @@ public:
 #endif
         }
     }
-    void reconnect()
-    {
-        if (obd.init()) return;
-        // try to re-connect to OBD
-        closeFile();
-        // turn off GPS power
-#if USE_GPS
-        gpsInit(0);
-#endif
-        clearState(STATE_OBD_READY | STATE_GPS_READY);
-        standby();
-    }
     void standby()
     {
-  #if ENABLE_GPS
-        if (checkState(STATE_GPS_READY)) {
+        closeFile();
+        if (checkState(STATE_GPS_FOUND)) {
           Serial.print("GPS:");
           gpsInit(0); // turn off GPS power
           Serial.println("OFF");
+          clearState(STATE_GPS_FOUND | STATE_GPS_READY);
         }
-  #endif
-        clearState(STATE_OBD_READY | STATE_GPS_READY);
+        pidErrors = 0;
+        clearState(STATE_OBD_READY);
         Serial.println("Standby");
         ble.println("Standby");
   #if MEMS_MODE
@@ -273,10 +269,11 @@ void showStats()
     Serial.print(logger.dataCount);
     Serial.print(" samples | ");
     Serial.print(sps, 1);
-    Serial.print(" sps | ");
+    Serial.print(" sps");
     if (fileSize > 0) {
       digitalWrite(PIN_LED, HIGH);
       logger.flushData(fileSize);
+      Serial.print(" | ");
       Serial.print(fileSize);
       Serial.print(" bytes");
       digitalWrite(PIN_LED, LOW);
@@ -330,7 +327,6 @@ void setup()
 
     logger.setup();
     digitalWrite(PIN_LED, LOW);
-    delay(1000);
 }
 
 void loop()
@@ -345,7 +341,6 @@ void loop()
 #endif
     if (!logger.checkState(STATE_FILE_READY) && logger.checkState(STATE_SD_READY)) {
       digitalWrite(PIN_LED, HIGH);
-#if USE_GPS
       if (logger.checkState(STATE_GPS_FOUND)) {
         // GPS connected
         logger.logGPSData();
@@ -360,14 +355,13 @@ void loop()
         }
       }
       else
-#endif
       {
         // no GPS connected
         if (logger.openFile(0)) {
           logger.setState(STATE_FILE_READY);
         }
       }
-      logger.sleep(1000);
+      delay(1000);
       digitalWrite(PIN_LED, LOW);
       return;
     }
@@ -387,7 +381,8 @@ void loop()
         ble.println(pidErrors);
         if (obd.errors >= 3) {
           obd.reset();
-          logger.reconnect();
+          logger.standby();
+          return;
         }
       }
 #endif
@@ -431,9 +426,9 @@ void loop()
     logger.log(PID_BATTERY_VOLTAGE, v);
 #endif
 
-#if USE_GPS
-    logger.logGPSData();
-#endif
+    if (logger.checkState(STATE_GPS_FOUND)) {
+      logger.logGPSData();
+    }
 
     showStats();
 }
diff --git a/firmware_v5/telelogger/telelogger.ino b/firmware_v5/telelogger/telelogger.ino
@@ -225,7 +225,7 @@ public:
       Serial.print("Waiting GPS time..");
       for (int i = 0; gdata.date == 0 && i < 60; i++) {
         Serial.print('.');
-        sleep(1000);
+        delay(1000);
         gpsGetData(&gdata);
       }
       Serial.println();
@@ -885,6 +885,11 @@ CTeleLogger logger;
 void setup()
 {
     delay(1000);
+
+    // init LED pin
+    pinMode(PIN_LED, OUTPUT);
+    digitalWrite(PIN_LED, HIGH);
+
     // initialize USB serial
     Serial.begin(115200);
     Serial.print("ESP32 ");
@@ -893,12 +898,9 @@ void setup()
     Serial.print(getFlashSize() >> 10);
     Serial.println("MB Flash");
 
-    // init LED pin
-    pinMode(PIN_LED, OUTPUT);
     // perform initializations
     BLE.begin(BLE_DEVICE_NAME);
 
-    digitalWrite(PIN_LED, HIGH);
     logger.setup();
     digitalWrite(PIN_LED, LOW);
 }
@@ -913,7 +915,7 @@ void loop()
         logger.setup();
         digitalWrite(PIN_LED, LOW);
         if (logger.checkState(STATE_ALL_GOOD)) break;
-        logger.sleep(3000);
+        delay(3000);
       }
       return;
     }

diff --git a/libraries/FreematicsPlus/FreematicsBase.h b/libraries/FreematicsPlus/FreematicsBase.h
@@ -44,8 +44,6 @@ class CFreematics
 public:
 	// hibernate (lower power consumption)
 	virtual void hibernate(unsigned int ms) { delay(ms); }
-	// normal delay
-	virtual void sleep(unsigned int ms) { delay(ms); }
 	// start xBee UART communication
 	virtual bool xbBegin(unsigned long baudrate = 115200L) = 0;
 	// read data to xBee UART

diff --git a/libraries/FreematicsPlus/FreematicsNetwork.cpp b/libraries/FreematicsPlus/FreematicsNetwork.cpp
@@ -93,7 +93,7 @@ bool CTeleClientSIM800::netBegin()
   for (byte n = 0; n < 10; n++) {
     // try turning on module
     xbTogglePower();
-    sleep(3000);
+    delay(3000);
     // discard any stale data
     xbPurge();
     for (byte m = 0; m < 3; m++) {
@@ -319,7 +319,7 @@ bool CTeleClientSIM5360::netBegin()
   for (byte n = 0; n < 10; n++) {
     // try turning on module
     xbTogglePower();
-    sleep(3000);
+    delay(3000);
     // discard any stale data
     xbPurge();
     for (byte m = 0; m < 5; m++) {
@@ -349,7 +349,7 @@ bool CTeleClientSIM5360::netSetup(const char* apn, bool only3G, unsigned int tim
   do {
     do {
       Serial.print('.');
-      sleep(500);
+      delay(500);
       success = netSendCommand("AT+CPSI?\r", 1000, "Online");
       if (success) {
         if (!strstr(m_buffer, "NO SERVICE"))
@@ -400,7 +400,7 @@ String CTeleClientSIM5360::getIP()
         }
       }
     }
-    sleep(500);
+    delay(500);
   } while (millis() - t < 15000);
   return "";
 }

diff --git a/libraries/FreematicsPlus/FreematicsOBD.cpp b/libraries/FreematicsPlus/FreematicsOBD.cpp
@@ -13,8 +13,6 @@
 HardwareSerial OBDUART(1);
 #endif
 
-void gps_decode_task(int timeout);
-
 #define SAFE_MODE 1
 //#define DEBUG Serial
 
@@ -772,6 +770,6 @@ void COBDSPI::sleep(unsigned int ms)
 	for (;;) {
 		uint32_t elapsed = millis() - t;
 		if (elapsed > ms) break;
-		gps_decode_task(ms - elapsed);
+		idleTasks();
 	}
 }
diff --git a/libraries/FreematicsPlus/FreematicsOBD.h b/libraries/FreematicsPlus/FreematicsOBD.h
@@ -77,10 +77,9 @@ class COBD
 	virtual int normalizeData(byte pid, char* data);
 	virtual byte checkErrorMessage(const char* buffer);
 	virtual char* getResultValue(char* buf);
+	virtual void idleTasks() { delay(10); }
+	virtual void recover();
 	OBD_STATES m_state = OBD_DISCONNECTED;
-private:
-	void recover();
-	virtual void idleTasks() {}
 };
 
 class COBDSPI : public COBD {