find current baudrate and reset to value from config-file

config/sbg_device_uart_default.yaml

@@ -30,6 +30,7 @@
 
       # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600)
       baudRate: 115200
+      fallbackBaudRate: 115200
 
       # Port Id
       # 0 PORT_A: Main communication interface. Full duplex.
@@ -323,4 +324,4 @@
       # Topic on which to publish nmea data
       topic_name: nmea
       # Namespace where to publish topic
-      namespace: ntrip_client
+      namespace: ntrip_client

include/sbg_driver/config_store.h

@@ -76,6 +76,7 @@ class ConfigStore
   std::string                 uart_port_name_;
   SbgEComOutputPort           output_port_;
   uint32_t                    uart_baud_rate_;
+  uint32_t                    fallback_uart_baud_rate_;
   bool                        serial_communication_;
 
   sbgIpAddress                sbg_ip_address_;
@@ -296,12 +297,19 @@ class ConfigStore
   const std::string &getUartPortName() const;
 
   /*!
-   * Get the UART baudrate communication.
+   * Get the UART baudrate.
    *
    * \return                      UART serial baudrate.
    */
   uint32_t getBaudRate() const;
 
+  /*!
+   * Get the fallback UART baudrate.
+   *
+   * \return                      UART serial baudrate.
+   */
+  uint32_t getFallbackBaudRate() const;
+
   /*!
    * Get the output port of the device.
    *

include/sbg_driver/sbg_device.h

@@ -126,8 +126,24 @@ class SbgDevice
    * Read the device informations.
    *
    * \throw                       Unable to read the device information.
+   * \return                      SBG_NO_ERROR if reading device info succeeded.
    */
