gnss(septentrio): first batch of bugfixes after internal testing

Internal testing revealed usability issues. Those and some other
problems are fixed.

src/drivers/gnss/septentrio/module.h

@@ -56,7 +56,7 @@
 #endif
 
 #ifdef SEP_LOG_ERROR
-#define SEP_ERR(...)            {PX4_WARN(__VA_ARGS__);}
+#define SEP_ERR(...)            {PX4_ERR(__VA_ARGS__);}
 #else
 #define SEP_ERR(...)            {}
 #endif

src/drivers/gnss/septentrio/sbf/messages.h

@@ -60,8 +60,8 @@ constexpr uint32_t k_dnu_u4_value {4294967295};
 constexpr uint32_t k_dnu_u4_bitfield {0};
 constexpr uint16_t k_dnu_u2_value {65535};
 constexpr uint16_t k_dnu_u2_bitfield {0};
-constexpr float k_dnu_f4_value {-2 * 10000000000};
+constexpr float k_dnu_f4_value {-2.0f * 10000000000.0f};
-constexpr double k_dnu_f8_value {-2 * 10000000000};
+constexpr double k_dnu_f8_value {-2.0 * 10000000000.0};
 
 /// Maximum size of all expected messages.
 /// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages.

src/drivers/gnss/septentrio/septentrio.cpp

@@ -55,6 +55,7 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/time.h>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
 
@@ -86,6 +87,11 @@ constexpr int k_max_receiver_read_timeout = 50;
 */
 constexpr size_t k_min_receiver_read_bytes = 32;
 
+/**
+ * The baud rate of Septentrio receivers with factory default configuration.
+*/
+constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200;
+
 constexpr uint8_t k_max_command_size            = 120;
 constexpr uint16_t k_timeout_5hz                = 500;
 constexpr uint32_t k_read_buffer_size           = 150;
@@ -134,11 +140,14 @@ constexpr const char *k_frequency_25_0hz = "msec40";
 constexpr const char *k_frequency_50_0hz = "msec20";
 
 px4::atomic<SeptentrioDriver *> SeptentrioDriver::_secondary_instance {nullptr};
+uint32_t SeptentrioDriver::k_supported_baud_rates[] {0, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1500000};
+uint32_t SeptentrioDriver::k_default_baud_rate {230400};
+orb_advert_t SeptentrioDriver::k_mavlink_log_pub {nullptr};
 
 SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate) :
 	Device(MODULE_NAME),
 	_instance(instance),
-	_baud_rate(baud_rate)
+	_chosen_baud_rate(baud_rate)
 {
 	strncpy(_port, device_path, sizeof(_port) - 1);
 	// Enforce null termination.
@@ -171,6 +180,10 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	get_parameter("SEP_STREAM_LOG", &receiver_stream_log);
 	_receiver_stream_log = receiver_stream_log;
 
+	if (_receiver_stream_log == _receiver_stream_main) {
+		mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Logging stream should be different from main stream");
+	}
+
 	int32_t automatic_configuration {true};
 	get_parameter("SEP_AUTO_CONFIG", &automatic_configuration);
 	_automatic_configuration = static_cast<bool>(automatic_configuration);
@@ -240,15 +253,13 @@ int SeptentrioDriver::print_status()
 		break;
 	}
 
-	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _baud_rate);
+	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate());
 	PX4_INFO("controller -> receiver data rate: %lu B/s", output_data_rate());
 	PX4_INFO("receiver -> controller data rate: %lu B/s", input_data_rate());
 	PX4_INFO("sat info: %s", (_message_satellite_info != nullptr) ? "enabled" : "disabled");
 
-	if (_message_gps_state.timestamp != 0) {
+	if (first_gps_uorb_message_created() && _state == State::ReceivingData) {
-
 		PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast<double>(rtcm_injection_frequency()));
-
 		print_message(ORB_ID(sensor_gps), _message_gps_state);
 	}
 
@@ -267,7 +278,7 @@ void SeptentrioDriver::run()
 				_uart.setPort(_port);
 
 				if (_uart.open()) {
-					_state = State::ConfiguringDevice;
+					_state = State::DetectingBaudRate;
 
 				} else {
 					// Failed to open port, so wait a bit before trying again.
@@ -277,14 +288,42 @@ void SeptentrioDriver::run()
 				break;
 			}
 
