From 783a7802070fba82cf43a44f1df8e1979917b470 Mon Sep 17 00:00:00 2001
From: Femtomes <55222239+Femtomes@users.noreply.github.com>
Date: Mon, 3 May 2021 01:46:39 +0800
Subject: [PATCH] drivers/gps: add femtomes gps driver protocol

---
 src/drivers/gps/CMakeLists.txt |  1 +
 src/drivers/gps/gps.cpp        | 22 ++++++++++++++++++++--
 src/drivers/gps/params.c       |  6 ++++--
 3 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt
index dda436354b..46c41c8792 100644
--- a/src/drivers/gps/CMakeLists.txt
+++ b/src/drivers/gps/CMakeLists.txt
@@ -47,6 +47,7 @@ px4_add_module(
 		devices/src/ubx.cpp
 		devices/src/rtcm.cpp
 		devices/src/emlid_reach.cpp
+		devices/src/femtomes.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a024acdcda..f20d9a8250 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -67,6 +67,7 @@
 # include "devices/src/ashtech.h"
 # include "devices/src/emlid_reach.h"
 # include "devices/src/mtk.h"
+# include "devices/src/femtomes.h"
 #endif // CONSTRAINED_FLASH
 #include "devices/src/ubx.h"
 
@@ -82,7 +83,8 @@ typedef enum {
 	GPS_DRIVER_MODE_UBX,
 	GPS_DRIVER_MODE_MTK,
 	GPS_DRIVER_MODE_ASHTECH,
-	GPS_DRIVER_MODE_EMLIDREACH
+	GPS_DRIVER_MODE_EMLIDREACH,
+	GPS_DRIVER_MODE_FEMTOMES
 } gps_driver_mode_t;
 
 /* struct for dynamic allocation of satellite info data */
@@ -774,6 +776,11 @@ GPS::run()
 			_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
 			set_device_type(DRV_GPS_DEVTYPE_EMLID_REACH);
 			break;
+
+		case GPS_DRIVER_MODE_FEMTOMES:
+			_helper = new GPSDriverFemto(&GPS::callback, this, &_report_gps_pos/*, _p_report_sat_info*/);
+			set_device_type(DRV_GPS_DEVTYPE_FEMTOMES);
+			break;
 #endif // CONSTRAINED_FLASH
 
 		default:
@@ -912,6 +919,10 @@ GPS::run()
 				break;
 
 			case GPS_DRIVER_MODE_EMLIDREACH:
+				_mode = GPS_DRIVER_MODE_FEMTOMES;
+				break;
+
+			case GPS_DRIVER_MODE_FEMTOMES:
 #endif // CONSTRAINED_FLASH
 				_mode = GPS_DRIVER_MODE_UBX;
 				px4_usleep(500000); // tried all possible drivers. Wait a bit before next round
@@ -969,6 +980,10 @@ GPS::print_status()
 	case GPS_DRIVER_MODE_EMLIDREACH:
 		PX4_INFO("protocol: EMLIDREACH");
 		break;
+
+	case GPS_DRIVER_MODE_FEMTOMES:
+		PX4_INFO("protocol: FEMTOMES");
+		break;
 #endif // CONSTRAINED_FLASH
 
 	default:
@@ -1135,7 +1150,7 @@ $ gps reset warm
 
 	PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('j', "uart", "spi|uart", "secondary GPS interface", true);
-	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml", "GPS Protocol (default=auto select)", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml|fem", "GPS Protocol (default=auto select)", true);
 
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset GPS device");
@@ -1276,6 +1291,9 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 
 			} else if (!strcmp(myoptarg, "eml")) {
 				mode = GPS_DRIVER_MODE_EMLIDREACH;
+
+			} else if (!strcmp(myoptarg, "fem")) {
+				mode = GPS_DRIVER_MODE_FEMTOMES;
 #endif // CONSTRAINED_FLASH
 			} else {
 				PX4_ERR("unknown protocol: %s", myoptarg);
diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c
index fd64bf221d..d872bbc1ac 100644
--- a/src/drivers/gps/params.c
+++ b/src/drivers/gps/params.c
@@ -115,12 +115,13 @@ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
  * Auto-detection will probe all protocols, and thus is a bit slower.
  *
  * @min 0
- * @max 4
+ * @max 5
  * @value 0 Auto detect
  * @value 1 u-blox
  * @value 2 MTK
  * @value 3 Ashtech / Trimble
  * @value 4 Emlid Reach
+ * @value 5 Femtomes
  *
  * @reboot_required true
  * @group GPS
@@ -135,12 +136,13 @@ PARAM_DEFINE_INT32(GPS_1_PROTOCOL, 1);
  * Auto-detection will probe all protocols, and thus is a bit slower.
  *
  * @min 0
- * @max 4
+ * @max 5
  * @value 0 Auto detect
  * @value 1 u-blox
  * @value 2 MTK
  * @value 3 Ashtech / Trimble
  * @value 4 Emlid Reach
+ * @value 5 Femtomes
  *
  * @reboot_required true
  * @group GPS