-  void readDeviceInfo();
+  SbgErrorCode readDeviceInfo();
+
+  /*!
+   * Find the baudrate currently configured on the device. Leaves the ECOM interface in an initialized state.
+   *
+   * \return                                 SBG_NO_ERROR if the current device baudrate was found.
+   * \throw                                  Unable to read the device information.
+   */
+  SbgErrorCode findCurrentDeviceBaudrate();
+
+  /*!
+   * Use the current baudrate to set the baudrate configured in the config file.
+   *
+   * \throw                       Unable to read the device information.
+   */
+  void setDeviceBaudrate();
 
   /*!
    * Get the SBG version as a string.

src/config_store.cpp

@@ -49,6 +49,7 @@ void ConfigStore::loadCommunicationParameters(const rclcpp::Node& ref_node_handl
     ref_node_handle.get_parameter_or<std::string>("uartConf.portName", uart_port_name_, "/dev/ttyUSB0");
 
     uart_baud_rate_ = getParameter<uint32_t>(ref_node_handle, "uartConf.baudRate", 0);
+    fallback_uart_baud_rate_ = getParameter<uint32_t>(ref_node_handle, "uartConf.fallbackBaudRate", 0);
     output_port_    = getParameter<SbgEComOutputPort>(ref_node_handle, "uartConf.portID", SBG_ECOM_OUTPUT_PORT_A);
   }
   else if (ref_node_handle.has_parameter("ipConf.ipAddress"))
@@ -241,6 +242,11 @@ uint32_t ConfigStore::getBaudRate() const
   return uart_baud_rate_;
 }
 
+uint32_t ConfigStore::getFallbackBaudRate() const
+{
+  return fallback_uart_baud_rate_;
+}
+
 SbgEComOutputPort ConfigStore::getOutputPort() const
 {
   return output_port_;

src/sbg_device.cpp

@@ -8,6 +8,7 @@
 
 // SbgECom headers
 #include <version/sbgVersion.h>
+#include <commands/sbgEComCmdInterface.h>
 
 using namespace std;
 using sbg::SbgDevice;
@@ -150,13 +151,106 @@ void SbgDevice::connect()
     rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code)));
   }
 
-  readDeviceInfo();
+  error_code = readDeviceInfo();
+
+  if (error_code == SBG_NO_ERROR)
+  {
+    return;
+  }
+
+  if (error_code == SBG_TIME_OUT && config_store_.isInterfaceSerial())
+  {
+    // readDeviceInfo() not successful - error could be that the baudrate configured on the device
+    // is different from the one configured in the config file. Re-try with different baudrate
+    error_code = findCurrentDeviceBaudrate();
+  }
+
+  if (error_code == SBG_NO_ERROR)
+  {
+    setDeviceBaudrate();
+  }
 }
 
-void SbgDevice::readDeviceInfo()
+SbgErrorCode SbgDevice::findCurrentDeviceBaudrate()
+{
+  SbgErrorCode error_code;
+  error_code = SBG_ERROR;
+
+  auto br = config_store_.getFallbackBaudRate();
+  if (br != 0)
+  {
+    RCLCPP_INFO(ref_node_.get_logger(), "Not successful with %d bps, trying with %d bps instead", config_store_.getBaudRate(), br);
+
+    sbgEComClose(&com_handle_);
+    sbgInterfaceDestroy(&sbg_interface_);
+
+    error_code = sbgInterfaceSerialCreate(&sbg_interface_, config_store_.getUartPortName().c_str(), br);
+
+    if (error_code != SBG_NO_ERROR)
+    {
+      rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the interface - " + std::string(sbgErrorCodeToString(error_code)));
+    }
+
+    error_code = sbgEComInit(&com_handle_, &sbg_interface_);
+
+    if (error_code != SBG_NO_ERROR)
+    {
+      rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code)));
+    }
+
+    error_code = readDeviceInfo();
+
+    if (error_code == SBG_NO_ERROR) {
+      // current device baud rate found
+      return SBG_NO_ERROR;
+    }
+  }
+  return error_code;
+}
+
+void SbgDevice::setDeviceBaudrate()
+{
+  SbgErrorCode error_code;
+  SbgEComInterfaceConf com_conf;
+  error_code = sbgEComCmdInterfaceGetUartConf(&com_handle_, SBG_ECOM_IF_COM_A, &com_conf);
+  if (error_code != SBG_NO_ERROR) {
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Reconfig] Unable to get config of device - " + std::string(sbgErrorCodeToString(error_code)));
+  }
+
+  com_conf.baudRate = config_store_.getBaudRate();
+  error_code = sbgEComCmdInterfaceSetUartConf(&com_handle_, SBG_ECOM_IF_COM_A, &com_conf);
+  if (error_code != SBG_NO_ERROR) {
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Reconfig] Unable to set new baudrate of device - " + std::string(sbgErrorCodeToString(error_code)));
+  }
+
+  error_code = sbgEComCmdSettingsAction(&com_handle_, SBG_ECOM_SAVE_SETTINGS);
+  if (error_code != SBG_NO_ERROR) {
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Reconfig] Unable to save settings on device - " + std::string(sbgErrorCodeToString(error_code)));
+  }
+
+  sbgEComClose(&com_handle_);
+  sbgInterfaceDestroy(&sbg_interface_);
+
+  error_code = sbgInterfaceSerialCreate(&sbg_interface_, config_store_.getUartPortName().c_str(), config_store_.getBaudRate());
+
+  if (error_code != SBG_NO_ERROR)
+  {
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the interface - " + std::string(sbgErrorCodeToString(error_code)));
+  }
+
+  error_code = sbgEComInit(&com_handle_, &sbg_interface_);
+
+  if (error_code != SBG_NO_ERROR)
+  {
+    rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, "SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code)));
+  }
+  RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - successfully reconfigured baudrate to %d", config_store_.getBaudRate());
+}
+
+SbgErrorCode SbgDevice::readDeviceInfo()
 {
   SbgEComDeviceInfo device_info;
-  SbgErrorCode      error_code;
+  SbgErrorCode error_code;
 
   error_code = sbgEComCmdGetInfo(&com_handle_, &device_info);
 
@@ -170,11 +264,14 @@ void SbgDevice::readDeviceInfo()
 
     RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - hardwareRev = %s", getVersionAsString(device_info.hardwareRev).c_str());
     RCLCPP_INFO(ref_node_.get_logger(), "SBG_DRIVER - firmwareRev = %s", getVersionAsString(device_info.firmwareRev).c_str());
+    return SBG_NO_ERROR;
   }
   else
   {
     RCLCPP_ERROR(ref_node_.get_logger(), "Unable to get the device Info : %s", sbgErrorCodeToString(error_code));
   }
+
+  return error_code;
 }
 
 std::string SbgDevice::getVersionAsString(uint32_t sbg_version_enc) const