OS time synchronization with external GNSS

ouster_client/include/ouster/client.h

@@ -53,6 +53,14 @@ std::shared_ptr<client> init_client(const std::string& hostname,
                                     lidar_mode mode = MODE_UNSPEC,
                                     timestamp_mode ts_mode = TIME_FROM_UNSPEC,
                                     int lidar_port = 0, int imu_port = 0,
+                                    multipurpose_io_mode io_mode = mio_mode_OFF,
+                                    nmea_baud_rate baud = BAUD_9600,
+                                    nmea_ignore_valid_char nmea_ignore = nmea_val_char_ignore,
+                                    nmea_in_polarity nmea_polarity = nmea_polarity_ACTIVE_LOW,
+                                    int nmea_leap_seconds = 0,
+                                    sync_pulse_in_polarity sync_pulse_pol = sync_pulse_in_ACTIVE_HIGH,
+                                    int azimuth_window_start = 0,
+                                    int azimuth_window_end = 360000,
                                     int timeout_sec = 30);
 
 /**

ouster_client/include/ouster/types.h

@@ -47,6 +47,41 @@ enum timestamp_mode {
     TIME_FROM_PTP_1588
 };
 
+enum multipurpose_io_mode {
+  mio_mode_UNSPEC = 0,
+  mio_mode_OFF = 1,
+  mio_INPUT_NMEA_UART,
+  mio_OUTPUT_FROM_INTERNAL_OSC,
+  mio_OUTPUT_FROM_SYNC_PULSE_IN,
+  mio_OUTPUT_FROM_PTP_1588,
+  mio_OUTPUT_FROM_ENCODER_ANGLE
+};
+
+enum nmea_baud_rate {
+  BAUD_UNSPEC = 0,
+  BAUD_9600 = 1,
+  BAUD_115200
+};
+
+enum nmea_in_polarity {
+  nmea_polarity_UNSPEC = 0,
+  nmea_polarity_ACTIVE_HIGH = 1,
+  nmea_polarity_ACTIVE_LOW
+};
+
+enum sync_pulse_in_polarity {
+  sync_pulse_in_UNSPEC = 0,
+  sync_pulse_in_ACTIVE_HIGH = 1,
+  sync_pulse_in_ACTIVE_LOW
+};
+
+enum nmea_ignore_valid_char{
+  nmea_val_char_UNSPEC = 0,
+  nmea_val_char_ignore = 1,
+  nmea_val_char_not_ignore
+};
+
+
 enum configuration_version {
     FW_2_0 = 3
 };
@@ -158,6 +193,98 @@ sensor_info metadata_from_json(const std::string& json_file);
  */
 std::string to_string(const sensor_info& metadata);
 
