add APIs to get CMD packet data

CHANGELOG.md

@@ -2,6 +2,12 @@
 
 All notable changes to Livox-SDK2 will be documentd in this file.
 
+## [1.2.3]
+### Added
+
+- Support to get CMD packet data;
+- Support full info keys of HAP. 
+
 ## [1.2.2]
 ### Added
 
@@ -29,4 +35,4 @@ All notable changes to Livox-SDK2 will be documentd in this file.
 
 - Support Mid-360 Lidar;
 - Partially support the firmware log functionality;
-- Support firmware upgrade for HAP & Mid-360.
+- Support firmware upgrade for HAP & Mid-360.

LICENSE.txt

@@ -304,4 +304,4 @@ 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.
+SOFTWARE.

include/livox_lidar_api.h

@@ -63,6 +63,18 @@ void LivoxLidarSdkUninit();
  */
 void SetLivoxLidarPointCloudCallBack(LivoxLidarPointCloudCallBack cb, void* client_data);
 
+/**
+ * Add the lidar command data observer.
+ * @param handle                 device handle.
+ * @param client_data            user data associated with the command.
+ */
+void LivoxLidarAddCmdObserver(LivoxLidarCmdObserverCallBack cb, void* client_data);
+
+/**
+ * Remove the lidar command data observer.
+ */
+void LivoxLidarRemoveCmdObserver();
+
 /**
  * Set the point cloud observer.
  * @param cb                     callback to receive Status Info.

include/livox_lidar_def.h

@@ -116,6 +116,7 @@ typedef enum {
   kKeyLidarFlashStatus        = 0x800F,
   kKeyFwType                  = 0x8010,
   kKeyHmsCode                 = 0x8011,
+  kKeyCurGlassHeatState       = 0x8012,
 
   kKeyRoiMode                 = 0xFFFE,
   kKeyLidarDiagInfoQuery      = 0xFFFF
@@ -136,6 +137,20 @@ typedef struct {
   uint8_t data[1];             /**< Point cloud data. */
 } LivoxLidarEthernetPacket;
 