+		case State::DetectingBaudRate: {
+				static uint32_t expected_baud_rates[] = {k_septentrio_receiver_default_baud_rate, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000};
+				expected_baud_rates[0] = _chosen_baud_rate != 0 ? _chosen_baud_rate : k_septentrio_receiver_default_baud_rate;
+
+				if (detect_receiver_baud_rate(expected_baud_rates[_current_baud_rate_index], true)) {
+					if (set_baudrate(expected_baud_rates[_current_baud_rate_index]) == PX4_OK) {
+						_state = State::ConfiguringDevice;
+
+					} else {
+						SEP_ERR("Setting local baud rate failed");
+					}
+
+				} else {
+					_current_baud_rate_index++;
+
+					if (_current_baud_rate_index == sizeof(expected_baud_rates) / sizeof(expected_baud_rates[0])) {
+						_current_baud_rate_index = 0;
+					}
+				}
+
+				break;
+			}
+
 		case State::ConfiguringDevice: {
-				if (configure() == PX4_OK) {
+				ConfigureResult result = configure();
-					SEP_INFO("Automatic configuration successful");
+
+				if (!(static_cast<int32_t>(result) & static_cast<int32_t>(ConfigureResult::FailedCompletely))) {
+					if (static_cast<int32_t>(result) & static_cast<int32_t>(ConfigureResult::NoLogging)) {
+						mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Failed to configure receiver internal logging");
+					}
+
+					SEP_INFO("Automatic configuration finished");
 					_state = State::ReceivingData;
 
 				} else {
-					// Failed to configure device, so wait a bit before trying again.
+					_state = State::DetectingBaudRate;
-					px4_usleep(200000);
 				}
 
 				break;
@@ -296,7 +335,7 @@ void SeptentrioDriver::run()
 				receive_result = receive(k_timeout_5hz);
 
 				if (receive_result == -1) {
-					_state = State::ConfiguringDevice;
+					_state = State::DetectingBaudRate;
 				}
 
 				if (_message_satellite_info && (receive_result & 2)) {
@@ -385,19 +424,51 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[])
 
 SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance instance)
 {
-	ModuleArguments arguments{};
+	ModuleArguments arguments {};
-	SeptentrioDriver *gps{nullptr};
+	SeptentrioDriver *gps {nullptr};
 
 	if (parse_cli_arguments(argc, argv, arguments) == PX4_ERROR) {
 		return nullptr;
 	}
 
+	if (arguments.device_path_main && arguments.device_path_secondary
+	    && arguments.device_path_main == arguments.device_path_secondary) {
+		mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Device paths must be different");
+		return nullptr;
+	}
+
+	bool valid_chosen_baud_rate {false};
+
+	for (uint8_t i = 0; i < sizeof(k_supported_baud_rates) / sizeof(k_supported_baud_rates[0]); i++) {
+		switch (instance) {
+		case Instance::Main:
+			if (arguments.baud_rate_main == static_cast<int>(k_supported_baud_rates[i])) {
+				valid_chosen_baud_rate = true;
+			}
+
+			break;
+
+		case Instance::Secondary:
+			if (arguments.baud_rate_secondary == static_cast<int>(k_supported_baud_rates[i])) {
+				valid_chosen_baud_rate = true;
+			}
+
+			break;
+		}
+	}
+
+	if (!valid_chosen_baud_rate) {
+		mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Baud rate %d is unsupported, falling back to default %lu",
+				     instance == Instance::Main ? arguments.baud_rate_main : arguments.baud_rate_secondary, k_default_baud_rate);
+	}
+
 	if (instance == Instance::Main) {
 		if (Serial::validatePort(arguments.device_path_main)) {
-			gps = new SeptentrioDriver(arguments.device_path_main, instance, arguments.baud_rate_main);
+			gps = new SeptentrioDriver(arguments.device_path_main, instance,
+						   valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate);
 
 		} else {
-			PX4_ERR("invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : "");
+			PX4_ERR("Invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : "");
 		}
 
 		if (gps && arguments.device_path_secondary) {
@@ -410,7 +481,8 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance
 
 	} else {
 		if (Serial::validatePort(arguments.device_path_secondary)) {
-			gps = new SeptentrioDriver(arguments.device_path_secondary, instance, arguments.baud_rate_secondary);
+			gps = new SeptentrioDriver(arguments.device_path_secondary, instance,
+						   valid_chosen_baud_rate ? arguments.baud_rate_secondary : k_default_baud_rate);
 
 		} else {
 			PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : "");
@@ -425,6 +497,7 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance
 int SeptentrioDriver::custom_command(int argc, char *argv[])
 {
 	bool handled = false;
+	const char *failure_reason {"unknown command"};
 	SeptentrioDriver *driver_instance;
 
 	if (!is_running()) {
@@ -448,7 +521,7 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 				type = ReceiverResetType::Cold;
 
 			} else {
-				print_usage("incorrect reset type");
+				failure_reason = "unknown reset type";
 			}
 
 			if (type != ReceiverResetType::None) {
@@ -457,11 +530,11 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 			}
 
 		} else {
-			print_usage("incorrect usage of reset command");
+			failure_reason = "incorrect usage of reset command";
 		}
 	}
 
-	return (handled) ? 0 : print_usage("unknown command");
+	return handled ? 0 : print_usage(failure_reason);
 }
 
 int SeptentrioDriver::print_usage(const char *reason)
@@ -473,25 +546,28 @@ int SeptentrioDriver::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-GPS driver module that handles the communication with Septentrio devices and publishes the position via uORB.
+Driver for Septentrio GNSS receivers. It can automatically configure them and make their output available for the rest of the system.
-
+A secondary receiver is supported for redundancy, logging and dual-receiver heading.
-The module supports a secondary GPS device, specified via `-e` parameter. The position will be published on
+Septentrio receiver baud rates from 57600 to 1500000 are supported. If others are used, the driver will use 230400 and give a warning.
-the second uORB topic instance. It can be used for logging and heading computation.
 
 ### Examples
 
-Starting 2 GPS devices (main one on /dev/ttyS3, secondary on /dev/ttyS4)
+Use one receiver on port `/dev/ttyS0` and automatically configure it to use baud rate 230400:
+$ septentrio start -d /dev/ttyS0 -b 230400
+
+Use two receivers, the primary on port `/dev/ttyS3` and the secondary on `/dev/ttyS4`,
+detect baud rate automatically and preserve them:
 $ septentrio start -d /dev/ttyS3 -e /dev/ttyS4
 
-Initiate warm restart of GPS device
+Perform warm reset of the receivers:
 $ gps reset warm
 )DESCR_STR");
 	PRINT_MODULE_USAGE_NAME("septentrio", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary Septentrio receiver", false);
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary receiver port", false);
-	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary baud rate", true);
+	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary receiver baud rate", true);
-	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary Septentrio receiver", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary receiver port", true);
-	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary baud rate", true);
+	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary receiver baud rate", true);
 
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver");
@@ -508,15 +584,15 @@ int SeptentrioDriver::reset(ReceiverResetType type)
 
 	switch (type) {
 	case ReceiverResetType::Hot:
-		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout_fast);
 		break;
 
 	case ReceiverResetType::Warm:
-		res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout_fast);
 		break;
 
 	case ReceiverResetType::Cold:
-		res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout_fast);
 		break;
 
 	default:
