From eb19d42ce4bff55f290565e506c8fe3b1b53763d Mon Sep 17 00:00:00 2001 From: Livox-SDK Date: Mon, 26 Oct 2020 20:25:42 +0800 Subject: [PATCH] feat:support Mid70 and Avia --- Doxyfile | 2 +- README.md | 2 +- README_CN.md | 2 +- sample/hub/main.c | 4 + sample/hub_lvx_file/lvx_file.cpp | 63 +++++--- sample/lidar/main.c | 4 + sample/lidar_lvx_file/lvx_file.cpp | 62 +++++--- sample/lidar_lvx_file/main.cpp | 1 + sample_cc/hub/lds_hub.cpp | 6 +- sample_cc/lidar/lds_lidar.cpp | 8 +- sdk_core/include/livox_def.h | 64 +++++++- sdk_core/include/livox_sdk.h | 86 ++++++++++- sdk_core/src/base/io_loop.cpp | 5 +- sdk_core/src/base/io_loop.h | 7 +- .../src/command_handler/command_channel.cpp | 28 ++-- .../src/command_handler/command_channel.h | 15 +- sdk_core/src/command_handler/command_impl.cpp | 143 +++++++++++++++--- sdk_core/src/command_handler/command_impl.h | 1 + sdk_core/src/data_handler/data_handler.cpp | 8 +- sdk_core/src/device_discovery.cpp | 11 +- sdk_core/src/device_discovery.h | 6 +- sdk_core/src/device_manager.cpp | 22 ++- sdk_core/src/device_manager.h | 4 +- 23 files changed, 441 insertions(+), 113 deletions(-) diff --git a/Doxyfile b/Doxyfile index ce3e7b9..dc4d1d8 100644 --- a/Doxyfile +++ b/Doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "Livox SDK API" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = V2.1.1 +PROJECT_NUMBER = V2.2.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/README.md b/README.md index 14ecfd7..608ee0c 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,7 @@ The Livox LiDAR sensors can be connected to host directly or through the Livox H # 4 Livox SDK API -Livox SDK API provides a set of C style functions which can be conveniently integrated in C/C++ programs. Please refer to the [Livox SDK API Reference](https://livox-sdk.github.io/Livox-SDK/) for further information. +Livox SDK API provides a set of C style functions which can be conveniently integrated in C/C++ programs. Please refer to the [Livox SDK API Reference](https://livox-sdk.github.io/Livox-SDK-Doc/) for further information. ## 4.1 Installation The installation procedures in Ubuntu 18.04/16.04/14.04 LTS and Windows 7/10 are shown here as examples. For Ubuntu 18.04/16.04/14.04 32-bit LTS and Mac, you can get it in [Livox-SDK wiki](https://github.com/Livox-SDK/Livox-SDK/wiki). diff --git a/README_CN.md b/README_CN.md index 8401359..7571ce1 100644 --- a/README_CN.md +++ b/README_CN.md @@ -26,7 +26,7 @@ Livox LiDAR 传感器可以直接连接主机或者通过 Livox Hub 连接到主 # 4 Livox SDK 应用接口 -Livox SDK 应用接口提供了一系列 C 风格的接口，可以很方便地集成进 C/C++ 程序。请查阅 [Livox SDK API Reference](https://livox-sdk.github.io/Livox-SDK/) 获取更多信息。 +Livox SDK 应用接口提供了一系列 C 风格的接口，可以很方便地集成进 C/C++ 程序。请查阅 [Livox SDK API Reference](https://livox-sdk.github.io/Livox-SDK-Doc/) 获取更多信息。 ## 4.1 安装 以下将介绍 Ubuntu 18.04/16.04/14.04 LTS 系统、 Windows 7/10 系统和 ARM-Linux 交叉编译的安装过程. 对于 Ubuntu 18.04/16.04/14.04 32-bit LTS 和 Mac 系统, 您可以查阅 [Livox-SDK wiki](https://github.com/Livox-SDK/Livox-SDK/wiki)。 diff --git a/sample/hub/main.c b/sample/hub/main.c index 83f9006..bb56a86 100644 --- a/sample/hub/main.c +++ b/sample/hub/main.c @@ -100,6 +100,10 @@ void GetHubData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *c LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data; }else if ( data ->data_type == kImu) { LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data; + }else if ( data ->data_type == kTripleExtendCartesian) { + LivoxTripleExtendRawPoint *p_point_data = (LivoxTripleExtendRawPoint *)data->data; + }else if ( data ->data_type == kTripleExtendSpherical) { + LivoxTripleExtendSpherPoint *p_point_data = (LivoxTripleExtendSpherPoint *)data->data; } printf("data_type %d data num %d\n", data->data_type, data_num); } diff --git a/sample/hub_lvx_file/lvx_file.cpp b/sample/hub_lvx_file/lvx_file.cpp index 0dc7e8e..2190316 100644 --- a/sample/hub_lvx_file/lvx_file.cpp +++ b/sample/hub_lvx_file/lvx_file.cpp @@ -31,6 +31,7 @@ #define RAW_POINT_NUM 100 #define SINGLE_POINT_NUM 96 #define DUAL_POINT_NUM 48 +#define TRIPLE_POINT_NUM 30 #define IMU_POINT_NUM 1 #define M_PI 3.14159265358979323846 @@ -133,27 +134,45 @@ void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data, LvxBasePackDetail &pa packet.timestamp_type = data->timestamp_type; packet.data_type = data->data_type; memcpy(packet.timestamp, data->timestamp, 8 * sizeof(uint8_t)); - if (packet.data_type == 0) { - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point) - \ - sizeof(packet.pack_size) + RAW_POINT_NUM*sizeof(LivoxRawPoint); - memcpy(packet.raw_point,(void *)data->data,RAW_POINT_NUM*sizeof(LivoxRawPoint)); - } else if(packet.data_type == 1){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +RAW_POINT_NUM*sizeof(LivoxSpherPoint); - memcpy(packet.raw_point,(void *)data->data,RAW_POINT_NUM*sizeof(LivoxSpherPoint)); - } else if(packet.data_type == 2){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint); - memcpy(packet.raw_point,(void *)data->data,SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint)); - } else if(packet.data_type == 3){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint); - memcpy(packet.raw_point,(void *)data->data,SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint)); - } else if(packet.data_type == 4){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint); - memcpy(packet.raw_point,(void *)data->data,DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint)); - } else if(packet.data_type == 5){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint); - memcpy(packet.raw_point,(void *)data->data,DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint)); - } else if(packet.data_type == 6){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +IMU_POINT_NUM*sizeof(LivoxImuPoint); - memcpy(packet.raw_point,(void *)data->data,IMU_POINT_NUM*sizeof(LivoxImuPoint)); + switch (packet.data_type) { + case PointDataType::kCartesian: + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point) - \ + sizeof(packet.pack_size) + RAW_POINT_NUM*sizeof(LivoxRawPoint); + memcpy(packet.raw_point,(void *)data->data, RAW_POINT_NUM*sizeof(LivoxRawPoint)); + break; + case PointDataType::kSpherical : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +RAW_POINT_NUM*sizeof(LivoxSpherPoint); + memcpy(packet.raw_point,(void *)data->data, RAW_POINT_NUM*sizeof(LivoxSpherPoint)); + break; + case PointDataType::kExtendCartesian : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint); + memcpy(packet.raw_point,(void *)data->data, SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint)); + break; + case PointDataType::kExtendSpherical : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint); + memcpy(packet.raw_point,(void *)data->data, SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint)); + break; + case PointDataType::kDualExtendCartesian : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint); + memcpy(packet.raw_point,(void *)data->data, DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint)); + break; + case PointDataType::kDualExtendSpherical : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint); + memcpy(packet.raw_point,(void *)data->data, DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint)); + break; + case PointDataType::kImu : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +IMU_POINT_NUM*sizeof(LivoxImuPoint); + memcpy(packet.raw_point,(void *)data->data, IMU_POINT_NUM*sizeof(LivoxImuPoint)); + break; + case PointDataType::kTripleExtendCartesian : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendRawPoint); + memcpy(packet.raw_point,(void *)data->data, TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendRawPoint)); + break; + case PointDataType::kTripleExtendSpherical : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendSpherPoint); + memcpy(packet.raw_point,(void *)data->data, TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendSpherPoint)); + break; + default: + break; } } diff --git a/sample/lidar/main.c b/sample/lidar/main.c index c568aaf..22abcea 100644 --- a/sample/lidar/main.c +++ b/sample/lidar/main.c @@ -105,6 +105,10 @@ void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data; }else if ( data ->data_type == kImu) { LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data; + }else if ( data ->data_type == kTripleExtendCartesian) { + LivoxTripleExtendRawPoint *p_point_data = (LivoxTripleExtendRawPoint *)data->data; + }else if ( data ->data_type == kTripleExtendSpherical) { + LivoxTripleExtendSpherPoint *p_point_data = (LivoxTripleExtendSpherPoint *)data->data; } printf("data_type %d packet num %d\n", data->data_type, data_recveive_count[handle]); } diff --git a/sample/lidar_lvx_file/lvx_file.cpp b/sample/lidar_lvx_file/lvx_file.cpp index bcf61ad..fcc2021 100644 --- a/sample/lidar_lvx_file/lvx_file.cpp +++ b/sample/lidar_lvx_file/lvx_file.cpp @@ -33,6 +33,7 @@ #define RAW_POINT_NUM 100 #define SINGLE_POINT_NUM 96 #define DUAL_POINT_NUM 48 +#define TRIPLE_POINT_NUM 30 #define IMU_POINT_NUM 1 #define M_PI 3.14159265358979323846 @@ -135,27 +136,46 @@ void LvxFileHandle::BasePointsHandle(LivoxEthPacket *data, LvxBasePackDetail &pa packet.timestamp_type = data->timestamp_type; packet.data_type = data->data_type; memcpy(packet.timestamp, data->timestamp, 8 * sizeof(uint8_t)); - if (packet.data_type == 0) { - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) + RAW_POINT_NUM*sizeof(LivoxRawPoint); - memcpy(packet.raw_point,(void *)data->data,RAW_POINT_NUM*sizeof(LivoxRawPoint)); - } else if(packet.data_type == 1){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +RAW_POINT_NUM*sizeof(LivoxSpherPoint); - memcpy(packet.raw_point,(void *)data->data,RAW_POINT_NUM*sizeof(LivoxSpherPoint)); - } else if(packet.data_type == 2){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint); - memcpy(packet.raw_point,(void *)data->data,SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint)); - } else if(packet.data_type == 3){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint); - memcpy(packet.raw_point,(void *)data->data,SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint)); - } else if(packet.data_type == 4){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint); - memcpy(packet.raw_point,(void *)data->data,DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint)); - } else if(packet.data_type == 5){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint); - memcpy(packet.raw_point,(void *)data->data,DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint)); - } else if(packet.data_type == 6){ - packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +IMU_POINT_NUM*sizeof(LivoxImuPoint); - memcpy(packet.raw_point,(void *)data->data,IMU_POINT_NUM*sizeof(LivoxImuPoint)); + switch (packet.data_type) { + case PointDataType::kCartesian: + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point) - \ + sizeof(packet.pack_size) + RAW_POINT_NUM*sizeof(LivoxRawPoint); + memcpy(packet.raw_point,(void *)data->data, RAW_POINT_NUM*sizeof(LivoxRawPoint)); + break; + case PointDataType::kSpherical : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +RAW_POINT_NUM*sizeof(LivoxSpherPoint); + memcpy(packet.raw_point,(void *)data->data, RAW_POINT_NUM*sizeof(LivoxSpherPoint)); + break; + case PointDataType::kExtendCartesian : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint); + memcpy(packet.raw_point,(void *)data->data, SINGLE_POINT_NUM*sizeof(LivoxExtendRawPoint)); + break; + case PointDataType::kExtendSpherical : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint); + memcpy(packet.raw_point,(void *)data->data, SINGLE_POINT_NUM*sizeof(LivoxExtendSpherPoint)); + break; + case PointDataType::kDualExtendCartesian : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint); + memcpy(packet.raw_point,(void *)data->data, DUAL_POINT_NUM*sizeof(LivoxDualExtendRawPoint)); + break; + case PointDataType::kDualExtendSpherical : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint); + memcpy(packet.raw_point,(void *)data->data, DUAL_POINT_NUM*sizeof(LivoxDualExtendSpherPoint)); + break; + case PointDataType::kImu : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +IMU_POINT_NUM*sizeof(LivoxImuPoint); + memcpy(packet.raw_point,(void *)data->data, IMU_POINT_NUM*sizeof(LivoxImuPoint)); + break; + case PointDataType::kTripleExtendCartesian : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendRawPoint); + memcpy(packet.raw_point,(void *)data->data, TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendRawPoint)); + break; + case PointDataType::kTripleExtendSpherical : + packet.pack_size = sizeof(LvxBasePackDetail) - sizeof(packet.raw_point)- sizeof(packet.pack_size) +TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendSpherPoint); + memcpy(packet.raw_point,(void *)data->data, TRIPLE_POINT_NUM*sizeof(LivoxTripleExtendSpherPoint)); + break; + default: + break; } } diff --git a/sample/lidar_lvx_file/main.cpp b/sample/lidar_lvx_file/main.cpp index 3f7bb38..f749126 100644 --- a/sample/lidar_lvx_file/main.cpp +++ b/sample/lidar_lvx_file/main.cpp @@ -140,6 +140,7 @@ void LidarGetExtrinsicFromXml(uint8_t handle) { LvxDeviceInfo lidar_info; ParseExtrinsicXml(devices[handle], lidar_info); lvx_file_handler.AddDeviceInfo(lidar_info); + lidar_info.extrinsic_enable = true; if (lvx_file_handler.GetDeviceInfoListSize() == broadcast_code_list.size()) { is_finish_extrinsic_parameter = true; extrinsic_condition.notify_one(); diff --git a/sample_cc/hub/lds_hub.cpp b/sample_cc/hub/lds_hub.cpp index 999cc7e..a821748 100644 --- a/sample_cc/hub/lds_hub.cpp +++ b/sample_cc/hub/lds_hub.cpp @@ -175,6 +175,10 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data, LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data; }else if ( data ->data_type == kImu) { LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data; + }else if ( data ->data_type == kTripleExtendCartesian) { + LivoxTripleExtendRawPoint *p_point_data = (LivoxTripleExtendRawPoint *)data->data; + }else if ( data ->data_type == kTripleExtendSpherical) { + LivoxTripleExtendSpherPoint *p_point_data = (LivoxTripleExtendSpherPoint *)data->data; } } } @@ -517,7 +521,7 @@ void LdsHub::AddLocalBroadcastCode(void) { for (size_t i = 0; i < sizeof(local_broadcast_code_list)/sizeof(intptr_t); ++i) { std::string invalid_bd = "000000000"; printf("Local broadcast code : %s\n", local_broadcast_code_list[i]); - if ((kBroadcastCodeSize == strlen(local_broadcast_code_list[i]) - 1) && \ + if ((kBroadcastCodeSize == strlen(local_broadcast_code_list[i]) + 1) && \ (nullptr == strstr(local_broadcast_code_list[i], invalid_bd.c_str()))) { AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]); } else { diff --git a/sample_cc/lidar/lds_lidar.cpp b/sample_cc/lidar/lds_lidar.cpp index 31dfc1f..93c9e84 100644 --- a/sample_cc/lidar/lds_lidar.cpp +++ b/sample_cc/lidar/lds_lidar.cpp @@ -167,6 +167,10 @@ void LdsLidar::GetLidarDataCb(uint8_t handle, LivoxEthPacket *data, LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data; }else if ( data ->data_type == kImu) { LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data; + }else if ( data ->data_type == kTripleExtendCartesian) { + LivoxTripleExtendRawPoint *p_point_data = (LivoxTripleExtendRawPoint *)data->data; + }else if ( data ->data_type == kTripleExtendSpherical) { + LivoxTripleExtendSpherPoint *p_point_data = (LivoxTripleExtendSpherPoint *)data->data; } } } @@ -261,7 +265,7 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) { p_lidar->config.set_bits |= kConfigReturnMode; } - if (kDeviceTypeLidarMid40 != info->type) { + if (kDeviceTypeLidarMid40 != info->type && kDeviceTypeLidarMid70 != info->type) { LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\ LdsLidar::SetImuRatePushFrequencyCb, g_lidars); p_lidar->config.set_bits |= kConfigImuRate; @@ -443,7 +447,7 @@ void LdsLidar::AddLocalBroadcastCode(void) { for (size_t i=0; i apr_time_from_msec(50)) { + TimePoint t = steady_clock::now();; + if (last_timeout_ == TimePoint() || t - last_timeout_ > std::chrono::milliseconds(50)) { last_timeout_ = t; // copy delegates_ in case delegate is removed in callback. DelegatesType delegates = delegates_; diff --git a/sdk_core/src/base/io_loop.h b/sdk_core/src/base/io_loop.h index f90e2ec..5bad545 100644 --- a/sdk_core/src/base/io_loop.h +++ b/sdk_core/src/base/io_loop.h @@ -43,10 +43,11 @@ namespace livox { class IOLoop : public noncopyable { public: + typedef std::chrono::steady_clock::time_point TimePoint; class IOLoopDelegate { public: virtual void OnData(apr_socket_t *, void *) {} - virtual void OnTimer(apr_time_t) {} + virtual void OnTimer(TimePoint) {} virtual void OnWake() {} }; @@ -54,7 +55,7 @@ class IOLoop : public noncopyable { public: explicit IOLoop(apr_pool_t *mem_pool, bool enable_timer = true, bool enable_wake = true) - : pollset_(NULL), mem_pool_(mem_pool), last_timeout_(0), enable_timer_(enable_timer), enable_wake_(enable_wake){}; + : pollset_(NULL), mem_pool_(mem_pool), last_timeout_(), enable_timer_(enable_timer), enable_wake_(enable_wake){}; bool Init(); void Uninit(); @@ -78,7 +79,7 @@ class IOLoop : public noncopyable { DelegatesType delegates_; apr_pollset_t *pollset_; apr_pool_t *mem_pool_; - apr_time_t last_timeout_; + TimePoint last_timeout_; std::mutex mutex_; bool enable_timer_; bool enable_wake_; diff --git a/sdk_core/src/command_handler/command_channel.cpp b/sdk_core/src/command_handler/command_channel.cpp index 0e864f7..d85a4dd 100644 --- a/sdk_core/src/command_handler/command_channel.cpp +++ b/sdk_core/src/command_handler/command_channel.cpp @@ -37,6 +37,7 @@ using std::make_pair; using std::map; using std::pair; using std::string; +using std::chrono::steady_clock; namespace livox { @@ -52,9 +53,9 @@ CommandChannel::CommandChannel(apr_port_t port, loop_(NULL), callback_(cb), comm_port_(new CommPort), - heartbeat_time_(0), + heartbeat_time_(), remote_ip_(remote_ip), - last_heartbeat_(0) {} + last_heartbeat_() {} bool CommandChannel::Bind(IOLoop *loop) { if (loop == NULL) { @@ -67,6 +68,7 @@ bool CommandChannel::Bind(IOLoop *loop) { } loop_->AddDelegate(sock_, this); + last_heartbeat_ = steady_clock::now(); return true; } @@ -118,11 +120,11 @@ void CommandChannel::SendAsync(const Command &command) { } } -void CommandChannel::OnTimer(apr_time_t now) { +void CommandChannel::OnTimer(TimePoint now) { list timeout_commands; - map >::iterator ite = commands_.begin(); + map >::iterator ite = commands_.begin(); while (ite != commands_.end()) { - pair &command_pair = ite->second; + pair &command_pair = ite->second; if (now > command_pair.second) { timeout_commands.push_back(command_pair.first); commands_.erase(ite++); @@ -132,7 +134,7 @@ void CommandChannel::OnTimer(apr_time_t now) { } for (list::iterator ite = timeout_commands.begin(); ite != timeout_commands.end(); ++ite) { - LOG_WARN("Apr time now: {}, Command Timeout: Set {}, Id {}, Seq {}", PrintAPRTime(apr_time_now()), + LOG_WARN("Command Timeout: Set {}, Id {}, Seq {}", (uint16_t)ite->packet.cmd_set, ite->packet.cmd_code, ite->packet.seq_num); if (callback_) { ite->packet.packet_type = kCommandTypeAck; @@ -140,7 +142,7 @@ void CommandChannel::OnTimer(apr_time_t now) { } } - if ((last_heartbeat_ != 0) && (now - last_heartbeat_ > apr_time_from_sec(3))) { + if (now - last_heartbeat_ > std::chrono::seconds(3)) { DeviceDisconnect(handle_); } else { HeartBeat(now); @@ -165,13 +167,13 @@ void CommandChannel::Uninit() { } commands_.clear(); - last_heartbeat_ = 0; - heartbeat_time_ = 0; + last_heartbeat_ = {}; + heartbeat_time_ = {}; remote_ip_ = ""; } -void CommandChannel::HeartBeat(apr_time_t t) { - if (heartbeat_time_ == 0 || (t - heartbeat_time_) > apr_time_from_msec(kHeartbeatTimer)) { +void CommandChannel::HeartBeat(TimePoint t) { + if (heartbeat_time_ == TimePoint() || (t - heartbeat_time_) > kHeartbeatTimer) { heartbeat_time_ = t; Command command(handle_, @@ -230,7 +232,7 @@ Command CommandChannel::DeepCopy(const Command &cmd) { } void CommandChannel::OnHeartbeatAck(const CommPacket &) { - last_heartbeat_ = apr_time_now(); + last_heartbeat_ = steady_clock::now(); } void CommandChannel::DeviceDisconnect(uint8_t handle) { @@ -240,7 +242,7 @@ void CommandChannel::DeviceDisconnect(uint8_t handle) { void CommandChannel::Send(const Command &command) { LOG_INFO(" Send Command: Set {} Id {} Seq {}", (uint16_t)command.packet.cmd_set, command.packet.cmd_code, command.packet.seq_num); SendInternal(command); - commands_[command.packet.seq_num] = make_pair(command, apr_time_now() + apr_time_from_msec(command.time_out)); + commands_[command.packet.seq_num] = make_pair(command, steady_clock::now() + std::chrono::milliseconds(command.time_out)); Command &cmd = commands_[command.packet.seq_num].first; if (cmd.packet.data != NULL) { delete[] cmd.packet.data; diff --git a/sdk_core/src/command_handler/command_channel.h b/sdk_core/src/command_handler/command_channel.h index 4089370..fc3f00b 100644 --- a/sdk_core/src/command_handler/command_channel.h +++ b/sdk_core/src/command_handler/command_channel.h @@ -28,6 +28,7 @@ #include #include #include +#include #include "apr_network_io.h" #include "base/io_loop.h" #include "comm/comm_port.h" @@ -71,6 +72,8 @@ class CommandChannelDelegate { */ class CommandChannel : public IOLoop::IOLoopDelegate { public: + typedef std::chrono::steady_clock::time_point TimePoint; + CommandChannel(apr_port_t port, uint8_t handle, const std::string &remote_ip, @@ -95,31 +98,31 @@ class CommandChannel : public IOLoop::IOLoopDelegate { void SendAsync(const Command &command); void OnData(apr_socket_t *, void *); - void OnTimer(apr_time_t now); + void OnTimer(TimePoint now); static uint16_t GenerateSeq(); private: void Send(const Command &cmd); - void HeartBeat(apr_time_t t); + void HeartBeat(TimePoint t); void SendInternal(const Command &command); Command DeepCopy(const Command &cmd); void OnHeartbeatAck(const CommPacket &packet); void DeviceDisconnect(uint8_t handle); private: - static const int kHeartbeatTimer = 800; + const std::chrono::milliseconds kHeartbeatTimer = std::chrono::milliseconds(800); uint8_t handle_; apr_port_t port_; apr_socket_t *sock_; apr_pool_t *mem_pool_; IOLoop *loop_; CommandChannelDelegate *callback_; - std::map > commands_; + std::map > commands_; std::unique_ptr comm_port_; - apr_time_t heartbeat_time_; + TimePoint heartbeat_time_; std::string remote_ip_; - apr_time_t last_heartbeat_; + TimePoint last_heartbeat_; }; } // namespace livox diff --git a/sdk_core/src/command_handler/command_impl.cpp b/sdk_core/src/command_handler/command_impl.cpp index 10b95b3..ddc34a1 100644 --- a/sdk_core/src/command_handler/command_impl.cpp +++ b/sdk_core/src/command_handler/command_impl.cpp @@ -158,7 +158,7 @@ bool ParseRmcTime(const char* rmc, uint16_t rmc_len, LidarSetUtcSyncTimeRequest* char utc_hms_buff[HMS_PARSE_BUF] = { 0 }; char utc_yy_buff[YY_PARSE_BUF] = { 0 }; - int hour = 0, minute = 0, second = 0, millisecond = 0; + int hour = 0, minute = 0, second = 0; memset(utc_time_req, 0, sizeof(LidarSetUtcSyncTimeRequest)); @@ -204,23 +204,10 @@ bool ParseRmcTime(const char* rmc, uint16_t rmc_len, LidarSetUtcSyncTimeRequest* return false; } - time_buff_size = strlen(utc_hms_buff); - switch (time_buff_size) { - case sizeof("hhmmss")-1: - if (3 != sscanf(utc_hms_buff, "%2d%2d%2d", &hour, &minute, &second)) { - return false; - } - break; - case sizeof("hhmmss.s")-1: - case sizeof("hhmmss.ss")-1: - case sizeof("hhmmss.sss")-1: - if (4 != sscanf(utc_hms_buff, "%2d%2d%2d.%2d", &hour, &minute, &second, &millisecond)) { - return false; - } - break; - default: - return false; + if (3 != sscanf(utc_hms_buff, "%2d%2d%2d", &hour, &minute, &second)) { + return false; } + utc_time_req->hour = hour; utc_time_req->mircrosecond = (minute * 60 * 1000 + second * 1000) * 1000; return true; @@ -391,7 +378,8 @@ livox_status RebootDevice(uint8_t handle, uint16_t timeout, CommonCommandCallbac } livox_status LidarEnableHighSensitivity(uint8_t handle, SetDeviceParametersCallback cb, void *client_data) { - if (!device_manager().IsLidarTele(handle)) { + if (!device_manager().IsLidarTele(handle) + && !device_manager().IsLidarAvia(handle)) { return kStatusNotSupported; } @@ -407,7 +395,8 @@ livox_status LidarEnableHighSensitivity(uint8_t handle, SetDeviceParametersCallb } livox_status LidarDisableHighSensitivity(uint8_t handle, SetDeviceParametersCallback cb, void *client_data) { - if (!device_manager().IsLidarTele(handle)) { + if (!device_manager().IsLidarTele(handle) + && !device_manager().IsLidarAvia(handle)) { return kStatusNotSupported; } uint8_t req_buff[kMaxCommandBufferSize] = {0}; @@ -422,7 +411,8 @@ livox_status LidarDisableHighSensitivity(uint8_t handle, SetDeviceParametersCall } livox_status LidarGetHighSensitivityState(uint8_t handle, GetDeviceParametersCallback cb, void *client_data) { - if (!device_manager().IsLidarTele(handle)) { + if (!device_manager().IsLidarTele(handle) + && !device_manager().IsLidarAvia(handle)) { return kStatusNotSupported; } @@ -432,6 +422,101 @@ livox_status LidarGetHighSensitivityState(uint8_t handle, GetDeviceParametersCal return GetDeiveParameter(handle, &req, cb, client_data); } +livox_status LidarSetSlotNum(uint8_t handle, uint8_t slot, SetDeviceParametersCallback cb, void *client_data) { + if (!device_manager().IsLidarMid70(handle) + && !device_manager().IsLidarAvia(handle)) { + return kStatusNotSupported; + } + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + KeyValueParam * kv = (KeyValueParam *)&req_buff[0]; + kv->key = static_cast(kKeySlotNum); + kv->length = 1; + kv->value[0] = slot; + req_len = sizeof(KeyValueParam); + + return SetDeviceParameters(handle, req_buff, req_len, cb, client_data); +} + +livox_status LidarGetSlotNum(uint8_t handle, GetDeviceParametersCallback cb, void *client_data) { + if (!device_manager().IsLidarMid70(handle) + && !device_manager().IsLidarAvia(handle)) { + return kStatusNotSupported; + } + + GetDeviceParameterRequest req; + req.param_num = 1; + req.key[0] = static_cast(kKeySlotNum); + return GetDeiveParameter(handle, &req, cb, client_data); +} + +livox_status LidarGetScanPattern(uint8_t handle, GetDeviceParametersCallback cb, void *client_data) { + if (!device_manager().IsLidarAvia(handle)) { + return kStatusNotSupported; + } + GetDeviceParameterRequest req; + req.param_num = 1; + req.key[0] = static_cast(kKeyScanPattern); + return GetDeiveParameter(handle, &req, cb, client_data); +} + +livox_status LidarSetScanPattern(uint8_t handle, LidarScanPattern pattern, SetDeviceParametersCallback cb, void *client_data) { + if (!device_manager().IsLidarAvia(handle)) { + return kStatusNotSupported; + } + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + KeyValueParam * kv = (KeyValueParam *)&req_buff[0]; + kv->key = static_cast(kKeyScanPattern); + kv->length = 1; + kv->value[0] = static_cast(pattern); + req_len = sizeof(KeyValueParam); + return SetDeviceParameters(handle, req_buff, req_len, cb, client_data); +} + +livox_status DeviceResetAllParameters(uint8_t handle, DeviceResetParametersCallback cb, void *client_data) { + if (!device_manager().IsLidarAvia(handle) + && !device_manager().IsLidarMid70(handle) + && !device_manager().IsLidarTele(handle)) { + return kStatusNotSupported; + } + ResetDeviceParameterRequest req = {}; + req.flag = 0; //Reset all keys + livox_status result = command_handler().SendCommand(handle, + kCommandSetGeneral, + kCommandIDGeneralResetDeviceParam, + (uint8_t *)&req, + sizeof(req), + MakeCommandCallback(cb, client_data)); + return result; +} + +livox_status DeviceResetParameters(uint8_t handle, DeviceParamKeyName * keys, uint8_t keys_num, DeviceResetParametersCallback cb, void *client_data) { + if (!device_manager().IsLidarAvia(handle) + && !device_manager().IsLidarMid70(handle) + && !device_manager().IsLidarTele(handle)) { + return kStatusNotSupported; + } + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + ResetDeviceParameterRequest * req = (ResetDeviceParameterRequest *)&req_buff[0]; + req->flag = 1; //Reset some keys + req->key_num = keys_num; + for (uint8_t i = 0; i < keys_num; i++) { + req->key[i] = static_cast(keys[i]); + } + req_len = sizeof(ResetDeviceParameterRequest) + (keys_num - 1) * sizeof(uint16_t); + + livox_status result = command_handler().SendCommand(handle, + kCommandSetGeneral, + kCommandIDGeneralResetDeviceParam, + (uint8_t *)req, + req_len, + MakeCommandCallback(cb, client_data)); + return result; +} + + livox_status LidarSetMode(uint8_t handle, LidarMode mode, CommonCommandCallback cb, void *client_data) { if (device_manager().device_mode() != kDeviceModeLidar) { return kStatusNotSupported; @@ -492,7 +577,9 @@ livox_status LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCall livox_status LidarTurnOnFan(uint8_t handle, CommonCommandCallback cb, void *client_data) { if (device_manager().device_mode() != kDeviceModeLidar - || device_manager().IsLidarMid40(handle)) { + || device_manager().IsLidarMid40(handle) + || device_manager().IsLidarMid70(handle) + || device_manager().IsLidarAvia(handle)) { return kStatusNotSupported; } return LidarFanControl(handle, true, cb, client_data); @@ -500,7 +587,9 @@ livox_status LidarTurnOnFan(uint8_t handle, CommonCommandCallback cb, void *clie livox_status LidarTurnOffFan(uint8_t handle, CommonCommandCallback cb, void *client_data) { if (device_manager().device_mode() != kDeviceModeLidar - || device_manager().IsLidarMid40(handle)) { + || device_manager().IsLidarMid40(handle) + || device_manager().IsLidarMid70(handle) + || device_manager().IsLidarAvia(handle)) { return kStatusNotSupported; } return LidarFanControl(handle, false, cb, client_data); @@ -508,7 +597,9 @@ livox_status LidarTurnOffFan(uint8_t handle, CommonCommandCallback cb, void *cli livox_status LidarGetFanState(uint8_t handle, LidarGetFanStateCallback cb, void * data) { if (device_manager().device_mode() != kDeviceModeLidar - || device_manager().IsLidarMid40(handle)) { + || device_manager().IsLidarMid40(handle) + || device_manager().IsLidarMid70(handle) + || device_manager().IsLidarAvia(handle)) { return kStatusNotSupported; } livox_status result = command_handler().SendCommand(handle, @@ -551,7 +642,8 @@ livox_status LidarGetPointCloudReturnMode(uint8_t handle, LidarGetPointCloudRetu livox_status LidarSetImuPushFrequency(uint8_t handle, ImuFreq freq, CommonCommandCallback cb, void * client_data) { if (device_manager().device_mode() != kDeviceModeLidar - || device_manager().IsLidarMid40(handle)) { + || device_manager().IsLidarMid40(handle) + || device_manager().IsLidarMid70(handle)) { return kStatusNotSupported; } uint8_t req = static_cast(freq); @@ -566,7 +658,8 @@ livox_status LidarSetImuPushFrequency(uint8_t handle, ImuFreq freq, CommonComman livox_status LidarGetImuPushFrequency(uint8_t handle, LidarGetImuPushFrequencyCallback cb, void * data) { if (device_manager().device_mode() != kDeviceModeLidar - || device_manager().IsLidarMid40(handle)) { + || device_manager().IsLidarMid40(handle) + || device_manager().IsLidarMid70(handle)) { return kStatusNotSupported; } livox_status result = command_handler().SendCommand(handle, diff --git a/sdk_core/src/command_handler/command_impl.h b/sdk_core/src/command_handler/command_impl.h index dd19c3f..627152e 100644 --- a/sdk_core/src/command_handler/command_impl.h +++ b/sdk_core/src/command_handler/command_impl.h @@ -115,6 +115,7 @@ static const uint16_t GeneralCommandTimeout[kCommandIDGeneralCommandCount] = {KD KDefaultTimeOut, KDefaultTimeOut, KDefaultTimeOut, + KDefaultTimeOut, KDefaultTimeOut,}; /** Enum that represents the command id. */ diff --git a/sdk_core/src/data_handler/data_handler.cpp b/sdk_core/src/data_handler/data_handler.cpp index fa526e4..3d81000 100644 --- a/sdk_core/src/data_handler/data_handler.cpp +++ b/sdk_core/src/data_handler/data_handler.cpp @@ -111,7 +111,13 @@ void DataHandler::OnDataCallback(uint8_t handle, void *data, uint16_t size) { case kImu: size = (size - kPrefixDataSize) / sizeof(LivoxImuPoint); break; - default: + case kTripleExtendCartesian: + size = (size - kPrefixDataSize) / sizeof(LivoxTripleExtendRawPoint); + break; + case kTripleExtendSpherical: + size = (size - kPrefixDataSize) / sizeof(LivoxTripleExtendSpherPoint); + break; + default: return; } if (cb) { diff --git a/sdk_core/src/device_discovery.cpp b/sdk_core/src/device_discovery.cpp index 6933e49..800ea72 100644 --- a/sdk_core/src/device_discovery.cpp +++ b/sdk_core/src/device_discovery.cpp @@ -43,6 +43,7 @@ using std::tuple; using std::string; using std::vector; +using std::chrono::steady_clock; namespace livox { @@ -119,11 +120,11 @@ void DeviceDiscovery::OnData(apr_socket_t *sock, void *) { } } -void DeviceDiscovery::OnTimer(apr_time_t now) { +void DeviceDiscovery::OnTimer(TimePoint now) { ConnectingDeviceMap::iterator ite = connecting_devices_.begin(); while (ite != connecting_devices_.end()) { - tuple &device_tuple = ite->second; - if (now - std::get<1>(device_tuple) > apr_time_from_msec(500)) { + tuple &device_tuple = ite->second; + if (now - std::get<1>(device_tuple) > std::chrono::milliseconds(500)) { loop_->RemoveDelegate(ite->first, this); apr_socket_close(ite->first); apr_pool_destroy(std::get<0>(device_tuple)); @@ -209,7 +210,7 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr } loop_->AddDelegate(cmd_sock, this); - OnTimer(apr_time_now()); + OnTimer(steady_clock::now()); std::get<0>(connecting_devices_[cmd_sock]) = pool; std::get<2>(connecting_devices_[cmd_sock]) = lidar_info; @@ -246,7 +247,7 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr result = false; break; } - std::get<1>(connecting_devices_[cmd_sock]) = apr_time_now(); + std::get<1>(connecting_devices_[cmd_sock]) = steady_clock::now(); result = true; } while (0); diff --git a/sdk_core/src/device_discovery.h b/sdk_core/src/device_discovery.h index 357a09c..3620905 100644 --- a/sdk_core/src/device_discovery.h +++ b/sdk_core/src/device_discovery.h @@ -43,6 +43,8 @@ namespace livox { */ class DeviceDiscovery : public noncopyable, IOLoop::IOLoopDelegate { public: + typedef std::chrono::steady_clock::time_point TimePoint; + DeviceDiscovery() : sock_(NULL), mem_pool_(NULL), loop_(NULL), comm_port_(nullptr) {} bool Init(); void Uninit(); @@ -59,7 +61,7 @@ class DeviceDiscovery : public noncopyable, IOLoop::IOLoopDelegate { * @param client_data client data passed in IOLoop::AddDelegate */ void OnData(apr_socket_t *, void *client_data); - void OnTimer(apr_time_t now); + void OnTimer(TimePoint now); private: void OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr); @@ -81,7 +83,7 @@ class DeviceDiscovery : public noncopyable, IOLoop::IOLoopDelegate { IOLoop *loop_; std::unique_ptr comm_port_; std::mutex mutex_; - typedef std::map > ConnectingDeviceMap; + typedef std::map > ConnectingDeviceMap; ConnectingDeviceMap connecting_devices_; }; diff --git a/sdk_core/src/device_manager.cpp b/sdk_core/src/device_manager.cpp index 7339a38..263ee71 100644 --- a/sdk_core/src/device_manager.cpp +++ b/sdk_core/src/device_manager.cpp @@ -43,7 +43,9 @@ inline bool IsHub(uint8_t mode) { } inline bool IsLidar(uint8_t mode) { - return mode == kDeviceTypeLidarTele || mode == kDeviceTypeLidarMid40 || mode == kDeviceTypeLidarHorizon; + return mode == kDeviceTypeLidarTele || mode == kDeviceTypeLidarMid40 + || mode == kDeviceTypeLidarHorizon || mode == kDeviceTypeLidarMid70 + || mode == kDeviceTypeLidarAvia; } bool DeviceManager::Init() { @@ -301,6 +303,24 @@ bool DeviceManager::IsLidarMid40(uint8_t handle) { return false; } +bool DeviceManager::IsLidarMid70(uint8_t handle) { + DeviceInfo lidar_info; + bool found = device_manager().FindDevice(handle, lidar_info); + if ( found && lidar_info.type == kDeviceTypeLidarMid70) { + return true; + } + return false; +} + +bool DeviceManager::IsLidarAvia(uint8_t handle) { + DeviceInfo lidar_info; + bool found = device_manager().FindDevice(handle, lidar_info); + if ( found && lidar_info.type == kDeviceTypeLidarAvia) { + return true; + } + return false; +} + bool DeviceManager::IsLidarTele(uint8_t handle) { DeviceInfo lidar_info; bool found = device_manager().FindDevice(handle, lidar_info); diff --git a/sdk_core/src/device_manager.h b/sdk_core/src/device_manager.h index c3977da..d4f0e7f 100644 --- a/sdk_core/src/device_manager.h +++ b/sdk_core/src/device_manager.h @@ -103,7 +103,9 @@ class DeviceManager : public noncopyable { void GetConnectedDevices(std::vector &devices); bool AddListeningDevice(const std::string &broadcast_code, DeviceMode mode, uint8_t &handle); bool IsLidarMid40(uint8_t handle); + bool IsLidarMid70(uint8_t handle); bool IsLidarTele(uint8_t handle); + bool IsLidarAvia(uint8_t handle); bool GetLidarFirmwareVersion(uint8_t handle, uint32_t &firmware_version); private: @@ -160,4 +162,4 @@ void DeviceRemove(uint8_t handle,DeviceEvent device_event); } // namespace livox -#endif // LIVOX_DEVICE_MANAGER_H_ \ No newline at end of file +#endif // LIVOX_DEVICE_MANAGER_H_