From 6a940156dd7151c3ab6a52442d86bc83613bd11b Mon Sep 17 00:00:00 2001 From: "louie.xu" Date: Sun, 18 Feb 2024 16:33:12 +0800 Subject: [PATCH] Support saving debug point cloud of Hap. Support setting the default work mode after boot of Hap. --- CHANGELOG.md | 5 +++++ include/livox_lidar_api.h | 9 ++++++++ include/livox_lidar_def.h | 13 ++++++++--- .../livox_lidar_rmc_time_sync/win/synchro.cpp | 8 +++---- sdk_core/comm/define.h | 1 + sdk_core/command_handler/command_impl.cpp | 22 +++++++++++++++++++ sdk_core/command_handler/command_impl.h | 2 ++ sdk_core/device_manager.cpp | 2 +- sdk_core/livox_lidar_sdk.cpp | 4 ++++ 9 files changed, 58 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d072bc8..85e133a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,11 @@ # Changelog All notable changes to Livox-SDK2 will be documentd in this file. +## [1.2.5] +### Added + +- Support saving debug point cloud of Hap. +- Support setting the default work mode after boot of Hap. ## [1.2.4] ### Added diff --git a/include/livox_lidar_api.h b/include/livox_lidar_api.h index abf0b80..3128642 100644 --- a/include/livox_lidar_api.h +++ b/include/livox_lidar_api.h @@ -446,6 +446,15 @@ livox_status SetLivoxLidarDebugPointCloud(uint32_t handle, bool enable, LivoxLid */ livox_status SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t rmc_length, LivoxLidarRmcSyncTimeCallBack cb, void* client_data); +/** + * Set LiDAR work mode after boot. + * @param handle device handle. + * @param work_mode lidar work mode after boot. + * @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 code. + */ +livox_status SetLivoxLidarWorkModeAfterBoot(const uint32_t handle,const LivoxLidarWorkModeAfterBoot work_mode, LivoxLidarAsyncControlCallback cb, void* client_data); /*******Upgrade Module***********/ /** diff --git a/include/livox_lidar_def.h b/include/livox_lidar_def.h index ed5e45b..16e10f3 100644 --- a/include/livox_lidar_def.h +++ b/include/livox_lidar_def.h @@ -31,9 +31,9 @@ #pragma pack(1) -#define LIVOX_LIDAR_SDK_MAJOR_VERSION 3 -#define LIVOX_LIDAR_SDK_MINOR_VERSION 1 -#define LIVOX_LIDAR_SDK_PATCH_VERSION 1 +#define LIVOX_LIDAR_SDK_MAJOR_VERSION 1 +#define LIVOX_LIDAR_SDK_MINOR_VERSION 2 +#define LIVOX_LIDAR_SDK_PATCH_VERSION 5 #define kBroadcastCodeSize 16 @@ -90,6 +90,7 @@ typedef enum { kKeyFovCfgEn = 0x0017, kKeyDetectMode = 0x0018, kKeyFuncIoCfg = 0x0019, + kKeyWorkModeAfterBoot = 0x0020, kKeyWorkMode = 0x001A, kKeyGlassHeat = 0x001B, kKeyImuDataEn = 0x001C, @@ -255,6 +256,12 @@ typedef enum { kLivoxLidarUpgrade = 0x08 } LivoxLidarWorkMode; +typedef enum { + kLivoxLidarWorkModeAfterBootDefault = 0x00, + kLivoxLidarWorkModeAfterBootNormal = 0x01, + kLivoxLidarWorkModeAfterBootWakeUp = 0x02 +} LivoxLidarWorkModeAfterBoot; + typedef struct { float roll_deg; float pitch_deg; diff --git a/samples/livox_lidar_rmc_time_sync/win/synchro.cpp b/samples/livox_lidar_rmc_time_sync/win/synchro.cpp index a5318e9..7a94f43 100644 --- a/samples/livox_lidar_rmc_time_sync/win/synchro.cpp +++ b/samples/livox_lidar_rmc_time_sync/win/synchro.cpp @@ -29,7 +29,7 @@ Synchro::Synchro() { hfile_ = INVALID_HANDLE_VALUE; - is_quite_ = false; + is_quit_ = false; rmc_buff_.resize(128,0); } @@ -53,13 +53,13 @@ bool Synchro::Start() { if (!Open()) { return false; } - is_quite_ = false; + is_quit_ = false; listener_ = std::make_shared(&Synchro::IOLoop, this); return true; } void Synchro::Stop() { - is_quite_ = true; + is_quit_ = true; if (listener_) { listener_->join(); listener_ = nullptr; @@ -70,7 +70,7 @@ void Synchro::Stop() { void Synchro::IOLoop() { uint8_t buff[READ_BUF]; size_t size = 0; - while (!is_quite_) { + while (!is_quit_) { if (0 < (size = Read(buff, READ_BUF))) { Decode(buff, size); } diff --git a/sdk_core/comm/define.h b/sdk_core/comm/define.h index 6a9fa29..66996bf 100644 --- a/sdk_core/comm/define.h +++ b/sdk_core/comm/define.h @@ -181,6 +181,7 @@ static const uint16_t kHAPPushMsgPort = 56000; static const uint16_t kHAPPointDataPort = 57000; static const uint16_t kHAPIMUPort = 58000; static const uint16_t kHAPLogPort = 59000; +static const uint16_t kHAPDebugPointCloudPort = 60000; /** kLogPort, which is the log port to be banned. */ static const uint16_t kLogPort = 0; diff --git a/sdk_core/command_handler/command_impl.cpp b/sdk_core/command_handler/command_impl.cpp index 0d860b2..d45391b 100644 --- a/sdk_core/command_handler/command_impl.cpp +++ b/sdk_core/command_handler/command_impl.cpp @@ -759,6 +759,28 @@ livox_status CommandImpl::SetLivoxLidarRmcSyncTime(uint32_t handle, const char* MakeCommandCallback(cb, client_data)); } +livox_status CommandImpl::SetLivoxLidarWorkModeAfterBoot(uint32_t handle, LivoxLidarWorkModeAfterBoot work_mode, LivoxLidarAsyncControlCallback cb, void* client_data) { + uint8_t req_buff[kMaxCommandBufferSize] = {0}; + uint16_t req_len = 0; + + uint16_t key_num = 1; + memcpy(&req_buff[req_len], &key_num, sizeof(key_num)); + req_len = sizeof(key_num) + sizeof(uint16_t); + + LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len]; + kv->key = static_cast(kKeyWorkModeAfterBoot); + kv->length = sizeof(uint8_t); + uint8_t* val_work_mode = reinterpret_cast(&kv->value[0]); + *val_work_mode = work_mode; + req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint8_t); + + return GeneralCommandHandler::GetInstance().SendCommand(handle, + kCommandIDLidarWorkModeControl, + req_buff, + req_len, + MakeCommandCallback(cb, client_data)); +} + // Upgrade livox_status CommandImpl::LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length, LivoxLidarStartUpgradeCallback cb, void* client_data) { diff --git a/sdk_core/command_handler/command_impl.h b/sdk_core/command_handler/command_impl.h index 466f894..2677716 100644 --- a/sdk_core/command_handler/command_impl.h +++ b/sdk_core/command_handler/command_impl.h @@ -110,6 +110,8 @@ class CommandImpl { static livox_status SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t rmc_length, LivoxLidarRmcSyncTimeCallBack cb, void* client_data); + static livox_status SetLivoxLidarWorkModeAfterBoot(uint32_t handle, LivoxLidarWorkModeAfterBoot work_mode, LivoxLidarAsyncControlCallback cb, void* client_data); + /*******Upgrade Module***********/ static livox_status LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, void* client_data); diff --git a/sdk_core/device_manager.cpp b/sdk_core/device_manager.cpp index e28101e..7e40a44 100644 --- a/sdk_core/device_manager.cpp +++ b/sdk_core/device_manager.cpp @@ -473,7 +473,7 @@ void DeviceManager::OnData(socket_t sock, void *client_data) { return; } - if (port == kMid360LidarDebugPointCloudPort) { + if (port == kMid360LidarDebugPointCloudPort || port == kHAPDebugPointCloudPort) { DebugPointCloudManager::GetInstance().Handler(handle, port, (uint8_t*)(buf.get()), size); } diff --git a/sdk_core/livox_lidar_sdk.cpp b/sdk_core/livox_lidar_sdk.cpp index 165a5bd..0ffa94c 100644 --- a/sdk_core/livox_lidar_sdk.cpp +++ b/sdk_core/livox_lidar_sdk.cpp @@ -298,6 +298,10 @@ livox_status SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t return CommandImpl::SetLivoxLidarRmcSyncTime(handle, rmc, rmc_length, cb, client_data); } +livox_status SetLivoxLidarWorkModeAfterBoot(const uint32_t handle,const LivoxLidarWorkModeAfterBoot work_mode, LivoxLidarAsyncControlCallback cb, void* client_data){ + return CommandImpl::SetLivoxLidarWorkModeAfterBoot(handle, work_mode, cb, client_data); +} + // reset lidar livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) { return CommandImpl::LivoxLidarRequestReset(handle, cb, client_data);