@@ -553,13 +629,13 @@ int SeptentrioDriver::parse_cli_arguments(int argc, char *argv[], ModuleArgument
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, arguments.baud_rate_main) != 0) {
-				PX4_ERR("baud rate parsing failed");
+				PX4_ERR("Baud rate parsing failed");
 				return PX4_ERROR;
 			}
 			break;
 		case 'g':
 			if (px4_get_parameter_value(myoptarg, arguments.baud_rate_secondary) != 0) {
-				PX4_ERR("baud rate parsing failed");
+				PX4_ERR("Baud rate parsing failed");
 				return PX4_ERROR;
 			}
 			break;
@@ -637,42 +713,31 @@ void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type)
 	}
 }
 
-uint32_t SeptentrioDriver::detect_receiver_baud_rate(bool forced_input) {
+bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) {
-	// So we can restore the port to its original state.
+	if (set_baudrate(baud_rate) != PX4_OK) {
-	const uint32_t original_baud_rate = _uart.getBaudrate();
+		return false;
-
-	// Baud rates we expect the receiver to be running at.
-	uint32_t expected_baud_rates[] = {115200, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000};
-	if (_baud_rate != 0) {
-		expected_baud_rates[0] = _baud_rate;
 	}
 
-	for (uint i = 0; i < sizeof(expected_baud_rates)/sizeof(expected_baud_rates[0]); i++) {
+	if (forced_input) {
-		if (set_baudrate(expected_baud_rates[i]) != PX4_OK) {
+		force_input();
-			set_baudrate(original_baud_rate);
-			return 0;
-		}
-
-		if (forced_input) {
-			force_input();
-		}
-
-		if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
-			SEP_INFO("Detected baud rate: %lu", expected_baud_rates[i]);
-			set_baudrate(original_baud_rate);
-			return expected_baud_rates[i];
-		}
 	}
 
-	set_baudrate(original_baud_rate);
+	// Make sure that any weird data is "flushed" in the receiver.
-	return 0;
+	(void)send_message("\n");
+
+	if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
+		SEP_INFO("Detected baud rate: %lu", baud_rate);
+		return true;
+	}
+
+	return false;
 }
 
 int SeptentrioDriver::detect_serial_port(char* const port_name) {
 	// Read buffer to get the COM port
 	char buf[k_read_buffer_size];
 	size_t buffer_offset = 0;   // The offset into the string where the next data should be read to.
-	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout;
+	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout_fast;
 	bool response_detected = false;
 
 	// Receiver prints prompt after a message.
@@ -682,7 +747,7 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) {
 
 	do {
 		// Read at most the amount of available bytes in the buffer after the current offset, -1 because we need '\0' at the end for a valid string.
-		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout);
+		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout_fast);
 
 		if (read_result < 0) {
 			SEP_WARN("SBF read error");
@@ -734,132 +799,81 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) {
 	}
 }
 
