diff --git a/config/params.yml b/config/params.yml index 80be4b8..9083a3e 100644 --- a/config/params.yml +++ b/config/params.yml @@ -161,6 +161,15 @@ gnss_glonass_enable : True gnss_galileo_enable : True gnss_beidou_enable : True +# ****************************************************************** +# Septentrio Aiding settings for 7-Series and Nova Development Board +# ****************************************************************** + +# If enabled, the septentrio_input_port will have Septentrio Binary Protocol input enabled, +# and have the baudrate configured to 4000000 baud which is required for Septentrio aiding. +# +septentrio_input_enable : False # Enable Sepentrio Binary Protocol input on a specified port (If ) +septentrio_input_port : 0x13 # Port to enable Sepntrio Binary protocol on, default value of UART3 (0x13) for use with Nova Development Board # ****************************************************************** # RTK Settings (only applicable for devices with RTK support (e.g. GQ7) ) diff --git a/src/config.cpp b/src/config.cpp index 778efd3..aa46c07 100644 --- a/src/config.cpp +++ b/src/config.cpp @@ -1356,6 +1356,32 @@ bool Config::configureSystem(RosNodeType* node) rtcm_on_main_port_ = false; } + // Configure Septentrio input on a given port if enabled. + + bool septentrio_input_enable; + uint8_t septentrio_input_port; + getParam(node, "septentrio_input_enable", septentrio_input_enable, false); + getParam(node, "septentrio_input_port", septentrio_input_port, false); + + // Enable Septentrio input on the provided port + uint32_t septentrio_protocol = 0x20000000; + + if (septentrio_input_enable) + { + MICROSTRAIN_DEBUG(node, "Configuring port %#X for Septentrio Binary Protocol input", septentrio_input_port); + if (!(mip_cmd_result = mip::commands_system::writeInterfaceControl(*mip_device_, static_cast(septentrio_input_port), static_cast(septentrio_protocol), mip::commands_system::CommsProtocol::NONE))) + { + MICROSTRAIN_MIP_SDK_ERROR(node_, mip_cmd_result, "Failed to enable Septentrio Binary Protocol Input"); + } + } + + // Configure the baud rate to be 4000000 on the selected port + MICROSTRAIN_DEBUG(node, "Configuring port %#X for 4000000 baud", septentrio_input_port); + if (!(mip_cmd_result = mip::commands_base::writeCommSpeed(*mip_device_, septentrio_input_port, 0x003d0900))) + { + MICROSTRAIN_MIP_SDK_ERROR(node_, mip_cmd_result, "Failed to set baudrate for port %#X", septentrio_input_port); + } + return true; }