fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 12:49:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Internal testing revealed usability issues. Those and some other
problems are fixed.
This commit is contained in:
Thomas Frans 2024-06-12 14:41:08 +02:00 committed by Ramon Roche
parent 6666c98d02
commit 24fdb3c359
No known key found for this signature in database
GPG Key ID: 275988FAE5821713
4 changed files with 334 additions and 235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/gnss/septentrio/module.h
View File

@ -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

4
src/drivers/gnss/septentrio/sbf/messages.h
View File

@ -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 double k_dnu_f8_value {-2 * 10000000000};
constexpr float k_dnu_f4_value {-2.0f * 10000000000.0f};
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.

497
src/drivers/gnss/septentrio/septentrio.cpp
View File

@ -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) {
SEP_INFO("Automatic configuration successful");
ConfigureResult result = configure();
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.
px4_usleep(200000);
_state = State::DetectingBaudRate;
}
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{};
SeptentrioDriver *gps{nullptr};
ModuleArguments arguments {};
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.
The module supports a secondary GPS device, specified via `-e` parameter. The position will be published on
the second uORB topic instance. It can be used for logging and heading computation.
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.
Septentrio receiver baud rates from 57600 to 1500000 are supported. If others are used, the driver will use 230400 and give a warning.
### 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_INT('b', 0, 57600, 1500000, "Primary baud rate", true);
PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary Septentrio receiver", true);
PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary baud rate", true);
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary receiver port", false);
PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary receiver baud rate", true);
PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary receiver port", 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) {
// So we can restore the port to its original state.
const uint32_t original_baud_rate = _uart.getBaudrate();
// 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;
bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) {
if (set_baudrate(baud_rate) != PX4_OK) {
return false;
}
for (uint i = 0; i < sizeof(expected_baud_rates)/sizeof(expected_baud_rates[0]); i++) {
if (set_baudrate(expected_baud_rates[i]) != PX4_OK) {
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];
}
if (forced_input) {
force_input();
}
set_baudrate(original_baud_rate);
return 0;
// Make sure that any weird data is "flushed" in the receiver.
(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");
return PX4_ERROR;
SEP_WARN("CONFIG: failed port detection");
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) {
snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate);
if (strstr(com_port, "COM") != nullptr && _chosen_baud_rate != 0) {
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");
return PX4_ERROR;
SEP_WARN("CONFIG: baud rate command write 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) {
SEP_INFO("CONFIG: failed local baud rate setting");
return PX4_ERROR;
if (set_baudrate(_chosen_baud_rate) != PX4_OK) {
SEP_WARN("CONFIG: failed local baud rate setting");
return ConfigureResult::FailedCompletely;
}
if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
SEP_INFO("CONFIG: failed remote baud rate setting");
return PX4_ERROR;
if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
SEP_WARN("CONFIG: failed remote baud rate setting");
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)) {
SEP_INFO("CONFIG: failed delete output");
return PX4_ERROR; // connection and/or baudrate detection failed
if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
SEP_WARN("CONFIG: failed delete output");
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
snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
return PX4_ERROR;
}
// 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");
}
// Set up the satellites used in PVT computation.
if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
char requested_satellites[40] {};
if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
strcat(requested_satellites, "GPS+");
}
} else {
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 (!send_message(msg)) {
SEP_INFO("Failed to configure attitude source");
return PX4_ERROR;
if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
strcat(requested_satellites, "GLONASS+");
}
if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
strcat(requested_satellites, "GALILEO+");
}
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)) {
SEP_ERR("Failed to configure logging");
return PX4_ERROR;
if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
}
} 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.
if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
char requested_satellites[40] {};
if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
strcat(requested_satellites, "GPS+");
// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
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)) {
strcat(requested_satellites, "GLONASS+");
}
if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
strcat(requested_satellites, "GALILEO+");
}
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;
} else {
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 (!send_message(msg)) {
SEP_WARN("CONFIG: Failed to configure attitude source");
return ConfigureResult::FailedCompletely;
}
}
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;
} else if (pvt_geodetic.mode_type == ModeType::PVTWithSBAS) {
break;
case ModeType::PVTWithSBAS:
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL;
} else if (pvt_geodetic.mode_type == ModeType::RTKFloat || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFloat) {
break;
case ModeType::RTKFloat:
case ModeType::MovingBaseRTKFloat:
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT;
} else if (pvt_geodetic.mode_type == ModeType::RTKFixed || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFixed) {
break;
case ModeType::RTKFixed:
case ModeType::MovingBaseRTKFixed:
_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED;
} else {
break;
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
// We're not just checking for the do-not-use value but for any value beyond the specified max values
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) {
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) {
_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;
} 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 (_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;
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);
}
}
@ -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 (!receiver_configuration_healthy()) {
return false;
}
if (_state == State::ReceivingData && receiver_configuration_healthy()) {
return true;
}
return true;
return false;
}
void SeptentrioDriver::reset_gps_state_message()

66
src/drivers/gnss/septentrio/septentrio.h
View File

@ -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