Add option to configure the set of used GNSS systems

Functionality not implemented yet, see issue PX4#68 for details
jbeyerstedt · Jan 11, 2021 · 075c14c · 075c14c
1 parent 9282d3d
commit 075c14c
Show file tree

Hide file tree

Showing 13 changed files with 59 additions and 35 deletions.
diff --git a/src/ashtech.cpp b/src/ashtech.cpp
@@ -942,9 +942,9 @@ int GPSDriverAshtech::waitForReply(NMEACommand command, const unsigned timeout)
 	return _command_state == NMEACommandState::received ? 0 : -1;
 }
 
-int GPSDriverAshtech::configure(unsigned &baudrate, OutputMode output_mode)
+int GPSDriverAshtech::configure(unsigned &baudrate, const GPSConfig &config)
 {
-	_output_mode = output_mode;
+	_output_mode = config.output_mode;
 	_correction_output_activated = false;
 	_configure_done = false;
 
@@ -1069,7 +1069,7 @@ int GPSDriverAshtech::configure(unsigned &baudrate, OutputMode output_mode)
 	// Enable dual antenna mode (2: both antennas are L1/L2 GNSS capable, flex mode, avoids the need to determine
 	// the baseline length through a prior calibration stage)
 	// Needs to be set before other commands
-	const bool use_dual_mode = output_mode == OutputMode::GPS && _board == AshtechBoard::trimble_mb_two;
+	const bool use_dual_mode = _output_mode == OutputMode::GPS && _board == AshtechBoard::trimble_mb_two;
 
 	if (use_dual_mode) {
 		ASH_DEBUG("Enabling DUO mode");
@@ -1118,7 +1118,7 @@ int GPSDriverAshtech::configure(unsigned &baudrate, OutputMode output_mode)
 	}
 
 
-	if (output_mode == OutputMode::RTCM && _board == AshtechBoard::trimble_mb_two) {
+	if (_output_mode == OutputMode::RTCM && _board == AshtechBoard::trimble_mb_two) {
 		SurveyInStatus status{};
 		status.latitude = status.longitude = (double)NAN;
 		status.altitude = NAN;

diff --git a/src/ashtech.h b/src/ashtech.h
@@ -59,7 +59,7 @@ class GPSDriverAshtech : public GPSBaseStationSupport
 
 	virtual ~GPSDriverAshtech();
 
-	int configure(unsigned &baudrate, OutputMode output_mode) override;
+	int configure(unsigned &baudrate, const GPSConfig &config) override;
 
 	int receive(unsigned timeout) override;
 

diff --git a/src/emlid_reach.cpp b/src/emlid_reach.cpp
@@ -78,11 +78,11 @@ GPSDriverEmlidReach::GPSDriverEmlidReach(GPSCallbackPtr callback, void *callback
 
 
 int
-GPSDriverEmlidReach::configure(unsigned &baudrate, OutputMode output_mode)
+GPSDriverEmlidReach::configure(unsigned &baudrate, const GPSConfig &config)
 {
 	// TODO RTK
-	if (output_mode != OutputMode::GPS) {
-		GPS_WARN("EMLIDREACH: Unsupported Output Mode %i", (int)output_mode);
+	if (config.output_mode != OutputMode::GPS) {
+		GPS_WARN("EMLIDREACH: Unsupported Output Mode %i", (int)config.output_mode);
 		return -1;
 	}
 

diff --git a/src/emlid_reach.h b/src/emlid_reach.h
@@ -147,7 +147,7 @@ class GPSDriverEmlidReach : public GPSHelper
 	virtual ~GPSDriverEmlidReach() = default;
 
 	int receive(unsigned timeout) override;
-	int configure(unsigned &baudrate, OutputMode output_mode) override;
+	int configure(unsigned &baudrate, const GPSConfig &config) override;
 
 private:
 

diff --git a/src/femtomes.cpp b/src/femtomes.cpp
@@ -322,11 +322,11 @@ int GPSDriverFemto::writeAckedCommandFemto(const char *command, const char *repl
 	return -1;
 }
 
-int GPSDriverFemto::configure(unsigned &baudrate, OutputMode output_mode)
+int GPSDriverFemto::configure(unsigned &baudrate, const GPSConfig &config)
 {
 
-	if (output_mode != OutputMode::GPS) {
-		FEMTO_DEBUG("Femto: Unsupported Output Mode %i", (int)output_mode);
+	if (config.output_mode != OutputMode::GPS) {
+		FEMTO_DEBUG("Femto: Unsupported Output Mode %i", (int)config.output_mode);
 		return -1;
 	}
 

diff --git a/src/femtomes.h b/src/femtomes.h
@@ -148,7 +148,7 @@ class GPSDriverFemto : public GPSHelper
 	virtual ~GPSDriverFemto() = default;
 
 	int receive(unsigned timeout) override;
-	int configure(unsigned &baudrate, OutputMode output_mode) override;
+	int configure(unsigned &baudrate, const GPSConfig &config) override;
 
 private:
 

diff --git a/src/gps_helper.h b/src/gps_helper.h
@@ -161,6 +161,24 @@ class GPSHelper
 		SPI
 	};
 
+	/**
+	 * Bitmask for GPS_1_GNSS and GPS_2_GNSS
+	 * No bits set should keep the receiver's default config
+	 */
+	enum class GNSSSystemsMask : int32_t {
+		RECEIVER_DEFAULTS = 0,
+		ENABLE_GPS =        1 << 0,
+		ENABLE_SBAS =       1 << 1,
+		ENABLE_GALILEO =    1 << 2,
+		ENABLE_BEIDOU =     1 << 3,
+		ENABLE_GLONASS =    1 << 4
+	};
+
+	struct GPSConfig {
+		OutputMode output_mode;
+		GNSSSystemsMask gnss_systems;
+	};
+
 
 	GPSHelper(GPSCallbackPtr callback, void *callback_user);
 	virtual ~GPSHelper() = default;
@@ -169,9 +187,10 @@ class GPSHelper
 	 * configure the device
 	 * @param baud Input and output parameter: if set to 0, the baudrate will be automatically detected and set to
 	 *             the detected baudrate. If not 0, a fixed baudrate is used.
+	 * @param config GPS Config
 	 * @return 0 on success, <0 otherwise
 	 */
-	virtual int configure(unsigned &baud, OutputMode output_mode) = 0;
+	virtual int configure(unsigned &baud, const GPSConfig &config) = 0;
 
 	/**
 	 * receive & handle new data from the device
@@ -278,3 +297,8 @@ class GPSHelper
 
 	uint64_t _interval_rate_start{0};
 };
+
+inline bool operator&(GPSHelper::GNSSSystemsMask a, GPSHelper::GNSSSystemsMask b)
+{
+	return static_cast<int32_t>(a) & static_cast<int32_t>(b);
+}
diff --git a/src/mtk.cpp b/src/mtk.cpp
@@ -55,10 +55,10 @@ GPSDriverMTK::GPSDriverMTK(GPSCallbackPtr callback, void *callback_user, sensor_
 }
 
 int
-GPSDriverMTK::configure(unsigned &baudrate, OutputMode output_mode)
+GPSDriverMTK::configure(unsigned &baudrate, const GPSConfig &config)
 {
-	if (output_mode != OutputMode::GPS) {
-		GPS_WARN("MTK: Unsupported Output Mode %i", (int)output_mode);
+	if (config.output_mode != OutputMode::GPS) {
+		GPS_WARN("MTK: Unsupported Output Mode %i", (int)config.output_mode);
 		return -1;
 	}
 

diff --git a/src/mtk.h b/src/mtk.h
@@ -92,7 +92,7 @@ class GPSDriverMTK : public GPSHelper
 	virtual ~GPSDriverMTK() = default;
 
 	int receive(unsigned timeout) override;
-	int configure(unsigned &baudrate, OutputMode output_mode) override;
+	int configure(unsigned &baudrate, const GPSConfig &config) override;
 
 private:
 	/**

diff --git a/src/sbf.cpp b/src/sbf.cpp
@@ -73,16 +73,16 @@ GPSDriverSBF::~GPSDriverSBF()
 }
 
 int
-GPSDriverSBF::configure(unsigned &baudrate, OutputMode output_mode)
+GPSDriverSBF::configure(unsigned &baudrate, const GPSConfig &config)
 {
 	_configured = false;
 
 	setBaudrate(SBF_TX_CFG_PRT_BAUDRATE);
 	baudrate = SBF_TX_CFG_PRT_BAUDRATE;
 
-	_output_mode = output_mode;
+	_output_mode = config.output_mode;
 
-	if (output_mode != OutputMode::RTCM) {
+	if (_output_mode != OutputMode::RTCM) {
 		sendMessage(SBF_CONFIG_FORCE_INPUT);
 	}
 

diff --git a/src/sbf.h b/src/sbf.h
@@ -316,7 +316,7 @@ class GPSDriverSBF : public GPSBaseStationSupport
 	virtual ~GPSDriverSBF() override;
 
 	int receive(unsigned timeout) override;
-	int configure(unsigned &baudrate, OutputMode output_mode) override;
+	int configure(unsigned &baudrate, const GPSConfig &config) override;
 	int reset(GPSRestartType restart_type) override;
 
 private:

diff --git a/src/ubx.cpp b/src/ubx.cpp
@@ -89,18 +89,18 @@ GPSDriverUBX::~GPSDriverUBX()
 }
 
 int
-GPSDriverUBX::configure(unsigned &baudrate, OutputMode output_mode)
+GPSDriverUBX::configure(unsigned &baudrate, const GPSConfig &config)
 {
 	_configured = false;
-	_output_mode = output_mode;
+	_output_mode = config.output_mode;
 
 	ubx_payload_tx_cfg_prt_t cfg_prt[2];
 
-	uint16_t out_proto_mask = output_mode == OutputMode::GPS ?
+	uint16_t out_proto_mask = _output_mode == OutputMode::GPS ?
 				  UBX_TX_CFG_PRT_OUTPROTOMASK_GPS :
 				  UBX_TX_CFG_PRT_OUTPROTOMASK_RTCM;
 
-	uint16_t in_proto_mask = output_mode == OutputMode::GPS ?
+	uint16_t in_proto_mask = _output_mode == OutputMode::GPS ?
 				 UBX_TX_CFG_PRT_INPROTOMASK_GPS :
 				 UBX_TX_CFG_PRT_INPROTOMASK_RTCM;
 
@@ -137,12 +137,12 @@ GPSDriverUBX::configure(unsigned &baudrate, OutputMode output_mode)
 			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1_DATABITS, 0, cfg_valset_msg_size);
 			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1_PARITY, 0, cfg_valset_msg_size);
 			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_UBX, 1, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, output_mode == OutputMode::GPS ? 1 : 0,
+			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_RTCM3X, _output_mode == OutputMode::GPS ? 1 : 0,
 					   cfg_valset_msg_size);
 			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1INPROT_NMEA, 0, cfg_valset_msg_size);
 			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_UBX, 1, cfg_valset_msg_size);
 
-			if (output_mode == OutputMode::RTCM) {
+			if (_output_mode == OutputMode::RTCM) {
 				cfgValset<uint8_t>(UBX_CFG_KEY_CFG_UART1OUTPROT_RTCM3X, 1, cfg_valset_msg_size);
 			}
 
@@ -151,12 +151,12 @@ GPSDriverUBX::configure(unsigned &baudrate, OutputMode output_mode)
 
 			// USB
 			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_UBX, 1, cfg_valset_msg_size);
-			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_RTCM3X, output_mode == OutputMode::GPS ? 1 : 0,
+			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_RTCM3X, _output_mode == OutputMode::GPS ? 1 : 0,
 					   cfg_valset_msg_size);
 			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBINPROT_NMEA, 0, cfg_valset_msg_size);
 			cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBOUTPROT_UBX, 1, cfg_valset_msg_size);
 
-			if (output_mode == OutputMode::RTCM) {
+			if (_output_mode == OutputMode::RTCM) {
 				cfgValset<uint8_t>(UBX_CFG_KEY_CFG_USBOUTPROT_RTCM3X, 1, cfg_valset_msg_size);
 			}
 
@@ -252,10 +252,10 @@ GPSDriverUBX::configure(unsigned &baudrate, OutputMode output_mode)
 		cfgValset<uint8_t>(UBX_CFG_KEY_SPI_ENABLED, 1, cfg_valset_msg_size);
 		cfgValset<uint8_t>(UBX_CFG_KEY_SPI_MAXFF, 1, cfg_valset_msg_size);
 		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_RTCM3X, output_mode == OutputMode::GPS ? 1 : 0, cfg_valset_msg_size);
+		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_RTCM3X, _output_mode == OutputMode::GPS ? 1 : 0, cfg_valset_msg_size);
 		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIINPROT_NMEA, 0, cfg_valset_msg_size);
 		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_UBX, 1, cfg_valset_msg_size);
-		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_RTCM3X, output_mode == OutputMode::GPS ? 0 : 1, cfg_valset_msg_size);
+		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_RTCM3X, _output_mode == OutputMode::GPS ? 0 : 1, cfg_valset_msg_size);
 		cfgValset<uint8_t>(UBX_CFG_KEY_CFG_SPIOUTPROT_NMEA, 0, cfg_valset_msg_size);
 
 		bool cfg_valset_success = false;
@@ -321,7 +321,7 @@ GPSDriverUBX::configure(unsigned &baudrate, OutputMode output_mode)
 	}
 
 
-	if (output_mode != OutputMode::GPS) {
+	if (_output_mode != OutputMode::GPS) {
 		// RTCM mode force stationary dynamic model
 		_dyn_model = 2;
 	}
@@ -339,7 +339,7 @@ GPSDriverUBX::configure(unsigned &baudrate, OutputMode output_mode)
 		return ret;
 	}
 
-	if (output_mode == OutputMode::RTCM) {
+	if (_output_mode == OutputMode::RTCM) {
 		if (restartSurveyIn() < 0) {
 			return -1;
 		}

diff --git a/src/ubx.h b/src/ubx.h
@@ -826,7 +826,7 @@ class GPSDriverUBX : public GPSBaseStationSupport
 
 	virtual ~GPSDriverUBX();
 
-	int configure(unsigned &baudrate, OutputMode output_mode) override;
+	int configure(unsigned &baudrate, const GPSConfig &config) override;
 	int receive(unsigned timeout) override;
 	int reset(GPSRestartType restart_type) override;