-int SeptentrioDriver::configure()
+SeptentrioDriver::ConfigureResult SeptentrioDriver::configure()
 {
 	char msg[k_max_command_size] {};
-
-	// Passively detect receiver baud rate.
-	uint32_t detected_receiver_baud_rate = detect_receiver_baud_rate(true);
-
-	if (detected_receiver_baud_rate == 0) {
-		SEP_INFO("CONFIG: failed baud detection");
-		return PX4_ERROR;
-	}
-
-	// Set same baud rate on our end.
-	if (set_baudrate(detected_receiver_baud_rate) != PX4_OK) {
-		SEP_INFO("CONFIG: failed local baud rate setting");
-		return PX4_ERROR;
-	}
-
 	char com_port[5] {};
+	ConfigureResult result {ConfigureResult::OK};
 
 	// Passively detect receiver port.
 	if (detect_serial_port(com_port) != PX4_OK) {
-		SEP_INFO("CONFIG: failed port detection");
+		SEP_WARN("CONFIG: failed port detection");
-		return PX4_ERROR;
+		return ConfigureResult::FailedCompletely;
 	}
 
 	// We should definitely match baud rates and detect used port, but don't do other configuration if not requested.
 	// This will force input on the receiver. That shouldn't be a problem as it's on our own connection.
 	if (!_automatic_configuration) {
-		return PX4_OK;
+		return ConfigureResult::OK;
 	}
 
 	// If user requested specific baud rate, set it now. Otherwise keep detected baud rate.
-	if (strstr(com_port, "COM") != nullptr && _baud_rate != 0) {
+	if (strstr(com_port, "COM") != nullptr && _chosen_baud_rate != 0) {
-		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate);
+		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _chosen_baud_rate);
 
 		if (!send_message(msg)) {
-			SEP_INFO("CONFIG: baud rate command write error");
+			SEP_WARN("CONFIG: baud rate command write error");
-			return PX4_ERROR;
+			return ConfigureResult::FailedCompletely;
 		}
 
 		// When sending a command and setting the baud rate right after, the controller could send the command at the new baud rate.
 		// From testing this could take some time.
-		px4_usleep(1000000);
+		px4_usleep(2000000);
 
-		if (set_baudrate(_baud_rate) != PX4_OK) {
+		if (set_baudrate(_chosen_baud_rate) != PX4_OK) {
-			SEP_INFO("CONFIG: failed local baud rate setting");
+			SEP_WARN("CONFIG: failed local baud rate setting");
-			return PX4_ERROR;
+			return ConfigureResult::FailedCompletely;
 		}
 
-		if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
+		if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
-			SEP_INFO("CONFIG: failed remote baud rate setting");
+			SEP_WARN("CONFIG: failed remote baud rate setting");
-			return PX4_ERROR;
+			return ConfigureResult::FailedCompletely;
 		}
-	} else {
-		_baud_rate = detected_receiver_baud_rate;
 	}
 
 	// Delete all sbf outputs on current COM port to remove clutter data
 	snprintf(msg, sizeof(msg), k_command_clear_sbf, _receiver_stream_main, com_port);
 
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
-		SEP_INFO("CONFIG: failed delete output");
+		SEP_WARN("CONFIG: failed delete output");
-		return PX4_ERROR; // connection and/or baudrate detection failed
+		return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed
 	}
 