+/**
+ * Get string representation of a multipurpose_io_mode
+ * @param multipurpose_io_mode
+ * @return string representation of the multipurpose_io_mode
+ */
+std::string to_string(multipurpose_io_mode v);
+
+/**
+ * Get multipurpose_io_mode from string
+ * @param string
+ * @return multipurpose_io_mode corresponding to the string, or 0 on error
+ */
+multipurpose_io_mode multipurpose_io_mode_of_string(const std::string& s);
+
+/**
+ * Get string representation of a nmea_baud_rate
+ * @param nmea_baud_rate
+ * @return string representation of the nmea_baud_rate
+ */
+std::string to_string(nmea_baud_rate v);
+
+/**
+ * Get nmea_baud_rate from string
+ * @param string
+ * @return nmea_baud_rate corresponding to the string, or 0 on error
+ */
+nmea_baud_rate nmea_baud_rate_of_string(const std::string& s);
+
+/**
+ * Get string representation of a nmea_in_polarity
+ * @param nmea_in_polarity
+ * @return string representation of the nmea_in_polarity
+ */
+std::string to_string(nmea_in_polarity v);
+
+/**
+ * Get nmea_in_polarity from string
+ * @param string
+ * @return nmea_in_polarity corresponding to the string, or 0 on error
+ */
+nmea_in_polarity nmea_in_polarity_of_string(const std::string& s);
+
+/**
+ * Get string representation of a sync_pulse_in_polarity
+ * @param sync_pulse_in_polarity
+ * @return string representation of the sync_pulse_in_polarity
+ */
+std::string to_string(sync_pulse_in_polarity v);
+
+/**
+ * Get sync_pulse_in_polarity from string
+ * @param string
+ * @return sync_pulse_in_polarity corresponding to the string, or 0 on error
+ */
+sync_pulse_in_polarity sync_pulse_in_polarity_of_string(const std::string& s);
+
+
+/**
+ * Get string representation of a nmea_ignore_valid_char
+ * @param nmea_ignore_valid_char
+ * @return string representation of the nmea_ignore_valid_char
+ */
+std::string to_string(nmea_ignore_valid_char v);
+
+/**
+ * Get nmea_ignore_valid_char from string
+ * @param string
+ * @return nmea_ignore_valid_char corresponding to the string, or 0 on error
+ */
+nmea_ignore_valid_char nmea_ignore_valid_char_of_string(const std::string& s);
+
+/**
+ * Get nmea_leap_seconds from string
+ * @param string
+ * @return nmea_leap_seconds corresponding to the string, or -999999 on error
+ */
+int nmea_leap_seconds_of_string(const std::string& s);
+
+/**
+ * Get azimuth_window value from string
+ * @param string
+ * @return azimuth_window corresponding to the string
+ */
+int azimuth_window_of_string(const std::string& s);
+
+/**
+ * Get string representation of a azimuth_window_start and azimuth_window_end
+ * @param int azimuth_window_start, int azimuth_window_end
+ * @return string representation of the azimuth_window
+ */
+std::string azimuth_window_to_string(const int start, const int end);
+
 /**
  * Table of accessors for extracting data from imu and lidar packets.
  *

ouster_client/src/client.cpp

@@ -312,6 +312,14 @@ std::shared_ptr<client> init_client(const std::string& hostname,
                                     const std::string& udp_dest_host,
                                     lidar_mode mode, timestamp_mode ts_mode,
                                     int lidar_port, int imu_port,
+                                    multipurpose_io_mode io_mode,
+                                    nmea_baud_rate baud,
+                                    nmea_ignore_valid_char nmea_ignore,
+                                    nmea_in_polarity nmea_polarity,
+                                    int nmea_leap_seconds,
+                                    sync_pulse_in_polarity sync_pulse_pol,
+                                    int azimuth_window_start,
+                                    int azimuth_window_end,
                                     int timeout_sec) {
     auto cli = init_client(hostname, lidar_port, imu_port);
     if (!cli) return std::shared_ptr<client>();
@@ -357,6 +365,41 @@ std::shared_ptr<client> init_client(const std::string& hostname,
         success &= res == "set_config_param";
     }
 
+    success &= do_tcp_cmd(
+        sock_fd, {"set_config_param", "multipurpose_io_mode", to_string(io_mode)},
+        res);
+    success &= res == "set_config_param";
+
+    success &= do_tcp_cmd(
+        sock_fd, {"set_config_param", "nmea_baud_rate", to_string(baud)},
+        res);
+    success &= res == "set_config_param";
+
+    success &= do_tcp_cmd(
+        sock_fd, {"set_config_param", "nmea_ignore_valid_char", to_string(nmea_ignore)},
+        res);
+    success &= res == "set_config_param";
+
+    success &= do_tcp_cmd(
+        sock_fd, {"set_config_param", "nmea_in_polarity", to_string(nmea_polarity)},
+        res);
+    success &= res == "set_config_param";
+
+    success &= do_tcp_cmd(
+        sock_fd, {"set_config_param", "sync_pulse_in_polarity", to_string(sync_pulse_pol)},
+        res);
+    success &= res == "set_config_param";
+
+    success &= do_tcp_cmd(
+        sock_fd, {"set_config_param", "nmea_leap_seconds", std::to_string(nmea_leap_seconds)},
+        res);
+    success &= res == "set_config_param";
+    success &= do_tcp_cmd(
+        sock_fd, {"set_config_param", "azimuth_window", 
+                  azimuth_window_to_string(azimuth_window_start, azimuth_window_end)},
+        res);
+    success &= res == "set_config_param";
+
     success &= do_tcp_cmd(sock_fd, {"reinitialize"}, res);
     success &= res == "reinitialize";
 

ouster_client/src/types.cpp

@@ -33,6 +33,42 @@ const std::array<std::pair<timestamp_mode, std::string>, 3>
          {TIME_FROM_SYNC_PULSE_IN, "TIME_FROM_SYNC_PULSE_IN"},
          {TIME_FROM_PTP_1588, "TIME_FROM_PTP_1588"}}};
 
+const std::array<std::pair<multipurpose_io_mode, std::string>, 7>
+    multipurpose_io_mode_strings = {
+        {{mio_mode_UNSPEC, "UNSPEC"},
+         {mio_mode_OFF, "OFF"},
+         {mio_INPUT_NMEA_UART, "INPUT_NMEA_UART"},
+         {mio_OUTPUT_FROM_INTERNAL_OSC, "OUTPUT_FROM_INTERNAL_OSC"},
+         {mio_OUTPUT_FROM_SYNC_PULSE_IN, "OUTPUT_FROM_SYNC_PULSE_IN"},
+         {mio_OUTPUT_FROM_PTP_1588, "OUTPUT_FROM_PTP_1588"},
+         {mio_OUTPUT_FROM_ENCODER_ANGLE, "OUTPUT_FROM_ENCODER_ANGLE"}}};
+
+
+const std::array<std::pair<nmea_baud_rate, std::string>, 3>
+    nmea_baud_rate_strings = {
+        {{BAUD_UNSPEC, "BAUD_UNSPEC"},
+         {BAUD_9600, "BAUD_9600"},
+         {BAUD_115200, "BAUD_115200"}}};
+
+const std::array<std::pair<nmea_in_polarity, std::string>, 3>
+   nmea_in_polarity_strings = {
+        {{nmea_polarity_UNSPEC, "nmea_polarity_UNSPEC"},
+         {nmea_polarity_ACTIVE_HIGH, "ACTIVE_HIGH"},
+         {nmea_polarity_ACTIVE_LOW, "ACTIVE_LOW"}}};
+
+const std::array<std::pair<sync_pulse_in_polarity, std::string>, 3>
+    sync_pulse_in_polarity_strings = {
+        {{sync_pulse_in_UNSPEC, "sync_pulse_in_UNSPEC"},
+         {sync_pulse_in_ACTIVE_HIGH, "ACTIVE_HIGH"},
+         {sync_pulse_in_ACTIVE_LOW, "ACTIVE_LOW"}}};
+
+const std::array<std::pair<nmea_ignore_valid_char, std::string>, 3>
+    nmea_ignore_valid_char_strings = {
+        {{nmea_val_char_UNSPEC, "nmea_val_char_UNSPEC"},
+         {nmea_val_char_ignore, "0"},
+         {nmea_val_char_not_ignore, "1"}}};
+
+
 }  // namespace
 
 bool operator==(const data_format& lhs, const data_format& rhs) {
@@ -195,6 +231,129 @@ timestamp_mode timestamp_mode_of_string(const std::string& s) {
     return res == end ? timestamp_mode(0) : res->first;
 }
 
+std::string to_string(multipurpose_io_mode mode) {
+    auto end = multipurpose_io_mode_strings.end();
+    auto res = std::find_if(multipurpose_io_mode_strings.begin(), end,
+                            [&](const std::pair<multipurpose_io_mode, std::string>& p) {
+                                return p.first == mode;
+                            });
+    return res == end ? "UNKNOWN" : res->second;
+}
+
+multipurpose_io_mode multipurpose_io_mode_of_string(const std::string& s) {
+    auto end = multipurpose_io_mode_strings.end();
+    auto res = std::find_if(multipurpose_io_mode_strings.begin(), end,
+                            [&](const std::pair<multipurpose_io_mode, std::string>& p) {
+                                return p.second == s;
+                            });
+    return res == end ? multipurpose_io_mode(0) : res->first;
+}
+
+std::string to_string(nmea_baud_rate baud) {
+    auto end = nmea_baud_rate_strings.end();
+    auto res = std::find_if(nmea_baud_rate_strings.begin(), end,
+                            [&](const std::pair<nmea_baud_rate, std::string>& p) {
+                                return p.first == baud;
+                            });
+
+    return res == end ? "UNKNOWN" : res->second;
+}
+
+nmea_baud_rate nmea_baud_rate_of_string(const std::string& s) {
+    auto end = nmea_baud_rate_strings.end();
+    auto res = std::find_if(nmea_baud_rate_strings.begin(), end,
+                            [&](const std::pair<nmea_baud_rate, std::string>& p) {
+                                return p.second == s;
+                            });
+
+    return res == end ? nmea_baud_rate(0) : res->first;
+}
+
+std::string to_string(nmea_in_polarity polarity) {
+    auto end = nmea_in_polarity_strings.end();
+    auto res = std::find_if(nmea_in_polarity_strings.begin(), end,
+                            [&](const std::pair<nmea_in_polarity, std::string>& p) {
+                                return p.first == polarity;
+                            });
+
+    return res == end ? "UNKNOWN" : res->second;
+}
+
+nmea_in_polarity nmea_in_polarity_of_string(const std::string& s) {
+    auto end = nmea_in_polarity_strings.end();
+    auto res = std::find_if(nmea_in_polarity_strings.begin(), end,
+                            [&](const std::pair<nmea_in_polarity, std::string>& p) {
+                                return p.second == s;
+                            });
+
+    return res == end ? nmea_in_polarity(0) : res->first;
+}
+
+std::string to_string(sync_pulse_in_polarity polarity) {
+    auto end = sync_pulse_in_polarity_strings.end();
+    auto res = std::find_if(sync_pulse_in_polarity_strings.begin(), end,
+                            [&](const std::pair<sync_pulse_in_polarity, std::string>& p) {
+                                return p.first == polarity;
+                            });
+
+    return res == end ? "UNKNOWN" : res->second;
+}
+
+sync_pulse_in_polarity sync_pulse_in_polarity_of_string(const std::string& s) {
+    auto end = sync_pulse_in_polarity_strings.end();
+    auto res = std::find_if(sync_pulse_in_polarity_strings.begin(), end,
+                            [&](const std::pair<sync_pulse_in_polarity, std::string>& p) {
+                                return p.second == s;
+                            });
+
+    return res == end ? sync_pulse_in_polarity(0) : res->first;
+}
+
+std::string to_string(nmea_ignore_valid_char mode) {
+    auto end = nmea_ignore_valid_char_strings.end();
+    auto res = std::find_if(nmea_ignore_valid_char_strings.begin(), end,
+                            [&](const std::pair<nmea_ignore_valid_char, std::string>& p) {
+                                return p.first == mode;
+                            });
+
+    return res == end ? "UNKNOWN" : res->second;
+}
+
+nmea_ignore_valid_char nmea_ignore_valid_char_of_string(const std::string& s) {
+    auto end = nmea_ignore_valid_char_strings.end();
+    auto res = std::find_if(nmea_ignore_valid_char_strings.begin(), end,
+                            [&](const std::pair<nmea_ignore_valid_char, std::string>& p) {
+                                return p.second == s;
+                            });
+
+    return res == end ? nmea_ignore_valid_char(0) : res->first;
+}
+
+int nmea_leap_seconds_of_string(const std::string& s)
+{
+
+    return s == "0" ? 0 : (std::atoi(s.c_str()) == 0 ? -999999 :std::atoi(s.c_str()));
+
+}
+
+int azimuth_window_of_string(const std::string& s)
+{
+    if (s != "0" && std::atoi(s.c_str()) == 0)
+        return -999999;
+    else if (std::atoi(s.c_str()) >= 0 && std::atoi(s.c_str()) <= 360000) 
+	return std::atoi(s.c_str());
+    else 
+	return -999999;
+}
+
+std::string azimuth_window_to_string(const int start, const int end)
+{
+    if (start >=0 && start <= 360000 && end >=0 && end <= 360000)
+        return "[" + std::to_string(start) + "," + std::to_string(end) + "]";
+    else
+        return "[0,360000]";
+}
+
 sensor_info parse_metadata(const std::string& meta) {
     Json::Value root{};
     Json::CharReaderBuilder builder{};