From e6ad321ab2df10b8585d4ae4140b5ff5765966a9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 13 Oct 2020 16:55:52 +0200
Subject: [PATCH] gps: add GPS_{1,2}_PROTOCOL param to select protocol, default
 to u-blox

u-blox is the most widely used GPS, so module detection should be a bit
faster in general.
---
 src/drivers/gps/gps.cpp  | 20 +++++++++++++++++++-
 src/drivers/gps/params.c | 39 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 34f2bf1322..39195d17fd 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -279,7 +279,25 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
 	}
 
-	_mode_auto = mode == GPS_DRIVER_MODE_NONE;
+	if (_mode == GPS_DRIVER_MODE_NONE) {
+		// use parameter to select mode if not provided via CLI
+		char protocol_param_name[16];
+		snprintf(protocol_param_name, sizeof(protocol_param_name), "GPS_%i_PROTOCOL", (int)_instance + 1);
+		int32_t protocol = 0;
+		param_get(param_find(protocol_param_name), &protocol);
+
+		switch (protocol) {
+		case 1: _mode = GPS_DRIVER_MODE_UBX; break;
+
+		case 2: _mode = GPS_DRIVER_MODE_MTK; break;
+
+		case 3: _mode = GPS_DRIVER_MODE_ASHTECH; break;
+
+		case 4: _mode = GPS_DRIVER_MODE_EMLIDREACH; break;
+		}
+	}
+
+	_mode_auto = _mode == GPS_DRIVER_MODE_NONE;
 }
 
 GPS::~GPS()
diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c
index ed5a7b3480..7cf6fc78f8 100644
--- a/src/drivers/gps/params.c
+++ b/src/drivers/gps/params.c
@@ -107,3 +107,42 @@ PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
  */
 PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
 
+/**
+ * Protocol for Main GPS
+ *
+ * Select the GPS protocol over serial.
+ *
+ * Auto-detection will probe all protocols, and thus is a bit slower.
+ *
+ * @min 0
+ * @max 4
+ * @value 0 Auto detect
+ * @value 1 u-blox
+ * @value 2 MTK
+ * @value 3 Ashtech / Trimble
+ * @value 4 Emlid Reach
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_1_PROTOCOL, 1);
+
+/**
+ * Protocol for Secondary GPS
+ *
+ * Select the GPS protocol over serial.
+ *
+ * Auto-detection will probe all protocols, and thus is a bit slower.
+ *
+ * @min 0
+ * @max 4
+ * @value 0 Auto detect
+ * @value 1 u-blox
+ * @value 2 MTK
+ * @value 3 Ashtech / Trimble
+ * @value 4 Emlid Reach
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_2_PROTOCOL, 1);