support Mid-360 GPS time synchronization

CHANGELOG.md

@@ -2,6 +2,11 @@
 
 All notable changes to Livox-SDK2 will be documentd in this file.
 
+## [1.2.4]
+### Added
+
+- Support Mid-360 GPS time synchronization.
+
 ## [1.2.3]
 ### Added
 

include/livox_lidar_api.h

@@ -435,6 +435,17 @@ livox_status LivoxLidarStopLogger(const uint32_t handle, const LivoxLidarLogType
  */
 livox_status SetLivoxLidarDebugPointCloud(uint32_t handle, bool enable, LivoxLidarLoggerCallback cb, void* client_data);
 
+/**
+ * Set LiDAR GPS "GPRMC" string to synchronize the time.
+ * @param  handle                           device handle.
+ * @param  rmc                              GPS "GPRMC" string.
+ * @param  rmc_length                       GPS "GPRMC" string length.
+ * @param  cb                               callback for the command.
+ * @param  client_data                      user data associated with the command.
+ * @return kStatusSuccess on successful return, see \ref LivoxStatus for other error codes.
+ */
+livox_status SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t rmc_length, LivoxLidarRmcSyncTimeCallBack cb, void* client_data);
+
 /*******Upgrade Module***********/
 
 /**

include/livox_lidar_def.h

@@ -464,6 +464,10 @@ typedef struct {
   uint8_t progress;
 } LivoxLidarUpgradeState;
 
+typedef struct {
+  uint8_t ret; // succ: 0, fail: 1
+} LivoxLidarRmcSyncTimeResponse;
+
 #pragma pack()
 
 /**
@@ -557,4 +561,13 @@ typedef void (*LivoxLidarRebootCallback)(livox_status status, uint32_t handle, L
 
 typedef void (*OnLivoxLidarUpgradeProgressCallback)(uint32_t handle, LivoxLidarUpgradeState state, void *client_data);
 
+/**
+ * Callback function for receiving point cloud data.
+ * @param status                 status info.
+ * @param handle                 device handle.
+ * @param data                   device's command data.
+ * @param client_data            user data associated with the command.
+ */
+typedef void (*LivoxLidarRmcSyncTimeCallBack)(livox_status status, uint32_t handle, LivoxLidarRmcSyncTimeResponse* data, void* client_data);
+
 #endif  // LIVOX_LIDAR_DEF_H_

samples/CMakeLists.txt

@@ -21,3 +21,4 @@ add_subdirectory(multi_lidars_upgrade)
 add_subdirectory(logger)
 add_subdirectory(debug_point_cloud)
 add_subdirectory(lidar_cmd_observer)
+add_subdirectory(livox_lidar_rmc_time_sync)

samples/livox_lidar_rmc_time_sync/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.0)
+
+set(DEMO_NAME livox_lidar_rmc_time_sync)
+if (WIN32)
+  add_executable(${DEMO_NAME} main.cpp win/synchro.cpp)
+elseif(UNIX)
+  add_executable(${DEMO_NAME} main.cpp linux/synchro.cpp)
+endif()
+
+target_link_libraries(${DEMO_NAME}
+        PUBLIC
+        livox_lidar_sdk_static
+)

samples/livox_lidar_rmc_time_sync/config.json

@@ -0,0 +1,23 @@
+{
+	"MID360": {
+		"lidar_net_info" : {
+			"cmd_data_port": 56100,
+			"push_msg_port": 56200,
+			"point_data_port": 56300,
+			"imu_data_port": 56400,
+			"log_data_port": 56500
+		},
+		"host_net_info" : {
+			"cmd_data_ip" : "192.168.1.5",
+			"cmd_data_port": 56101,
+			"push_msg_ip": "192.168.1.5",
+			"push_msg_port": 56201,
+			"point_data_ip": "192.168.1.5",
+			"point_data_port": 56301,
+			"imu_data_ip" : "192.168.1.5",
+			"imu_data_port": 56401,
+			"log_data_ip" : "192.168.1.5",
+			"log_data_port": 56501
+		}
+	}
+}	