+typedef struct {
+  uint8_t  sof;
+  uint8_t  version;
+  uint16_t length;
+  uint32_t seq_num;
+  uint16_t cmd_id;
+  uint8_t  cmd_type;
+  uint8_t  sender_type;
+  char     rsvd[6];
+  uint16_t crc16_h;
+  uint32_t crc32_d;
+  uint8_t  data[1];
+} LivoxLidarCmdPacket;
+
 typedef struct {
   float gyro_x;
   float gyro_y;
@@ -460,6 +475,14 @@ typedef struct {
  */
 typedef void (*LivoxLidarPointCloudCallBack)(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* data, void* client_data);
 
+/**
+ * Callback function for receiving point cloud data.
+ * @param handle                 device handle.
+ * @param data                   device's command data.
+ * @param client_data            user data associated with the command.
+ */
+typedef void (*LivoxLidarCmdObserverCallBack)(const uint32_t handle, const LivoxLidarCmdPacket* data, void* client_data);
+
 /**
  * Callback function for point cloud observer.
  * @param handle                 device handle.

samples/CMakeLists.txt

@@ -20,3 +20,4 @@ add_subdirectory(livox_lidar_quick_start)
 add_subdirectory(multi_lidars_upgrade)
 add_subdirectory(logger)
 add_subdirectory(debug_point_cloud)
+add_subdirectory(lidar_cmd_observer)

samples/debug_point_cloud/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/lidar_cmd_observer/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.0)
+
+set(DEMO_NAME lidar_cmd_observer)
+add_executable(${DEMO_NAME} main.cpp)
+
+target_include_directories(
+        ${DEMO_NAME}
+        PRIVATE
+        ../../3rdparty/
+        )
+
+target_link_libraries(${DEMO_NAME}
+        PUBLIC
+        livox_lidar_sdk_static
+				)

samples/lidar_cmd_observer/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/lidar_cmd_observer/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/lidar_cmd_observer/main.cpp

@@ -0,0 +1,89 @@
+//
+// 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 "livox_lidar_def.h"
+#include "livox_lidar_api.h"
+
+#ifdef _WIN32
+#include <winsock2.h>
+#else
+#include <unistd.h>
+#include <arpa/inet.h>
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <thread>
+#include <chrono>
+#include <iostream>
+
+#include "FastCRC/FastCRC.h"
+
+int main(int argc, const char *argv[]) {
+  if (argc != 2) {
+    printf("Params Invalid, must input config path.\n");
+    return -1;
+  }
+  const std::string path = argv[1];
+
+  // REQUIRED, to init Livox SDK2
+  if (!LivoxLidarSdkInit(path.c_str())) {
+    printf("Livox Init Failed\n");
+    LivoxLidarSdkUninit();
+    return -1;
+  }
+
+  LivoxLidarAddCmdObserver([](const uint32_t handle, const LivoxLidarCmdPacket* data, void* client_data) {
+    static FastCRC16 crc_16;
+    static FastCRC32 crc_32;
+    static const uint8_t offset_of_crc16 = offsetof(LivoxLidarCmdPacket, crc16_h);
+    static const uint8_t offset_of_data  = offsetof(LivoxLidarCmdPacket, data);
+    struct in_addr tmp_addr;
+    tmp_addr.s_addr = handle;
+    if (crc_16.ccitt(reinterpret_cast<const uint8_t*>(data), offset_of_crc16) != data->crc16_h) {
+      printf("Error: liadr: %s, crc16 check failure.\n", inet_ntoa(tmp_addr));
+      exit(-1);
+    }
+    if (crc_32.crc32(reinterpret_cast<const uint8_t*>(data) + offset_of_data, data->length - offset_of_data) != data->crc32_d) {
+      printf("Error: liadr: %s, crc32 check failure.\n", inet_ntoa(tmp_addr));
+      exit(-1);
+    }
+    printf("--------------------------------------------------\n");
+    printf("liadr: %s, crc check success.\n", inet_ntoa(tmp_addr));
+    printf("--------------------------------------------------\n");
+  }, nullptr);
+
+#ifdef WIN32
+  Sleep(300000);
+#else
+  sleep(300);
+#endif
+
+  LivoxLidarRemoveCmdObserver();
+
+  LivoxLidarSdkUninit();
+  printf("Livox Quick Start Demo End!\n");
+  return 0;
+}

samples/lidar_cmd_observer/mid360_config.json

@@ -0,0 +1,22 @@
+{
+  "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" : [
+      {
+        "host_ip"        : "192.168.1.5",
+        "multicast_ip"   : "224.1.1.5",
+        "cmd_data_port"  : 56101,
+        "push_msg_port"  : 56201,
+        "point_data_port": 56301,
+        "imu_data_port"  : 56401,
+        "log_data_port"  : 56501
+      }
+    ]
+  }
+}

sdk_core/command_handler/general_command_handler.cpp

@@ -183,6 +183,11 @@ void GeneralCommandHandler::Handler(const uint8_t dev_type, const uint32_t handl
   if (buf == nullptr || buf_size == 0) {
     return;
   }
+
+  if (cmd_observer_cb_) {
+    cmd_observer_cb_(handle, reinterpret_cast<LivoxLidarCmdPacket*>(buf), cmd_observer_client_data_);
+  }
+
   if (dev_type == kLivoxLidarTypePA && lidar_port == kPaLidarFaultPort) {
     std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(dev_type);
     if (cmd_handler == nullptr) {
@@ -575,19 +580,31 @@ bool GeneralCommandHandler::GetQueryLidarInternalInfoKeys(const uint32_t handle,
       std::set<ParamKeyName> tmp_key_sets {
         kKeyPclDataType,
         kKeyPatternMode,
+        kKeyDualEmitEn,
+        kKeyPointSendEn,
         kKeyLidarIpCfg,
         kKeyLidarPointDataHostIpCfg,
         kKeyLidarImuHostIpCfg,
+        kKeyLogHostIpCfg,
         kKeyInstallAttitude,
+        kKeyBlindSpotSet,
         kKeyWorkMode,
+        kKeyGlassHeat,
         kKeyImuDataEn,
+        kKeyFusaEn,
+        kKeyForceHeatEn,
         kKeySn,
         kKeyProductInfo,
         kKeyVersionApp,
         kKeyVersionLoader,
         kKeyVersionHardware,
         kKeyMac,
-        kKeyCurWorkState
+        kKeyCurWorkState,
+        kKeyStatusCode,
+        kKeyLidarDiagStatus,
+        kKeyLidarFlashStatus,
+        kKeyFwType,
+        kKeyCurGlassHeatState
       };
       key_sets.swap(tmp_key_sets);
       return true;

sdk_core/command_handler/general_command_handler.h

@@ -100,6 +100,16 @@ class GeneralCommandHandler : public noncopyable {
     livox_lidar_info_client_data_ = client_data;
   }
 
+  void LivoxLidarAddCmdObserver(LivoxLidarCmdObserverCallBack cb, void* client_data) {
+    cmd_observer_cb_ = cb;
+    cmd_observer_client_data_ = client_data;
+  }
+
+  void LivoxLidarRemoveCmdObserver() {
+    cmd_observer_cb_ = nullptr;
+    cmd_observer_client_data_ = nullptr;
+  }
+
   void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info);
   void UpdateLidarCfg(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_cmd_port);
   void LivoxLidarInfoChange(const uint32_t handle);
@@ -140,6 +150,9 @@ class GeneralCommandHandler : public noncopyable {
   LivoxLidarInfoCallback livox_lidar_info_cb_;
   void* livox_lidar_info_client_data_;
 
+  LivoxLidarCmdObserverCallBack cmd_observer_cb_{nullptr};
+  void* cmd_observer_client_data_{nullptr};
+
   std::string detection_host_ip_;
   bool is_view_;
 };

sdk_core/livox_lidar_sdk.cpp

@@ -147,6 +147,14 @@ void SetLivoxLidarPointCloudCallBack(LivoxLidarPointCloudCallBack cb, void *clie
   DataHandler::GetInstance().SetPointDataCallback(cb, client_data);
 }
 
+void LivoxLidarAddCmdObserver(LivoxLidarCmdObserverCallBack cb, void *client_data) {
+  GeneralCommandHandler::GetInstance().LivoxLidarAddCmdObserver(cb, client_data);
+}
+
+void LivoxLidarRemoveCmdObserver() {
+  GeneralCommandHandler::GetInstance().LivoxLidarRemoveCmdObserver();
+}
+
 void SetLivoxLidarImuDataCallback(LivoxLidarImuDataCallback cb, void* client_data) {
   DataHandler::GetInstance().SetImuDataCallback(cb, client_data);
 }