From 7e0f929ad1b0ef391e6587bb30476247e9a675dd Mon Sep 17 00:00:00 2001
From: Seppe Geuens <geuensseppe@gmail.com>
Date: Thu, 13 Apr 2023 09:44:35 +0400
Subject: [PATCH] gps: add sbf with heading (#19374)

Co-authored-by: Seppe Geuens <seppe.geuens@tii.ae>
---
 src/drivers/gps/CMakeLists.txt |  1 +
 src/drivers/gps/devices        |  2 +-
 src/drivers/gps/gps.cpp        | 29 ++++++++++++++++++++++++++++-
 src/drivers/gps/params.c       | 23 +++++++++++++++++++++--
 4 files changed, 51 insertions(+), 4 deletions(-)

diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt
index 4719e0e34e..9796d24fb3 100644
--- a/src/drivers/gps/CMakeLists.txt
+++ b/src/drivers/gps/CMakeLists.txt
@@ -51,6 +51,7 @@ px4_add_module(
 		devices/src/nmea.cpp
 		devices/src/unicore.cpp
 		devices/src/crc.cpp
+		devices/src/sbf.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index c46ef0831c..f0b892fab9 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit c46ef0831cf7b4538fd8e3ec7fbd6d0d636be731
+Subproject commit f0b892fab957c6e0c19cfba7802671edb3f40144
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 8a4b929fd8..bae8460428 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -71,6 +71,8 @@
 # include "devices/src/mtk.h"
 # include "devices/src/femtomes.h"
 # include "devices/src/nmea.h"
+# include "devices/src/sbf.h"
+
 #endif // CONSTRAINED_FLASH
 #include "devices/src/ubx.h"
 
@@ -91,7 +93,8 @@ enum class gps_driver_mode_t {
 	ASHTECH,
 	EMLIDREACH,
 	FEMTOMES,
-	NMEA
+	NMEA,
+	SBF
 };
 
 enum class gps_dump_comm_mode_t : int32_t {
@@ -349,6 +352,8 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		case 5: _mode = gps_driver_mode_t::FEMTOMES; break;
 
 		case 6: _mode = gps_driver_mode_t::NMEA; break;
+
+		case 7: _mode = gps_driver_mode_t::SBF; break;
 #endif // CONSTRAINED_FLASH
 		}
 	}
@@ -750,6 +755,13 @@ GPS::run()
 		heading_offset = matrix::wrap_pi(math::radians(heading_offset));
 	}
 
+	handle = param_find("GPS_PITCH_OFFSET");
+	float pitch_offset = 0.f;
+
+	if (handle != PARAM_INVALID) {
+		param_get(handle, &pitch_offset);
+	}
+
 	int32_t gps_ubx_dynmodel = 7; // default to 7: airborne with <2g acceleration
 	handle = param_find("GPS_UBX_DYNMODEL");
 
@@ -889,6 +901,11 @@ GPS::run()
 			_helper = new GPSDriverNMEA(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
 			set_device_type(DRV_GPS_DEVTYPE_NMEA);
 			break;
+
+		case gps_driver_mode_t::SBF:
+			_helper = new GPSDriverSBF(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset, pitch_offset);
+			set_device_type(DRV_GPS_DEVTYPE_SBF);
+			break;
 #endif // CONSTRAINED_FLASH
 
 		default:
@@ -1059,6 +1076,10 @@ GPS::run()
 				break;
 
 			case gps_driver_mode_t::FEMTOMES:
+				_mode = gps_driver_mode_t::SBF;
+				break;
+
+			case gps_driver_mode_t::SBF:
 			case gps_driver_mode_t::NMEA: // skip NMEA for auto-detection to avoid false positive matching
 #endif // CONSTRAINED_FLASH
 				_mode = gps_driver_mode_t::UBX;
@@ -1120,6 +1141,9 @@ GPS::print_status()
 	case gps_driver_mode_t::NMEA:
 		PX4_INFO("protocol: NMEA");
 		break;
+
+	case gps_driver_mode_t::SBF:
+		PX4_INFO("protocol: SBF");
 #endif // CONSTRAINED_FLASH
 
 	default:
@@ -1491,6 +1515,9 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance)
 
 			} else if (!strcmp(myoptarg, "nmea")) {
 				mode = gps_driver_mode_t::NMEA;
+
+			} else if (!strcmp(myoptarg, "sbf")) {
+				mode = gps_driver_mode_t::SBF;
 #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 f6f37d2085..49b4da7065 100644
--- a/src/drivers/gps/params.c
+++ b/src/drivers/gps/params.c
@@ -162,12 +162,30 @@ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0);
  * @max 360
  * @unit deg
  * @reboot_required true
- * @decimal 0
+ * @decimal 3
  *
  * @group GPS
  */
 PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
 
+/**
+ * Pitch offset for dual antenna GPS
+ *
+ * Vertical offsets can be compensated for by adjusting the Pitch offset (Septentrio).
+ *
+ * Note that this can be interpreted as the "roll" angle in case the antennas are aligned along the perpendicular axis. This occurs in situations where the two antenna ARPs may not be exactly at the same height in the vehicle reference frame. Since pitch is defined as the right-handed rotation about the vehicle Y axis, a situation where the main antenna is mounted lower than the aux antenna (assuming the default antenna setup) will result in a positive pitch.
+ *
+ *
+ * @min -90
+ * @max 90
+ * @unit deg
+ * @reboot_required true
+ * @decimal 3
+ *
+ * @group GPS
+ */
+PARAM_DEFINE_FLOAT(GPS_PITCH_OFFSET, 0.f);
+
 /**
  * Protocol for Main GPS
  *
@@ -176,7 +194,7 @@ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
  * Auto-detection will probe all protocols, and thus is a bit slower.
  *
  * @min 0
- * @max 6
+ * @max 7
  * @value 0 Auto detect
  * @value 1 u-blox
  * @value 2 MTK
@@ -184,6 +202,7 @@ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
  * @value 4 Emlid Reach
  * @value 5 Femtomes
  * @value 6 NMEA (generic)
+ * @value 7 Septentrio (SBF)
  *
  * @reboot_required true
  * @group GPS