samples/livox_lidar_rmc_time_sync/hap_config.json

@@ -0,0 +1,22 @@
+{
+  "HAP": {
+    "lidar_net_info" : {
+      "cmd_data_port"  : 56000,
+      "push_msg_port"  : 0,
+      "point_data_port": 57000,
+      "imu_data_port"  : 58000,
+      "log_data_port"  : 59000
+    },
+    "host_net_info" : [
+      {
+        "host_ip"        : "192.168.1.5",
+        "multicast_ip"   : "224.1.1.5",
+        "cmd_data_port"  : 56000,
+        "push_msg_port"  : 0,
+        "point_data_port": 57000,
+        "imu_data_port"  : 58000,
+        "log_data_port"  : 59000
+      }
+    ]
+  }
+}

samples/livox_lidar_rmc_time_sync/linux/synchro.cpp

@@ -0,0 +1,254 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2022 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "../synchro.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <termios.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/fcntl.h>
+#include <fcntl.h>
+
+Synchro::Synchro() {
+  fd_ = 0; 
+  is_quit_ = false;
+  rmc_buff_.resize(128,0);
+}
+
+Synchro::~Synchro() {
+  Stop();
+}
+
+void Synchro::SetBaudRate(BaudRate baudrate) {
+  baudrate_ = baudrate;
+}
+
+void Synchro::SetParity(Parity parity) {
+  parity_ = parity;
+}
+
+void Synchro::SetPortName(std::string port_name) {
+  port_name_ = std::move(port_name);
+}
+
+bool Synchro::Start() {
+  if (!Open()) {
+    return false;
+  }
+  is_quit_ = false;
+  listener_ = std::make_shared<std::thread>(&Synchro::IOLoop, this);
+  return true;
+}
+
+void Synchro::Stop() {
+  is_quit_ = true;
+  if (listener_) {
+    listener_->join();
+    listener_ = nullptr;
+  }
+  Close();
+}
+
+void Synchro::IOLoop() {
+  uint8_t buff[READ_BUF];
+  size_t size = 0;
+  while (!is_quit_) {
+    if (0 < (size = Read(buff, READ_BUF))) {
+      Decode(buff, size);
+    }
+  }
+}
+
+bool Synchro::ParseGps(uint8_t in_byte) {
+  if (cur_len_ < strlen(kGPRMC)) {
+    rmc_buff_[0] = rmc_buff_[1];
+    rmc_buff_[1] = rmc_buff_[2];
+    rmc_buff_[2] = rmc_buff_[3];
+    rmc_buff_[3] = rmc_buff_[4];
+    rmc_buff_[4] = rmc_buff_[5];
+    rmc_buff_[5] = in_byte;
+    cur_len_++;
+
+    if (cur_len_ == strlen(kGPRMC)) {
+      if (strncmp((const char*)&rmc_buff_[0], kGPRMC, strlen(kGPRMC)) != 0 &&  \
+          strncmp((const char*)&rmc_buff_[0], kGNRMC, strlen(kGNRMC)) != 0) {
+        cur_len_--;
+      } 
+    }
+    return false;
+  }
+
+  if (cur_len_ >= rmc_buff_.size()) {
+    Clear();
+    return false;
+  }
+
+  rmc_buff_[cur_len_] = in_byte;
+/** Checksum $GPRMC/$GNRMC. */
+  if (rmc_buff_[cur_len_ - 2] == '*') {
+     uint8_t result = 0;
+     for (int i = 1; i < cur_len_ - 2; i++) {
+       result ^= rmc_buff_[i];
+     }
+     char crc[3] = {0};
+     uint32_t crc_num = 0;
+     strncpy(crc, &rmc_buff_[cur_len_ - 1], 2);
+     sscanf(crc, "%x", &crc_num);
+     result ^= (uint8_t)crc_num;
+     if (result == 0) {
+       return true;
+     }
+  }
+  cur_len_++;
+  return false; 
+}
+
+void Synchro::SetSyncTimerCallback(UtcTimerCallback cb) {
+  sync_timer_cb_ = cb;
+}
+
+void Synchro::Decode(uint8_t * buff, size_t size) {
+  for (size_t i = 0; i < size; i++) {
+    if (ParseGps(buff[i])) {
+      if (sync_timer_cb_) {
+        sync_timer_cb_(&rmc_buff_.at(0), cur_len_);
+        Clear();
+      }
+    }
+  }
+}
+
+void Synchro::Clear() {
+   cur_len_ = 0;
+   std::fill(rmc_buff_.begin(), rmc_buff_.end(), 0);
+}
+
+bool Synchro::Open() {
+  fd_ = open(port_name_.c_str(), O_RDONLY | O_NOCTTY | O_NONBLOCK);
+  if (fd_ < 0) {
+    printf("Open %s serials fail!\n", port_name_.c_str());
+    return false;
+  }
+
+  /* Set baudrate and parity,etc. */
+  Setup(baudrate_, parity_);
+  printf("Set baudrate success.\n");
+  return true;
+}
+
+void Synchro::Close() {
+  if (fd_ > 0) {
+    /* Flush the port */
+    tcflush(fd_,TCOFLUSH); 
+    tcflush(fd_,TCIFLUSH); 
+
+    close(fd_);
+  }
+  fd_ = 0;
+}
+
+/* Sets up the port parameters */
+int Synchro::Setup(enum BaudRate baud, enum Parity parity) {
+  static uint32_t baud_map[19] = {
+                                   B2400, B4800, B9600, B19200, B38400, B57600,B115200, B230400
+                                 };
+  tcflag_t baudrate;
+  struct termios options;
+
+  /* Clear old setting completely,must add here for A3 CDC serial */
+  tcgetattr(fd_, &options);
+  memset(&options, 0, sizeof(options));
+  tcflush(fd_, TCIOFLUSH);
+  tcsetattr(fd_, TCSANOW, &options);
+  // usleep(100);
+
+  /* Minimum number of characters to read */
+  options.c_cc[VMIN] = 0;
+  /* Time to wait for data */
+  options.c_cc[VTIME] = 100;
+
+  /* Enable the receiver and set local mode... */
+  options.c_cflag |= (CLOCAL | CREAD);
+
+  /* Set boadrate */
+  baudrate = baud_map[baud];
+  cfsetispeed(&options, baudrate);
+  cfsetospeed(&options, baudrate);
+  printf("[Baudrate]: %d %u\r\n", baud, baudrate);
+
+  switch (parity) {
+    case P_8N1:
+      /* No parity (8N1)  */
+      options.c_cflag &= ~PARENB;
+      options.c_cflag &= ~CSTOPB;
+      options.c_cflag &= ~CSIZE;
+      options.c_cflag |= CS8;
+    break;
+    case P_7E1:
+      /* Even parity (7E1) */
+      options.c_cflag |= PARENB;
+      options.c_cflag &= ~PARODD;
+      options.c_cflag &= ~CSTOPB;
+      options.c_cflag &= ~CSIZE;
+      options.c_cflag |= CS7;
+    break;
+    case P_7O1:
+      /* Odd parity (7O1) */
+      options.c_cflag |= PARENB;
+      options.c_cflag |= PARODD;
+      options.c_cflag &= ~CSTOPB;
+      options.c_cflag &= ~CSIZE;
+      options.c_cflag |= CS7;
+    break;
+    case P_7S1:
+      /* Space parity is setup the same as no parity (7S1)  */
+      options.c_cflag &= ~PARENB;
+      options.c_cflag &= ~CSTOPB;
+      options.c_cflag &= ~CSIZE;
+      options.c_cflag |= CS8;
+    break;
+    default:
+      return -1;
+  }    
+
+  /* flush the port */
+  tcflush(fd_, TCIOFLUSH);
+
+  /* send new config to the port */
+  tcsetattr(fd_, TCSANOW, &options);
+
+  return 0;
+}
+
+size_t Synchro::Read(uint8_t *buffer, size_t size) {
+  if (fd_ > 0) {
+    auto len = read(fd_, buffer, size);
+    return len < 0 ? 0 : len;
+  } else {
+    return 0;
+  }
+}