diff --git a/FreematicsOBD/datalogger/customized_GPS.cpp b/FreematicsOBD/datalogger/customized_GPS.cpp
index eceabb2..03e218e 100644
--- a/FreematicsOBD/datalogger/customized_GPS.cpp
+++ b/FreematicsOBD/datalogger/customized_GPS.cpp
@@ -5,6 +5,7 @@
 
 extern void clr_buff(char*,uint32_t);
 extern FreematicsESP32 sys;
+int init_gps=0;
 GPS_DATA* gd=0;
 
 void init_GPS_dev(void){
@@ -23,6 +24,7 @@ void init_GPS_dev(void){
 	pinMode(PIN_GPS_POWER, OUTPUT);
 	digitalWrite(PIN_GPS_POWER, HIGH);
 	int cnt=0;
+	init_gps=0;
 	while(!isDataGPSReady(10000)){
 		delay(100);
 		cnt++;
@@ -32,15 +34,27 @@ void init_GPS_dev(void){
 			esp_deep_sleep_start();
 		}
 	}
+	init_gps=1;
 	UpdateTime();
 	SetSysInstance(&sys);
 }
 
 int isDataGPSReady(int wait_MS){
 	for (uint32_t t=millis();millis()-t<wait_MS;){
-		digitalWrite(26, digitalRead(34));
+		if (!init_gps){
+			digitalWrite(26, digitalRead(34));
+		}else{
+			vTaskSuspendAll();
+			digitalWrite(26, digitalRead(34));
+			xTaskResumeAll();
+		}
 		if (sys.gpsGetData(&gd))
 			return 1;
+		if (!init_gps){ 
+			delay(5);
+		}else{
+			vTaskDelay(pdMS_TO_TICKS(5));
+		}
 	}
 	return 0;
 }
@@ -111,13 +125,11 @@ int FilterGpsData(char* isoTime, double* res){
 	
 	if (slope_flag){
 		double dist 		= (double) TinyGPS::distance_between((float) meanGPSCoord[prev][2],(float) meanGPSCoord[prev][1],(float) res[2],(float) res[1]);
-		#if USE_OBD==0
-			double dist_max		= gd->speed*shiftTime/3.6*1.25;
-		#else
-			double dist_max		= GetOBDSpeed()*shiftTime/3.6*1.2;
-		#endif
-		if ((dist>dist_max) || (dist>(85*shiftTime)))
+		//double dist_max		= gd->speed*shiftTime/3.6*1.25;
+		
+		if (dist>(85*shiftTime))
 			return -1;
+		
 		res[3]=(res[0]-meanGPSCoord[prev][0])/(dist+10)*100;
 		meanGPSCoord[n][0]=res[0];
 		meanGPSCoord[n][1]=res[1];
@@ -129,40 +141,9 @@ int FilterGpsData(char* isoTime, double* res){
 	for (j=0;j<time_size;j++){
 		lastTimeStamp2[j]=isoTime[j];
 	}
-	#if USE_OBD==0
-		isVehicleStopped(isoTime,res[4]);
-	#endif
 	return slope_flag;
 }
 
-void isVehicleStopped(char* isoTime,double val){
-	static char lastTimeStamp[time_size]={0};
-	static float meanSpeed=0;
-	int j;
-
-	if (lastTimeStamp[0]){
-		if (GetShiftTime(lastTimeStamp,isoTime)>NBR_GPS_POINTS){
-			if ((meanSpeed<2)&&(abs(val-meanSpeed)<0.5)){
-				esp_sleep_enable_timer_wakeup(5 * uS_TO_S_FACTOR);
-				esp_deep_sleep_start();
-			}else{
-				meanSpeed=val;
-				for (j=0;j<time_size;j++){
-					lastTimeStamp[j]=isoTime[j];
-				}
-			}
-		}else{
-			meanSpeed+=val;
-			meanSpeed/=2;
-		}
-	}else{
-		meanSpeed=val;
-		for (j=0;j<time_size;j++){
-			lastTimeStamp[j]=isoTime[j];
-		}
-	}
-}
-
 int AddGPSData(char* isoTime,char* p, int* nbr){
 	
 	int GPS_Stat=0;
diff --git a/FreematicsOBD/datalogger/customized_GPS.h b/FreematicsOBD/datalogger/customized_GPS.h
index 7dc5480..1b4880a 100644
--- a/FreematicsOBD/datalogger/customized_GPS.h
+++ b/FreematicsOBD/datalogger/customized_GPS.h
@@ -10,9 +10,6 @@ int isDataGPSReady(int);
 //return -1 if GPS coordinates are incorrect accroding to speed
 int FilterGpsData(char*, double*);
 
-//check if the vehicle has stopped
-void isVehicleStopped(char*,double);
-
 int AddGPSData(char*,char*,int*);
 
 #endif /* __customized_GPS_H */
diff --git a/FreematicsOBD/datalogger/customized_OBD.cpp b/FreematicsOBD/datalogger/customized_OBD.cpp
index feb473d..c14e321 100644
--- a/FreematicsOBD/datalogger/customized_OBD.cpp
+++ b/FreematicsOBD/datalogger/customized_OBD.cpp
@@ -15,36 +15,21 @@ static SemaphoreHandle_t xSemaphore_err=NULL;
 //Semaphore to synchronize access to ODB
 static SemaphoreHandle_t xSemaphore_OBD=NULL;
 
-static SemaphoreHandle_t xSemaphore_Speed=NULL;
-
-int OBD_Speed = 0;
-
-
-int GetOBDSpeed(void){
-	int res=0;
-	xSemaphoreTake(xSemaphore_Speed,portMAX_DELAY);
-	res = OBD_Speed;
-	xSemaphoreGive(xSemaphore_Speed);
-	return res;
-}
-void setOBDSpeed(int val){
-	xSemaphoreTake(xSemaphore_Speed,portMAX_DELAY);
-	OBD_Speed = val;
-	xSemaphoreGive(xSemaphore_Speed);
-}
-
 
 void init_OBD_dev(void){
 	xSemaphore_err= xSemaphoreCreateMutex();
 	//creating mutex to synchronze the access to OBD
 	xSemaphore_OBD= xSemaphoreCreateMutex();
-	xSemaphore_Speed= xSemaphoreCreateMutex();
 	obd.begin(sys.link);
 	int cnt=0;
 	while(!set_obd()){
 		cnt++;
 		delay(200);
-		if (cnt==10){
+		#if USE_OBD==0
+			break;
+		#endif
+
+		if (cnt==5){
 			esp_sleep_enable_timer_wakeup(5 * uS_TO_S_FACTOR);
 			esp_deep_sleep_start();
 		}
@@ -72,11 +57,14 @@ void ErrorOBDCheck(void){
 		obd.reset();
 		int cnt=0;
 		while(!set_obd()){
-			delay(200);
+			#if USE_OBD==0
+				break;
+			#endif
 			if (cnt==5){
 				esp_sleep_enable_timer_wakeup(2 * uS_TO_S_FACTOR);
 				esp_deep_sleep_start();
 			}
+			cnt++;
 		}
 	}
 	xSemaphoreGive(xSemaphore_err);
@@ -126,7 +114,7 @@ int OBD_looping(ObdInfoPid* PIDs, int nbr_elem,int fr_max){
 	int j;
 	int val;
 	int OBD_flag;
-	int res=5;
+	int res=90;
 	
 	if (!fr_max)
 		res=60000;
@@ -184,13 +172,14 @@ void CheckObdSpeed(char* label,char* isoTime,int val, int* res){
 	int j;
 	
 	if (!strcmp(label,"PID_SPEED")){
-		setOBDSpeed(val);
 		if (lastTimeStamp[0]){
 			if (GetShiftTime(lastTimeStamp,isoTime)>3){
 				if (abs(val-meanSpeed)<5){
 					if (val<2){
-						esp_sleep_enable_timer_wakeup(5 * uS_TO_S_FACTOR);
-						esp_deep_sleep_start();
+						#if STOP_SLEEP==1
+							esp_sleep_enable_timer_wakeup(5 * uS_TO_S_FACTOR);
+							esp_deep_sleep_start();
+						#endif
 					}
 					*res=100;
 				}else{
@@ -210,4 +199,4 @@ void CheckObdSpeed(char* label,char* isoTime,int val, int* res){
 			}
 		}
 	}
-}
\ No newline at end of file
+}
diff --git a/FreematicsOBD/datalogger/simple_obd_test.bin b/FreematicsOBD/datalogger/simple_obd_test.bin
index 8a10cd2..320dc50 100644
Binary files a/FreematicsOBD/datalogger/simple_obd_test.bin and b/FreematicsOBD/datalogger/simple_obd_test.bin differ
diff --git a/FreematicsOBD/datalogger/simple_obd_test.h b/FreematicsOBD/datalogger/simple_obd_test.h
index 8cac837..863d5f2 100644
--- a/FreematicsOBD/datalogger/simple_obd_test.h
+++ b/FreematicsOBD/datalogger/simple_obd_test.h
@@ -38,13 +38,13 @@
 #include <freertos/portmacro.h>
 #include <freertos/semphr.h>
 
-#define USE_SERIAL 0
+#define USE_SERIAL 1
 #define uS_TO_S_FACTOR 1000000  /* Conversion factor for micro seconds to seconds */
 #define STACK_SIZE	1024 // in words (= 4*STACK_SIZE bytes)
 #define buff_SIZE 500
 #define time_size 100
 #define USE_OBD 1
-#define NBR_GPS_POINTS 10
-
+#define NBR_GPS_POINTS 1
+#define SLEEP_STOP 0
 
 #endif /* __simple_obd_test_H */
diff --git a/FreematicsOBD/datalogger/simple_obd_test.ino b/FreematicsOBD/datalogger/simple_obd_test.ino
index 29c83b8..31840f7 100644
--- a/FreematicsOBD/datalogger/simple_obd_test.ino
+++ b/FreematicsOBD/datalogger/simple_obd_test.ino
@@ -70,9 +70,8 @@ void setup(){
 	//initialize MEMS device
 	init_MEMS_dev();
 
-	#if USE_OBD==1
-		init_OBD_dev();
-	#endif
+	
+	init_OBD_dev();
 
 	// turn on buzzer at 2000Hz frequency 
 	sys.buzzer(2000);
@@ -81,20 +80,16 @@ void setup(){
 	sys.buzzer(0);
 
 	//creating queue with a size (15*sizeof(Irp*))
-	xQueue_print = xQueueCreate(15,sizeof(Irp*));
+	xQueue_print = xQueueCreate(25,sizeof(Irp*));
 	
-	#if USE_OBD==1
-		//f=fmax
-		xTaskCreate(write_task_1,"write_task_1",STACK_SIZE*3,(void*) 0,tskIDLE_PRIORITY+1,NULL);
-	#endif
+	//f=fmax
+	xTaskCreate(write_task_1,"write_task_1",STACK_SIZE*3,(void*) 0,tskIDLE_PRIORITY+1,NULL);
 
 	//f=10Hz
-	xTaskCreate(write_task_2,"write_task_2",STACK_SIZE*5,(void*) 0,tskIDLE_PRIORITY+4,NULL);
+	xTaskCreate(write_task_2,"write_task_2",STACK_SIZE*5,(void*) 0,tskIDLE_PRIORITY+1,NULL);
 
-	#if USE_OBD==1
-		//f=1/60 Hz
-		xTaskCreate(write_task_3,"write_task_3",STACK_SIZE*3,(void*) 0,tskIDLE_PRIORITY+1,NULL);
-	#endif
+	//f=1/60 Hz
+	xTaskCreate(write_task_3,"write_task_3",STACK_SIZE*3,(void*) 0,tskIDLE_PRIORITY+1,NULL);
 	
 	//create daemon task
 	xTaskCreate(vTask_Transmit,"vTask_Transmit",STACK_SIZE*40,(void*) 0,tskIDLE_PRIORITY+3,NULL);
@@ -110,6 +105,7 @@ void write_task_1(void* parameter)
 		sleep_tme=OBD_looping(PIDs,nbr_elem,1);
 		taskYIELD();
 		ErrorOBDCheck();
+		Serial.println("OBD signal lost !");
 		vTaskDelay(pdMS_TO_TICKS(sleep_tme));
 	}
 	vTaskDelete(NULL);
@@ -139,7 +135,7 @@ void write_task_2(void* parameter)
 			sprintf(p,"\n");
 			print_data(buff,nbr+1);
 		}
-		vTaskDelay(pdMS_TO_TICKS(100));
+		vTaskDelay(pdMS_TO_TICKS(25));
 	}
 	vTaskDelete(NULL);
 }
@@ -180,7 +176,7 @@ void vTask_Transmit(void *pvParameters){
 		appendFile(SD, getFileName(),buff);
 		//free the semaphore
 			xSemaphoreGive(ReceiveItem->completion);			
-		taskYIELD();
+		vTaskDelay(pdMS_TO_TICKS(20));
 	}
 
 }
--
libgit2 0.21.4