-	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
+	// Set up the satellites used in PVT computation.
-	snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
+	if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
-
+		char requested_satellites[40] {};
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
-		return PX4_ERROR;
+			strcat(requested_satellites, "GPS+");
-	}
-
-	// Specify the offsets that the receiver applies to the computed attitude angles.
-	snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset);
-
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	snprintf(msg, sizeof(msg), k_command_set_dynamics, "high");
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	const char *sbf_frequency {k_frequency_10_0hz};
-	switch (_sbf_output_frequency) {
-	case SBFOutputFrequency::Hz5_0:
-		sbf_frequency = k_frequency_5_0hz;
-		break;
-	case SBFOutputFrequency::Hz10_0:
-		sbf_frequency = k_frequency_10_0hz;
-		break;
-	case SBFOutputFrequency::Hz20_0:
-		sbf_frequency = k_frequency_20_0hz;
-		break;
-	case SBFOutputFrequency::Hz25_0:
-		sbf_frequency = k_frequency_25_0hz;
-		break;
-	}
-
-	// Output a set of SBF blocks on a given connection at a regular interval.
-	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		SEP_INFO("Failed to configure SBF");
-		return PX4_ERROR;
-	}
-
-	if (_receiver_setup == ReceiverSetup::MovingBase) {
-		if (_instance == Instance::Secondary) {
-			snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3");
-			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-				SEP_INFO("Failed to configure RTCM output");
-			}
-		} else {
-			snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base);
-			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-				SEP_INFO("Failed to configure attitude source");
-			}
 		}
-	} else {
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
-		snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna);
+			strcat(requested_satellites, "GLONASS+");
-		// This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement.
+		}
-		if (!send_message(msg)) {
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
-			SEP_INFO("Failed to configure attitude source");
+			strcat(requested_satellites, "GALILEO+");
-			return PX4_ERROR;
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::SBAS)) {
+			strcat(requested_satellites, "SBAS+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::BeiDou)) {
+			strcat(requested_satellites, "BEIDOU+");
+		}
+		// Make sure to remove the trailing '+' if any.
+		requested_satellites[math::max(static_cast<int>(strlen(requested_satellites)) - 1, 0)] = '\0';
+		snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites);
+		// Use a longer timeout as the `setSatelliteUsage` command acknowledges a bit slower on mosaic-H-based receivers.
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_slow)) {
+			SEP_WARN("CONFIG: Failed to configure constellation usage");
+			return ConfigureResult::FailedCompletely;
 		}
 	}
 
@@ -919,42 +933,77 @@ int SeptentrioDriver::configure()
 		}
 
 		snprintf(msg, sizeof(msg), k_command_set_sbf_output, _receiver_stream_log, "DSK1", _receiver_logging_overwrite ? "" : "+", level, frequency);
-		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
-			SEP_ERR("Failed to configure logging");
+			result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
-			return PX4_ERROR;
 		}
 	} else if (_receiver_stream_log == _receiver_stream_main) {
-		SEP_WARN("Skipping logging setup: logging stream can't equal main stream");
+		result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
 	}
 
-	// Set up the satellites used in PVT computation.
+	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
-	if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
+	snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
-		char requested_satellites[40] {};
+
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
-			strcat(requested_satellites, "GPS+");
+		return ConfigureResult::FailedCompletely;
+	}
+
+	// Specify the offsets that the receiver applies to the computed attitude angles.
+	snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset);
+
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	snprintf(msg, sizeof(msg), k_command_set_dynamics, "high");
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	const char *sbf_frequency {k_frequency_10_0hz};
+	switch (_sbf_output_frequency) {
+	case SBFOutputFrequency::Hz5_0:
+		sbf_frequency = k_frequency_5_0hz;
+		break;
+	case SBFOutputFrequency::Hz10_0:
+		sbf_frequency = k_frequency_10_0hz;
+		break;
+	case SBFOutputFrequency::Hz20_0:
+		sbf_frequency = k_frequency_20_0hz;
+		break;
+	case SBFOutputFrequency::Hz25_0:
+		sbf_frequency = k_frequency_25_0hz;
+		break;
+	}
+
+	// Output a set of SBF blocks on a given connection at a regular interval.
+	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		SEP_WARN("CONFIG: Failed to configure SBF");
+		return ConfigureResult::FailedCompletely;
+	}
+
+	if (_receiver_setup == ReceiverSetup::MovingBase) {
+		if (_instance == Instance::Secondary) {
+			snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3");
+			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+				SEP_WARN("CONFIG: Failed to configure RTCM output");
+			}
+		} else {
+			snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base);
+			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+				SEP_WARN("CONFIG: Failed to configure attitude source");
+			}
 		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
