feature:support save lvx file for lidar;

sdk_core/src/command_handler/command_impl.cpp

@@ -75,8 +75,8 @@ uint8_t GetConnectedDevices(DeviceInfo *devices, uint8_t *size) {
   return kStatusSuccess;
 }
 
-void SetDataCallback(uint8_t handle, DataCallback cb) {
-  data_handler().AddDataListener(handle, cb);
+void SetDataCallback(uint8_t handle, DataCallback cb, void *client_data) {
+  data_handler().AddDataListener(handle, cb, client_data);
 }
 
 uint8_t DeviceSampleControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) {
@@ -292,18 +292,15 @@ uint8_t HubSetExtrinsicParameter(HubSetExtrinsicParameterRequest *req,
   return result ? kStatusSuccess : kStatusFailure;
 }
 
-uint8_t HubGetExtrinsicParameter(HubGetExtrinsicParameterRequest *req,
-                                 uint16_t length,
-                                 HubGetExtrinsicParameterCallback cb,
-                                 void *client_data) {
+uint8_t HubGetExtrinsicParameter(HubGetExtrinsicParameterCallback cb, void *client_data) {
   if (device_manager().device_mode() != kDeviceModeHub) {
     return kStatusNotSupported;
   }
   bool result = command_handler().SendCommand(kHubDefaultHandle,
                                               kCommandSetHub,
                                               kCommandIDHubGetExtrinsicParameter,
-                                              (uint8_t *)req,
-                                              length,
+                                              NULL,
+                                              0,
                                               MakeCommandCallback<HubGetExtrinsicParameterResponse>(cb, client_data));
   return result ? kStatusSuccess : kStatusFailure;
 }
@@ -350,3 +347,16 @@ uint8_t HubRainFogSuppress(HubRainFogSuppressRequest *req,
                                               MakeCommandCallback<HubRainFogSuppressResponse>(cb, client_data));
   return result ? kStatusSuccess : kStatusFailure;
 }
+
+uint8_t HubQuerySlotPowerStatus(HubQuerySlotPowerStatusCallback cb, void *client_data) {
+  if (device_manager().device_mode() != kDeviceModeHub) {
+    return kStatusNotSupported;
+  }
+  bool result = command_handler().SendCommand(kHubDefaultHandle,
+                                              kCommandSetHub,
+                                              kCommandIDHubQuerySlotPowerStatus,
+                                              NULL,
+                                              0,
+                                              MakeCommandCallback<HubQuerySlotPowerStatusResponse>(cb, client_data));
+  return result ? kStatusSuccess : kStatusFailure;
+}

sdk_core/src/command_handler/command_impl.h

@@ -164,6 +164,10 @@ typedef enum {
    * Hub command set, open or close the rain and fog mode of the connected Livox LiDAR.
    */
   kCommandIDHubRainFogSuppression = 7,
+  /**
+  * Hub command set, get the power supply state of each hub slot.
+  */
+  kCommandIDHubQuerySlotPowerStatus = 8,
 
   /**
    * ******************************************************
@@ -179,6 +183,7 @@ static const uint16_t HubCommandTimeout[kCommandIDHubCommandCount] = {KDefaultTi
                                                                       KDefaultTimeOut,
                                                                       KDefaultTimeOut,
                                                                       KDefaultTimeOut,
+                                                                      KDefaultTimeOut,
                                                                       KDefaultTimeOut};
 
 /**

sdk_core/src/data_handler/data_handler.cpp

@@ -38,10 +38,11 @@ DataHandler &data_handler() {
   return handler;
 }
 
-bool DataHandler::AddDataListener(uint8_t handle, const DataCallback &cb) {
+bool DataHandler::AddDataListener(uint8_t handle, const DataCallback &cb, void *client_data) {
   if (handle >= callbacks_.size()) {
     return false;
   }
+  client_data_[handle] = client_data;
   callbacks_[handle] = cb;
   return true;
 }
@@ -107,7 +108,7 @@ void DataHandler::OnDataCallback(uint8_t handle, void *data, uint16_t size) {
     //LOG_INFO(" timeStampType: {}", (uint32_t) (lidar_data->timestamp_type));
     //LOG_INFO(" timeStamp: {}", (uint32_t) *(lidar_data->time_stamp));
     //LOG_INFO(" dataType: {}", (uint16_t) lidar_data->data_type);
-    cb(handle, lidar_data, size);
+    cb(handle, lidar_data, size, client_data_[handle]);
   }
 }
 

sdk_core/src/data_handler/data_handler.h

@@ -38,7 +38,7 @@ class DataHandlerImpl;
 
 class DataHandler : public noncopyable {
  public:
-  typedef boost::function<void(uint8_t handle, LivoxEthPacket *data, uint32_t data_num)> DataCallback;
+  typedef boost::function<void(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data)> DataCallback;
 
  public:
   DataHandler() : mem_pool_(NULL) {}
@@ -49,13 +49,13 @@ class DataHandler : public noncopyable {
   bool AddDevice(const DeviceInfo &info);
   void RemoveDevice(uint8_t handle);
 
-  bool AddDataListener(uint8_t handle, const DataCallback &cb);
-  //  bool RemoveDataListener(uint8_t handle);
+  bool AddDataListener(uint8_t handle, const DataCallback &cb, void *client_data);
   void OnDataCallback(uint8_t handle, void *data, uint16_t size);
 
  private:
   apr_pool_t *mem_pool_;
   boost::array<DataCallback, kMaxConnectedDeviceNum> callbacks_;
+  boost::array<void *, kMaxConnectedDeviceNum> client_data_;
   boost::scoped_ptr<DataHandlerImpl> impl_;
 };
 

sdk_core/src/device_manager.cpp

@@ -214,8 +214,6 @@ void DeviceManager::UpdateDeviceState(uint8_t handle, const HeartbeatResponse &r
   if (handle >= devices_.size()) {
     return;
   }
-
-  //LOG_INFO(" Update State {}, connect {}", (uint16_t)response.state, devices_[handle].connected);
   bool update = false;
   DeviceInfo &info = devices_[handle].info;
   if (info.state != response.state) {