From 9afb45da83fb8e8b462cdd067106921e56556a34 Mon Sep 17 00:00:00 2001 From: Thomas Frans Date: Mon, 6 May 2024 13:59:53 +0200 Subject: [PATCH] septentrio: add dual-receiver support and fix reset command Add support for two receivers which was also present in the original GPS drivers. Also fix the reset command which didn't work. --- src/drivers/gnss/septentrio/module.yaml | 9 +- src/drivers/gnss/septentrio/septentrio.cpp | 169 ++++++++++++++++++--- src/drivers/gnss/septentrio/septentrio.h | 27 +++- 3 files changed, 177 insertions(+), 28 deletions(-) diff --git a/src/drivers/gnss/septentrio/module.yaml b/src/drivers/gnss/septentrio/module.yaml index 14b38694de7f..32cb8db5110a 100644 --- a/src/drivers/gnss/septentrio/module.yaml +++ b/src/drivers/gnss/septentrio/module.yaml @@ -1,9 +1,14 @@ module_name: Septentrio serial_config: - - command: septentrio start -d ${SERIAL_DEV} + - command: set DUAL_GPS_ARGS "-e ${SERIAL_DEV}" port_config_param: - name: SEP_PORT_CONFIG + name: SEP_PORT2_CFG + group: Septentrio GNSS Receiver + label: Secondary GPS port + - command: septentrio start -d ${SERIAL_DEV} ${DUAL_GPS_ARGS} + port_config_param: + name: SEP_PORT1_CFG group: Septentrio GNSS Receiver default: GPS1 label: GPS Port diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index 073bc5751065..3d20e1412e79 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -87,14 +87,11 @@ using namespace time_literals; // Commands #define SBF_FORCE_INPUT "SSSSSSSSSS\n" /**< Force input on the connected port */ -#define SBF_CONFIG_RESET_HOT "" \ - SBF_FORCE_INPUT"ExeResetReceiver, soft, none\n" +#define SBF_CONFIG_RESET_HOT "ExeResetReceiver, soft, none\n" -#define SBF_CONFIG_RESET_WARM "" \ - SBF_FORCE_INPUT"ExeResetReceiver, soft, PVTData\n" +#define SBF_CONFIG_RESET_WARM "ExeResetReceiver, soft, PVTData\n" -#define SBF_CONFIG_RESET_COLD "" \ - SBF_FORCE_INPUT"ExeResetReceiver, hard, SatData\n" +#define SBF_CONFIG_RESET_COLD "ExeResetReceiver, hard, SatData\n" #define SBF_CONFIG "setSBFOutput, Stream1, %s, PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler, msec100\n" /**< Configure the correct blocks for GPS positioning and heading */ @@ -110,8 +107,11 @@ using namespace time_literals; static constexpr int SEP_SET_CLOCK_DRIFT_TIME_S{5}; ///< RTC drift time when time synchronization is needed (in seconds) -SeptentrioGPS::SeptentrioGPS(const char *device_path) : - Device(MODULE_NAME) +px4::atomic SeptentrioGPS::_secondary_instance{nullptr}; + +SeptentrioGPS::SeptentrioGPS(const char *device_path, SeptentrioInstance instance) : + Device(MODULE_NAME), + _instance(instance) { strncpy(_port, device_path, sizeof(_port) - 1); // Enforce null termination. @@ -133,6 +133,20 @@ SeptentrioGPS::SeptentrioGPS(const char *device_path) : SeptentrioGPS::~SeptentrioGPS() { + SeptentrioGPS *secondary_instance = _secondary_instance.load(); + + if (_instance == SeptentrioInstance::Main && secondary_instance) { + secondary_instance->request_stop(); + + // Wait for exit + uint32_t i = 0; + + do { + px4_usleep(20000); + ++i; + } while (_secondary_instance.load() && i < 100); + } + delete _dump_from_device; delete _dump_to_device; delete _rtcm_parsing; @@ -141,6 +155,21 @@ SeptentrioGPS::~SeptentrioGPS() int SeptentrioGPS::print_status() { + SeptentrioGPS *secondary_instance = _secondary_instance.load(); + + switch (_instance) { + case SeptentrioInstance::Main: + PX4_INFO("Main GPS"); + break; + + case SeptentrioInstance::Secondary: + PX4_INFO(""); + PX4_INFO("Secondary GPS"); + + default: + break; + } + PX4_INFO("status: %s, port: %s", _healthy ? "OK" : "NOT OK", _port); PX4_INFO("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); PX4_INFO("rate reading: \t\t%6i B/s", _rate_reading); @@ -155,6 +184,10 @@ int SeptentrioGPS::print_status() print_message(ORB_ID(sensor_gps), _report_gps_pos); } + if (_instance == SeptentrioInstance::Main && secondary_instance) { + secondary_instance->print_status(); + } + return 0; } @@ -177,6 +210,7 @@ void SeptentrioGPS::run() // Set up the communication, configure the receiver and start processing data until we have to exit. while (!should_exit()) { _uart.setPort(_port); + if (!_uart.open()) { PX4_ERR("Error opening serial device %s", _port); continue; @@ -256,13 +290,26 @@ void SeptentrioGPS::run() int SeptentrioGPS::task_spawn(int argc, char *argv[]) { + return task_spawn(argc, argv, SeptentrioInstance::Main); +} + +int SeptentrioGPS::task_spawn(int argc, char *argv[], SeptentrioInstance instance) +{ + px4_main_t entry_point; static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(2040); + if (instance == SeptentrioInstance::Main) { + entry_point = (px4_main_t)&run_trampoline; + + } else { + entry_point = (px4_main_t)&run_trampoline_secondary; + } + px4_task_t task_id = px4_task_spawn_cmd("septentrio", SCHED_DEFAULT, SCHED_PRIORITY_SLOW_DRIVER, TASK_STACK_SIZE, - &run_trampoline, + entry_point, (char *const *)argv); if (task_id < 0) { @@ -272,26 +319,56 @@ int SeptentrioGPS::task_spawn(int argc, char *argv[]) return -errno; } - _task_id = task_id; + if (instance == SeptentrioInstance::Main) { + _task_id = task_id; + } + + return 0; +} + +int SeptentrioGPS::run_trampoline_secondary(int argc, char *argv[]) +{ + // Get rid of the task name (first argument) + argc -= 1; + argv += 1; + + SeptentrioGPS *gps = instantiate(argc, argv, SeptentrioInstance::Secondary); + + if (gps) { + _secondary_instance.store(gps); + gps->run(); + _secondary_instance.store(nullptr); + delete gps; + } return 0; } SeptentrioGPS *SeptentrioGPS::instantiate(int argc, char *argv[]) +{ + return instantiate(argc, argv, SeptentrioInstance::Main); +} + +SeptentrioGPS *SeptentrioGPS::instantiate(int argc, char *argv[], SeptentrioInstance instance) { const char *device_path = nullptr; + const char *device_path_secondary = nullptr; SeptentrioGPS *gps = nullptr; bool error_flag = false; int ch; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:e:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': device_path = myoptarg; break; + case 'e': + device_path_secondary = myoptarg; + break; + case '?': error_flag = true; break; @@ -307,11 +384,36 @@ SeptentrioGPS *SeptentrioGPS::instantiate(int argc, char *argv[]) return nullptr; } - if (device::Serial::validatePort(device_path)) { - gps = new SeptentrioGPS(device_path); + if (instance == SeptentrioInstance::Main) { + if (device::Serial::validatePort(device_path)) { + gps = new SeptentrioGPS(device_path, instance); + + } else { + PX4_ERR("invalid device (-d) %s", device_path ? device_path : ""); + } + + if (gps && device_path_secondary) { + task_spawn(argc, argv, SeptentrioInstance::Secondary); + + // Wait until the secondary instance is running + uint32_t i = 0; + + do { + px4_usleep(2500); + } while (!_secondary_instance.load() && ++i < 400); + + if (i == 400) { + PX4_ERR("Timed out while waiting for second instance to start"); + } + } } else { - PX4_ERR("invalid device (-d) %s", device_path ? device_path : ""); + if (device::Serial::validatePort(device_path_secondary)) { + gps = new SeptentrioGPS(device_path_secondary, instance); + + } else { + PX4_ERR("Invalid secondary device (-e) %s", device_path_secondary ? device_path_secondary : ""); + } } return gps; @@ -366,11 +468,21 @@ int SeptentrioGPS::print_usage(const char *reason) ### Description GPS driver module that handles the communication with Septentrio devices and publishes the position via uORB. -The module currently only supports a single GPS device. +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. + +### Examples + +Starting 2 GPS devices (main one on /dev/ttyS3, secondary on /dev/ttyS4) +$ septentrio start -d /dev/ttyS3 -e /dev/ttyS4 + +Initiate warm restart of GPS device +$ gps reset warm )DESCR_STR"); PRINT_MODULE_USAGE_NAME("septentrio", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "GPS device", true); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "Primary Septentrio receiver", true); + PRINT_MODULE_USAGE_PARAM_STRING('e', "/dev/ttyS4", "", "Secondary Septentrio receiver", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver"); @@ -383,6 +495,11 @@ int SeptentrioGPS::reset(SeptentrioGPSResetType type) { bool res = false; + send_message(SBF_FORCE_INPUT); + + // The receiver can't receive commands right after forcing input. + px4_usleep(500000); + switch (type) { case SeptentrioGPSResetType::Hot: res = send_message_and_wait_for_ack(SBF_CONFIG_RESET_HOT, SBF_CONFIG_TIMEOUT); @@ -400,7 +517,11 @@ int SeptentrioGPS::reset(SeptentrioGPSResetType type) break; } - return !res; + if (res) { + return PX4_OK; + } else { + return PX4_ERROR; + } } float SeptentrioGPS::get_position_update_rate() @@ -415,7 +536,13 @@ float SeptentrioGPS::get_velocity_update_rate() void SeptentrioGPS::schedule_reset(SeptentrioGPSResetType reset_type) { + SeptentrioGPS *secondary_instance = _secondary_instance.load(); + _scheduled_reset.store((int)reset_type); + + if (_instance == SeptentrioInstance::Main && secondary_instance) { + secondary_instance->schedule_reset(reset_type); + } } int SeptentrioGPS::detect_serial_port(char* const port_name) { @@ -1058,12 +1185,10 @@ void SeptentrioGPS::reset_if_scheduled() _scheduled_reset.store((int)SeptentrioGPSResetType::None); int res = reset(reset_type); - if (res == -1) { - PX4_INFO("Reset is not supported on this device."); - } else if (res > 0) { - PX4_INFO("Reset failed."); - } else { + if (res == PX4_OK) { PX4_INFO("Reset succeeded."); + } else { + PX4_INFO("Reset failed."); } } } diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index 6b1be8f0fbe5..1db253cfb1d1 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -81,6 +81,11 @@ enum class SeptentrioDumpCommMode : int32_t { RTCM, ///< dump received RTCM from Main GPS }; +enum class SeptentrioInstance : uint8_t { + Main = 0, + Secondary, +}; + enum class SeptentrioGPSResetType { /** * There is no pending GPS reset. @@ -122,7 +127,7 @@ enum class SeptentrioGPSOutputMode { class SeptentrioGPS : public ModuleBase, public device::Device { public: - SeptentrioGPS(const char *device_path); + SeptentrioGPS(const char *device_path, SeptentrioInstance instance); ~SeptentrioGPS() override; int print_status() override; @@ -132,9 +137,18 @@ class SeptentrioGPS : public ModuleBase, public device::Device /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); + static int task_spawn(int argc, char *argv[], SeptentrioInstance instance); + + /** + * @brief Secondary run trampoline to support two driver instances. + */ + static int run_trampoline_secondary(int argc, char *argv[]); + /** @see ModuleBase */ static SeptentrioGPS *instantiate(int argc, char *argv[]); + static SeptentrioGPS *instantiate(int argc, char *argv[], SeptentrioInstance instance); + /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); @@ -144,7 +158,7 @@ class SeptentrioGPS : public ModuleBase, public device::Device /** * @brief Reset the connected GPS receiver. * - * @return 0 on success, -1 on not implemented, >0 on error + * @return `PX4_OK` on success, `PX4_ERROR` on otherwise */ int reset(SeptentrioGPSResetType type); @@ -174,7 +188,7 @@ class SeptentrioGPS : public ModuleBase, public device::Device * * @return `PX4_OK` on success, `PX4_ERROR` on error */ - int detect_serial_port(char* const port_name); + int detect_serial_port(char *const port_name); /** * Configure the receiver. @@ -362,12 +376,17 @@ class SeptentrioGPS : public ModuleBase, public device::Device hrt_abstime _last_rtcm_injection_time{0}; ///< Time of last RTCM injection uint8_t _msg_status{0}; uint16_t _rx_payload_index{0}; ///< State for the message parser - sbf_buf_t _buf; ///< The complete received message + sbf_buf_t + _buf; ///< The complete received message RTCMParsing *_rtcm_parsing{nullptr}; ///< RTCM message parser uint8_t _selected_rtcm_instance{0}; ///< uORB instance that is being used for RTCM corrections bool _healthy{false}; ///< Flag to signal if the GPS is OK uint8_t _spoofing_state{0}; ///< Receiver spoofing state uint8_t _jamming_state{0}; ///< Receiver jamming state + const SeptentrioInstance + _instance; ///< The receiver that this instance of the driver controls + static px4::atomic + _secondary_instance; ///< Optional secondary instance of the driver // uORB topics and subscriptions gps_dump_s *_dump_to_device{nullptr}; ///< uORB GPS dump data (to the receiver)