+	} else {
-			strcat(requested_satellites, "GLONASS+");
+		snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna);
-		}
+		// This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement.
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
+		if (!send_message(msg)) {
-			strcat(requested_satellites, "GALILEO+");
+			SEP_WARN("CONFIG: Failed to configure attitude source");
-		}
+			return ConfigureResult::FailedCompletely;
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::SBAS)) {
-			strcat(requested_satellites, "SBAS+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::BeiDou)) {
-			strcat(requested_satellites, "BEIDOU+");
-		}
-		// Make sure to remove the trailing '+' if any.
-		requested_satellites[math::max(static_cast<int>(strlen(requested_satellites)) - 1, 0)] = '\0';
-		snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites);
-		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-			SEP_ERR("Failed to configure constellation usage");
-			return PX4_ERROR;
 		}
 	}
 
-	return PX4_OK;
+	return result;
 }
 
 int SeptentrioDriver::parse_char(const uint8_t byte)
@@ -1035,36 +1084,37 @@ int SeptentrioDriver::process_message()
 			PVTGeodetic pvt_geodetic;
 
 			if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) {
-				if (pvt_geodetic.mode_type < ModeType::StandAlonePVT) {
+				switch (pvt_geodetic.mode_type) {
+				case ModeType::NoPVT:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
-
+					break;
-				} else if (pvt_geodetic.mode_type == ModeType::PVTWithSBAS) {
+				case ModeType::PVTWithSBAS:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL;
-
+					break;
-				} else if (pvt_geodetic.mode_type == ModeType::RTKFloat || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFloat) {
+				case ModeType::RTKFloat:
+				case ModeType::MovingBaseRTKFloat:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT;
-
+					break;
-				} else if (pvt_geodetic.mode_type == ModeType::RTKFixed || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFixed) {
+				case ModeType::RTKFixed:
+				case ModeType::MovingBaseRTKFixed:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED;
-
+					break;
-				} else {
+				default:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D;
+					break;
 				}
 
-				// Check fix and error code
-				_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
-
 				// Check boundaries and invalidate GPS velocities
 				if (pvt_geodetic.vn <= k_dnu_f4_value || pvt_geodetic.ve <= k_dnu_f4_value || pvt_geodetic.vu <= k_dnu_f4_value) {
 					_message_gps_state.vel_ned_valid = false;
 				}
 
-				// Check boundaries and invalidate position
+				if (pvt_geodetic.latitude > k_dnu_f8_value && pvt_geodetic.longitude > k_dnu_f8_value && pvt_geodetic.height > k_dnu_f8_value && pvt_geodetic.undulation > k_dnu_f4_value) {
-				// We're not just checking for the do-not-use value but for any value beyond the specified max values
+					_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
-				if (pvt_geodetic.latitude <= k_dnu_f8_value ||
+					_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
-				    pvt_geodetic.longitude <= k_dnu_f8_value ||
+					_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
-				    pvt_geodetic.height <= k_dnu_f8_value ||
+					_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
-				    pvt_geodetic.undulation <= k_dnu_f4_value) {
+				} else {
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
 				}
 
@@ -1082,23 +1132,22 @@ int SeptentrioDriver::process_message()
 					_message_gps_state.satellites_used = 0;
 				}
 
-				_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
-				_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
-				_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
-				_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
-
 				/* H and V accuracy are reported in 2DRMS, but based off the u-blox reporting we expect RMS.
 				 * Divide by 100 from cm to m and in addition divide by 2 to get RMS. */
 				_message_gps_state.eph = static_cast<float>(pvt_geodetic.h_accuracy) / 200.0f;
 				_message_gps_state.epv = static_cast<float>(pvt_geodetic.v_accuracy) / 200.0f;
 
+				// Check fix and error code
+				_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
 				_message_gps_state.vel_n_m_s = pvt_geodetic.vn;
 				_message_gps_state.vel_e_m_s = pvt_geodetic.ve;
 				_message_gps_state.vel_d_m_s = -1.0f * pvt_geodetic.vu;
 				_message_gps_state.vel_m_s = sqrtf(_message_gps_state.vel_n_m_s * _message_gps_state.vel_n_m_s +
 							_message_gps_state.vel_e_m_s * _message_gps_state.vel_e_m_s);
 
-				_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
+				if (pvt_geodetic.cog > k_dnu_f4_value) {
+					_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
+				}
 				_message_gps_state.c_variance_rad = M_DEG_TO_RAD_F;
 
 				_message_gps_state.time_utc_usec = 0;
@@ -1178,14 +1227,8 @@ int SeptentrioDriver::process_message()
 			VelCovGeodetic vel_cov_geodetic;
 
 			if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) {
-				_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_ve_ve;
+				if (vel_cov_geodetic.cov_ve_ve > k_dnu_f4_value && vel_cov_geodetic.cov_vn_vn > k_dnu_f4_value && vel_cov_geodetic.cov_vu_vu > k_dnu_f4_value) {
-
+					_message_gps_state.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu);
-				if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vn_vn) {
-					_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vn_vn;
-				}
-
-				if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vu_vu) {
-					_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vu_vu;
 				}
 			}
 
