GPS watchdog - health detection fixes

2 files changed, 7 insertions, 12 deletions

diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c
index 604dba05c..7ba4f52b0 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -410,6 +410,8 @@ void *mtk_watchdog_loop(void *args)
 		} else {
 			/* gps healthy */
 			mtk_success_count++;
+			mtk_fail_count = 0;
+			once_ok = true; // XXX Should this be true on a single success, or on same criteria as mtk_healthy?
 
 			if (!mtk_healthy && mtk_success_count >= MTK_HEALTH_SUCCESS_COUNTER_LIMIT) {
 				printf("[gps] MTK module found, status ok (baud=%d)\r\n", current_gps_speed);
@@ -418,11 +420,8 @@ void *mtk_watchdog_loop(void *args)
 				mtk_gps->satellite_info_available = 0;
 				// global_data_send_subsystem_info(&mtk_present_enabled_healthy);
 				mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n");
+				mtk_healthy = true;
 			}
-
-			mtk_healthy = true;
-			mtk_fail_count = 0;
-			once_ok = true;
 		}
 
 		usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index 21e917bf8..2bbecb12e 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -786,22 +786,18 @@ void *ubx_watchdog_loop(void *args)
 			sleep(1);
 
 		} else {
+			/* gps healthy */
+			ubx_success_count++;
+			ubx_fail_count = 0;
+			once_ok = true; // XXX Should this be true on a single success, or on same criteria as ubx_healthy?
 
 			if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
 				//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
 				// global_data_send_subsystem_info(&ubx_present_enabled_healthy);
 				mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n");
 				ubx_healthy = true;
-				ubx_fail_count = 0;
-				once_ok = true;
 			}
-
-			/* gps healthy */
-			ubx_success_count++;
-			ubx_healthy = true;
-			ubx_fail_count = 0;
 		}
-
 		usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
 	}