@@ -1206,7 +1249,8 @@ int SeptentrioDriver::process_message()
 			if (_sbf_decoder.parse(&att_euler) &&
 			    !att_euler.error_not_requested &&
 			    att_euler.error_aux1 == Error::None &&
-			    att_euler.error_aux2 == Error::None) {
+			    att_euler.error_aux2 == Error::None &&
+			    att_euler.heading > k_dnu_f4_value) {
 				float heading = att_euler.heading * M_PI_F / 180.0f; // Range of degrees to range of radians in [0, 2PI].
 
 				// Ensure range is in [-PI, PI].
@@ -1230,7 +1274,8 @@ int SeptentrioDriver::process_message()
 			if (_sbf_decoder.parse(&att_cov_euler) == PX4_OK &&
 			    !att_cov_euler.error_not_requested &&
 			    att_cov_euler.error_aux1 == Error::None &&
-			    att_cov_euler.error_aux2 == Error::None) {
+			    att_cov_euler.error_aux2 == Error::None &&
+			    att_cov_euler.cov_headhead > k_dnu_f4_value) {
 				_message_gps_state.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI]
 			}
 
@@ -1289,13 +1334,16 @@ bool SeptentrioDriver::send_message_and_wait_for_ack(const char *msg, const int
 				return true;
 			} else if (expected_response[response_check_character] == buf[i]) {
 				++response_check_character;
+			} else if (buf[i] == '$') {
+				// Special case makes sure we don't miss start of new response if that happened to be the character we weren't expecting next (e.g., `$R: ge$R: gecm`)
+				response_check_character = 1;
 			} else {
 				response_check_character = 0;
 			}
 		}
 	} while (timeout_time > hrt_absolute_time());
 
-	PX4_DEBUG("Response: timeout");
+	SEP_WARN("Response: timeout");
 	return false;
 }
 
@@ -1523,6 +1571,11 @@ void SeptentrioDriver::publish_satellite_info()
 	}
 }
 
+bool SeptentrioDriver::first_gps_uorb_message_created() const
+{
+	return _message_gps_state.timestamp != 0;
+}
+
 void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len)
 {
 	gps_inject_data_s gps_inject_data{};
@@ -1668,13 +1721,11 @@ void SeptentrioDriver::set_clock(timespec rtc_gps_time)
 
 bool SeptentrioDriver::is_healthy() const
 {
-	if (_state == State::ReceivingData) {
+	if (_state == State::ReceivingData && receiver_configuration_healthy()) {
-		if (!receiver_configuration_healthy()) {
+		return true;
-			return false;
-		}
 	}
 
-	return true;
+	return false;
 }
 
 void SeptentrioDriver::reset_gps_state_message()

src/drivers/gnss/septentrio/septentrio.h

@@ -47,6 +47,7 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/Serial.hpp>
+#include <uORB/uORB.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
@@ -271,9 +272,20 @@ public:
 	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise
 	 */
 	int force_input();
+
+	/**
+	 * Standard baud rates the driver can be started with. `0` means the driver matches baud rates but does not change them.
+	 */
+	static uint32_t k_supported_baud_rates[];
+
+	/**
+	 * Default baud rate, used when the user requested an invalid baud rate.
+	 */
+	static uint32_t k_default_baud_rate;
 private:
 	enum class State {
 		OpeningSerialPort,
+		DetectingBaudRate,
 		ConfiguringDevice,
 		ReceivingData,
 	};
@@ -295,9 +307,24 @@ private:
 	};
 
 	/**
-	 * Maximum timeout to wait for command acknowledgement by the receiver.
+	 * The result of trying to configure the receiver.
+	 */
+	enum class ConfigureResult : int32_t {
+		OK               = 0,
+		FailedCompletely = 1 << 0,
+		NoLogging        = 1 << 1,
+	};
+
+	/**
+	 * Maximum timeout to wait for fast command acknowledgement by the receiver.
 	*/
-	static constexpr uint16_t k_receiver_ack_timeout = 200;
+	static constexpr uint16_t k_receiver_ack_timeout_fast = 200;
+
+	/**
+	 * Maximum timeout to wait for slow command acknowledgement by the receiver.
+	 * Might be the case for commands that send more output back as reply.
+	 */
+	static constexpr uint16_t k_receiver_ack_timeout_slow = 400;
 
 	/**
 	 * Duration of one update monitoring interval in us.
@@ -306,6 +333,11 @@ private:
 	*/
 	static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s;
 
+	/**
+	 * uORB type to send messages to ground control stations.
+	 */
+	static orb_advert_t k_mavlink_log_pub;
+
 	/**
 	 * The default stream for output of PVT data.
 	*/
@@ -347,13 +379,15 @@ private:
 	void schedule_reset(ReceiverResetType type);
 
 	/**
-	 * @brief Detect the current baud rate used by the receiver on the connected port.
+	 * @brief Detect whether the receiver is running at the given baud rate.
+	 * Does not preserve local baud rate!
 	 *
-	 * @param force_input Choose whether the receiver forces input on the port
+	 * @param baud_rate The baud rate to check.
+	 * @param force_input Choose whether the receiver forces input on the port.
 	 *
-	 * @return The detected baud rate on success, or `0` on error
+	 * @return `true` if running at the baud rate, or `false` on error.
 	 */
-	uint32_t detect_receiver_baud_rate(bool forced_input);
+	bool detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input);
 
 	/**
 	 * @brief Try to detect the serial port used on the receiver side.
@@ -369,9 +403,9 @@ private:
 	 *
 	 * If the user has disabled automatic configuration, only execute the steps that do not touch the receiver (e.g., baud rate detection, port detection...).
 	 *
-	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise.
+	 * @return `ConfigureResult::OK` if configured, or error.
 	 */
-	int configure();
+	ConfigureResult configure();
 
 	/**
 	 * @brief Parse the next byte of a received message from the receiver.
@@ -505,6 +539,13 @@ private:
 	 */
 	void publish_satellite_info();
 
+	/**
+	 * @brief Check whether the driver has created its first complete `SensorGPS` uORB message.
+	 *
+	 * @return `true` if the driver has created its first complete `SensorGPS` uORB message, `false` if still waiting.
+	 */
+	bool first_gps_uorb_message_created() const;
+
 	/**
 	 * @brief Publish RTCM corrections.
 	 *
@@ -579,6 +620,9 @@ private:
 	/**
 	 * @brief Check whether the current receiver configuration is likely healthy.
 	 *
+	 * This is checked by passively monitoring output from the receiver and validating whether it is what is
+	 * expected.
+	 *
 	 * @return `true` if the receiver is operating correctly, `false` if it needs to be reconfigured.
 	*/
 	bool receiver_configuration_healthy() const;
@@ -611,6 +655,9 @@ private:
 	/**
 	 * @brief Check whether the driver is operating correctly.
 	 *
+	 * The driver is operating correctly when it has fully configured the receiver and is actively receiving all the
+	 * expected data.
+	 *
 	 * @return `true` if the driver is working as expected, `false` otherwise.
 	*/
 	bool is_healthy() const;
@@ -666,7 +713,7 @@ private:
 	uint8_t                                _spoofing_state {0};                                          ///< Receiver spoofing state
 	uint8_t                                _jamming_state {0};                                           ///< Receiver jamming state
 	const Instance                         _instance {Instance::Main};                                   ///< The receiver that this instance of the driver controls
-	uint32_t                               _baud_rate {0};
+	uint32_t                               _chosen_baud_rate {0};                                        ///< The baud rate requested by the user
 	static px4::atomic<SeptentrioDriver *> _secondary_instance;
 	hrt_abstime                            _sleep_end {0};                                               ///< End time for sleeping
 	State                                  _resume_state {State::OpeningSerialPort};                     ///< Resume state after sleep
@@ -683,6 +730,7 @@ private:
 	bool                                   _automatic_configuration {true};                              ///< Automatic configuration of the receiver given by the `SEP_AUTO_CONFIG` parameter
 	ReceiverSetup                          _receiver_setup {ReceiverSetup::Default};                     ///< Purpose of the receivers, given by the `SEP_HARDW_SETUP` parameter
 	int32_t                                _receiver_constellation_usage {0};                            ///< Constellation usage in PVT computation given by the `SEP_CONST_USAGE` parameter
+	uint8_t                                _current_baud_rate_index {0};                                 ///< Index of the current baud rate to check
 
 	// Decoding and parsing
 	DecodingStatus                                 _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into