mmr/Livox-SDK
Archived
1
0
Fork 0
Code Issues 105 Pull Requests 11 Actions Packages Projects Releases 9 Wiki Activity

feature:support for conneting horizon

Browse Source
This commit is contained in:
Livox-SDK
2019-12-19 15:25:48 +08:00
parent 83def61d80
commit da19f31bc4
1442 changed files with 52220 additions and 16430 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sample_cc/hub/lds_hub.cpp
+219 -37
View File
@@ -83,7 +83,7 @@ int LdsHub::InitLdsHub(std::vector<std::string>& broadcast_code_strs) {
printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
SetBroadcastCallback(LdsHub::OnDeviceBroadcast);
SetDeviceStateUpdateCallback(LdsHub::OnDeviceChange);
SetDeviceStateUpdateCallback(LdsHub::OnDeviceInfoChange);
/** Add commandline input broadcast code */
for (auto input_str : broadcast_code_strs) {
@@ -138,7 +138,7 @@ int LdsHub::DeInitLdsHub(void) {
}
/** Static function in LdsLidar for callback */
void LdsHub::GetLidarDataCb(uint8_t hub_handle, LivoxEthPacket *data,
void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
uint32_t data_num, void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
@@ -157,11 +157,25 @@ void LdsHub::GetLidarDataCb(uint8_t hub_handle, LivoxEthPacket *data,
if (data) {
lds_hub->receive_packet_count_++;
if (0 == (lds_hub->receive_packet_count_ % 100)) {
printf("Receive packet count %d %d\n", handle, lds_hub->receive_packet_count_);
printf("Receive packet handle: %d count: %d\n", handle, lds_hub->receive_packet_count_);
/** Parsing the timestamp and the point cloud data. */
uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
if(data ->data_type == kCartesian) {
LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
}else if ( data ->data_type == kSpherical) {
LivoxSpherPoint *p_point_data = (LivoxSpherPoint *)data->data;
}else if ( data ->data_type == kExtendCartesian) {
LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
}else if ( data ->data_type == kExtendSpherical) {
LivoxExtendSpherPoint *p_point_data = (LivoxExtendSpherPoint *)data->data;
}else if ( data ->data_type == kDualExtendCartesian) {
LivoxDualExtendRawPoint *p_point_data = (LivoxDualExtendRawPoint *)data->data;
}else if ( data ->data_type == kDualExtendSpherical) {
LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data;
}else if ( data ->data_type == kImu) {
LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data;
}
}
}
}
@@ -193,17 +207,19 @@ void LdsHub::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
uint8_t handle = 0;
result = AddHubToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, LdsHub::GetLidarDataCb, (void *)g_lds_hub);
SetDataCallback(handle, LdsHub::OnHubDataCb, (void *)g_lds_hub);
p_hub->handle = handle;
p_hub->connect_state = kConnectStateOff;
//config hub's parameter
p_hub->config.coordinate = kCoordinateCartesian;
} else {
printf("Add Hub to connect is failed : %d %d \n", result, handle);
printf("Add Hub to connect failed, result : %d, handle: %d.\n", result, handle);
}
}
}
/** Callback function of changing of device state. */
void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
void LdsHub::OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
@@ -213,32 +229,36 @@ void LdsHub::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
}
LidarDevice* p_hub = &g_lds_hub->hub_;
if (type == kEventConnect) {
if (type == kEventHubConnectionChange) {
if (p_hub->connect_state == kConnectStateOff) {
p_hub->connect_state = kConnectStateOn;
p_hub->info = *info;
}
printf("[WARNING] Hub sn: [%s] Connection Change!!!\n", info->broadcast_code);
} else if (type == kEventDisconnect) {
p_hub->connect_state = kConnectStateOff;
printf("Hub[%s] disconnect!\n", info->broadcast_code);
printf("[WARNING] Hub sn: [%s] Disconnect!!!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
p_hub->info = *info;
printf("Hub[%s] StateChange\n", info->broadcast_code);
printf("Hub sn [%s] StateChange!!!\n", info->broadcast_code);
}
if (p_hub->connect_state == kConnectStateOn) {
printf("Hub[%s] status_code[%d] working state[%d] feature[%d]\n", \
p_hub->info.broadcast_code,\
p_hub->info.status.status_code,\
p_hub->info.state,\
p_hub->info.feature);
printf("Hub Working State %d\n", p_hub->info.state);
if (p_hub->info.state == kLidarStateInit) {
printf("Hub State Change Progress %u\n", p_hub->info.status.progress);
} else {
printf("Hub State Error Code 0X%08x\n", p_hub->info.status.status_code.error_code);
}
printf("Hub feature %d\n", p_hub->info.feature);
SetErrorMessageCallback(p_hub->handle, LdsHub::HubErrorStatusCb);
if (p_hub->info.state == kLidarStateNormal) {
HubQueryLidarInformation(HubQueryLidarInfoCb, g_lds_hub);
}
}
}
void LdsHub::HubQueryLidarInfoCb(uint8_t status, uint8_t handle, \
void LdsHub::HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
HubQueryLidarInformationResponse *response,\
void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
@@ -263,6 +283,11 @@ void LdsHub::HubQueryLidarInfoCb(uint8_t status, uint8_t handle, \
strncpy(p_lidar->info.broadcast_code, \
response->device_info_list[i].broadcast_code, \
sizeof(p_lidar->info.broadcast_code));
//config lidar's parameter
p_lidar->config.enable_fan = true;
p_lidar->config.return_mode = kStrongestReturn;
p_lidar->config.imu_rate = kImuFreq200Hz;
printf("[%d]%s DeviceType[%d] Slot[%d] Ver[%d.%d.%d.%d]\n", index, \
p_lidar->info.broadcast_code,\
p_lidar->info.type, p_lidar->info.slot,\
@@ -272,45 +297,202 @@ void LdsHub::HubQueryLidarInfoCb(uint8_t status, uint8_t handle, \
response->device_info_list[i].version[3]);
}
}
HubStartSampling(StartSampleCb, lds_hub);
lds_hub->hub_.connect_state = kConnectStateSampling;
LdsHub::ConfigLidarsOfHub(lds_hub);
} else {
printf("Hub have no lidar, will not start sample!\n");
HubQueryLidarInformation(HubQueryLidarInfoCb, lds_hub);
}
} else {
printf("Device Query Informations Failed %d\n", status);
HubQueryLidarInformation(HubQueryLidarInfoCb, lds_hub);
}
}
/** Callback function of hub error message. */
void LdsHub::HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message) {
static uint32_t error_message_count = 0;
if (message != NULL) {
++error_message_count;
if (0 == (error_message_count % 100)) {
printf("handle: %u\n", handle);
printf("sync_status : %u\n", message->hub_error_code.sync_status);
printf("temp_status : %u\n", message->hub_error_code.temp_status);
printf("lidar_status :%u\n", message->hub_error_code.lidar_status);
printf("lidar_link_status : %u\n", message->hub_error_code.lidar_link_status);
printf("firmware_err : %u\n", message->hub_error_code.firmware_err);
printf("system_status : %u\n", message->hub_error_code.system_status);
}
}
}
void LdsHub::ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
}
void LdsHub::HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
HubSetPointCloudReturnModeResponse* response,\
void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
if ((handle >= kMaxLidarCount) || !response) {
return;
}
if ((status != kStatusSuccess) || (response->ret_code)) {
printf("Hub set return mode fail!\n");
ConfigPointCloudReturnMode(lds_hub);
} else {
printf("Hub set return mode success!\n");
lds_hub->hub_.config.set_bits &= ~((uint32_t)(kConfigReturnMode));
if (!lds_hub->hub_.config.set_bits) {
HubStartSampling(LdsHub::StartSampleCb, lds_hub);
lds_hub->hub_.connect_state = kConnectStateSampling;
}
}
}
void LdsHub::SetCoordinateCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_hub = &(lds_hub->hub_);
if (status == kStatusSuccess) {
p_hub->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
printf("Set coordinate success!\n");
if (!p_hub->config.set_bits) {
HubStartSampling(LdsHub::StartSampleCb, lds_hub);
p_hub->connect_state = kConnectStateSampling;
}
} else {
if (p_hub->config.coordinate != 0) {
SetSphericalCoordinate(handle, LdsHub::SetCoordinateCb, lds_hub);
} else {
SetCartesianCoordinate(handle, LdsHub::SetCoordinateCb, lds_hub);
}
printf("Set coordinate fail, try again!\n");
}
}
void LdsHub::HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
HubSetImuPushFrequencyResponse* response,\
void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
if ((handle >= kMaxLidarCount) || !response) {
return;
}
if ((status != kStatusSuccess) || (response->ret_code)) {
printf("Hub set imu freq fail [%d]!\n", response->ret_code);
ConfigImuPushFrequency(lds_hub);
} else {
printf("Hub set imu frequency success!\n");
lds_hub->hub_.config.set_bits &= ~((uint32_t)(kConfigImuRate));
if (!lds_hub->hub_.config.set_bits) {
HubStartSampling(LdsHub::StartSampleCb, lds_hub);
lds_hub->hub_.connect_state = kConnectStateSampling;
}
}
}
/** Callback function of starting sampling. */
void LdsHub::StartSampleCb(uint8_t status, uint8_t handle, \
uint8_t response, void *clent_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(clent_data);
void LdsHub::StartSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
LdsHub* lds_hub = static_cast<LdsHub *>(client_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_hub = &lds_hub->hub_;
if (status == kStatusSuccess) {
if (response != 0) {
p_hub->connect_state = kConnectStateOn;
printf("Hub start sample fail : state[%d] handle[%d] res[%d]\n", \
status, handle, response);
} else {
printf("Hub start sample success!\n");
}
} else if (status == kStatusTimeout) {
if ((status != kStatusSuccess) || (response != 0)) {
p_hub->connect_state = kConnectStateOn;
printf("Hub start sample timeout : state[%d] handle[%d] res[%d]\n", \
printf("Hub start sample fail : state[%d] handle[%d] res[%d]\n", \
status, handle, response);
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]);
if (p_lidar->connect_state == kConnectStateSampling) {
p_lidar->connect_state = kConnectStateOn;
}
}
} else {
printf("Hub start sample success!\n");
}
}
/** Callback function of stopping sampling. */
void LdsHub::StopSampleCb(uint8_t status, uint8_t handle, \
uint8_t response, void *clent_data) {
void LdsHub::StopSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
}
void LdsHub::ConfigPointCloudReturnMode(LdsHub* lds_hub) {
uint8_t req_buf[1024];
HubSetPointCloudReturnModeRequest* req = (HubSetPointCloudReturnModeRequest *)req_buf;
req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \
(p_lidar->connect_state == kConnectStateSampling)) {
strncpy(req->lidar_cfg_list[req->count].broadcast_code, \
p_lidar->info.broadcast_code, \
sizeof(req->lidar_cfg_list[req->count].broadcast_code));
req->lidar_cfg_list[req->count].mode = p_lidar->config.return_mode;
req->count++;
}
}
if (req->count) {
uint32_t length = sizeof(HubSetPointCloudReturnModeRequest) + sizeof(SetPointCloudReturnModeRequestItem) * (req->count - 1);
HubSetPointCloudReturnMode(req, length,\
LdsHub::HubSetPointCloudReturnModeCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigReturnMode;
}
}
void LdsHub::ConfigImuPushFrequency(LdsHub* lds_hub) {
uint8_t req_buf[1024];
HubSetImuPushFrequencyRequest* req = (HubSetImuPushFrequencyRequest *)req_buf;
req->count = 0;
for (int i = 0; i < kMaxLidarCount; i++) {
LidarDevice* p_lidar = &(lds_hub->lidars_[i]);
if ((p_lidar->info.type != kDeviceTypeLidarMid40) && \
(p_lidar->connect_state == kConnectStateSampling)) {
strncpy(req->lidar_cfg_list[req->count].broadcast_code, \
p_lidar->info.broadcast_code, \
sizeof(req->lidar_cfg_list[req->count].broadcast_code));
req->lidar_cfg_list[req->count].freq = p_lidar->config.imu_rate;
req->count++;
}
}
if (req->count) {
uint32_t length = sizeof(HubSetImuPushFrequencyRequest) + sizeof(SetImuPushFrequencyRequestItem) * (req->count -1);
HubSetImuPushFrequency(req, length,\
LdsHub::HubSetImuRatePushFrequencyCb, lds_hub);
lds_hub->hub_.config.set_bits |= kConfigImuRate;
}
}
void LdsHub::ConfigLidarsOfHub(LdsHub* lds_hub) {
ConfigPointCloudReturnMode(lds_hub);
ConfigImuPushFrequency(lds_hub);
if (lds_hub->hub_.config.coordinate != 0) {
SetSphericalCoordinate(lds_hub->hub_.handle, LdsHub::SetCoordinateCb, lds_hub);
printf("Hub set coordinate spherical\n");
} else {
printf("Hub set coordinate cartesian\n");
SetCartesianCoordinate(lds_hub->hub_.handle, LdsHub::SetCoordinateCb, lds_hub);
}
lds_hub->hub_.config.set_bits |= kConfigCoordinate;
lds_hub->hub_.connect_state = kConnectStateConfig;
}
/** Add broadcast code to whitelist */
@@ -332,12 +514,12 @@ int LdsHub::AddBroadcastCodeToWhitelist(const char* broadcast_code) {
}
void LdsHub::AddLocalBroadcastCode(void) {
for (size_t i=0; i<sizeof(local_broadcast_code_list)/sizeof(intptr_t); ++i) {
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])) && \
(nullptr == strstr(local_broadcast_code_list[i], invalid_bd.c_str()))) {
LdsHub::AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]);
AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]);
} else {
printf("Invalid local broadcast code : %s\n", local_broadcast_code_list[i]);
}
@@ -349,7 +531,7 @@ bool LdsHub::FindInWhitelist(const char* broadcast_code) {
return false;
}
for (uint32_t i=0; i<whitelist_count_; i++) {
for (uint32_t i = 0; i < whitelist_count_; i++) {
if (strncmp(broadcast_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) {
return true;
}
sample_cc/hub/lds_hub.h
+44 -7
View File
@@ -36,13 +36,36 @@
typedef enum {
kConnectStateOff = 0,
kConnectStateOn = 1,
kConnectStateSampling = 2,
kConnectStateConfig = 2,
kConnectStateSampling = 3,
} LidarConnectState;
typedef enum {
kConfigFan = 1,
kConfigReturnMode = 2,
kConfigCoordinate = 4,
kConfigImuRate = 8
} LidarConfigCodeBit;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef struct {
bool enable_fan;
uint32_t return_mode;
uint32_t coordinate;
uint32_t imu_rate;
volatile uint32_t set_bits;
volatile uint32_t get_bits;
} UserConfig;
typedef struct {
uint8_t handle;
LidarConnectState connect_state;
DeviceInfo info;
UserConfig config;
} LidarDevice;
/**
@@ -65,14 +88,29 @@ class LdsHub {
LdsHub& operator=(const LdsHub&) = delete;
/** LiDAR data receive callback */
static void GetLidarDataCb(uint8_t hub_handle, LivoxEthPacket *data,\
static void OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,\
uint32_t data_num, void *client_data);
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
static void StartSampleCb(uint8_t status, uint8_t handle, uint8_t response, void *clent_data);
static void StopSampleCb(uint8_t status, uint8_t handle, uint8_t response, void *clent_data);
static void HubQueryLidarInfoCb(uint8_t status, uint8_t handle, \
static void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type);
static void StartSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void StopSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void HubQueryLidarInfoCb(livox_status status, uint8_t handle, \
HubQueryLidarInformationResponse *response, void *client_data);
static void HubErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message);
static void ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void HubSetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
HubSetPointCloudReturnModeResponse* response,\
void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void HubSetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
HubSetImuPushFrequencyResponse* response,\
void *clent_data);
static void ConfigPointCloudReturnMode(LdsHub* lds_hub);
static void ConfigImuPushFrequency(LdsHub* lds_hub);
static void ConfigLidarsOfHub(LdsHub* lds_hub);
int AddBroadcastCodeToWhitelist(const char* broadcast_code);
void AddLocalBroadcastCode(void);
@@ -92,7 +130,6 @@ class LdsHub {
LidarDevice lidars_[kMaxLidarCount];
LidarDevice hub_;
uint32_t receive_packet_count_;
};
#endif
sample_cc/lidar/lds_lidar.cpp
+170 -21
View File
@@ -50,7 +50,8 @@ LdsLidar::LdsLidar() {
memset(lidars_, 0, sizeof(lidars_));
for (uint32_t i=0; i<kMaxLidarCount; i++) {
lidars_[i].handle = kMaxLidarCount; /** Unallocated state */
lidars_[i].handle = kMaxLidarCount;
/** Unallocated state */
lidars_[i].connect_state = kConnectStateOff;
}
}
@@ -146,13 +147,27 @@ void LdsLidar::GetLidarDataCb(uint8_t handle, LivoxEthPacket *data,
}
if (eth_packet) {
lidar_this->data_recveive_count_[handle] += data_num;
if (lidar_this->data_recveive_count_[handle] % 10000 == 0) {
lidar_this->data_recveive_count_[handle] ++;
if (lidar_this->data_recveive_count_[handle] % 100 == 0) {
printf("receive packet count %d %d\n", handle, lidar_this->data_recveive_count_[handle]);
/** Parsing the timestamp and the point cloud data. */
uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
if(data ->data_type == kCartesian) {
LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
}else if ( data ->data_type == kSpherical) {
LivoxSpherPoint *p_point_data = (LivoxSpherPoint *)data->data;
}else if ( data ->data_type == kExtendCartesian) {
LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
}else if ( data ->data_type == kExtendSpherical) {
LivoxExtendSpherPoint *p_point_data = (LivoxExtendSpherPoint *)data->data;
}else if ( data ->data_type == kDualExtendCartesian) {
LivoxDualExtendRawPoint *p_point_data = (LivoxDualExtendRawPoint *)data->data;
}else if ( data ->data_type == kDualExtendSpherical) {
LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *)data->data;
}else if ( data ->data_type == kImu) {
LivoxImuPoint *p_point_data = (LivoxImuPoint *)data->data;
}
}
}
}
@@ -182,8 +197,13 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
result = AddLidarToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
SetDataCallback(handle, LdsLidar::GetLidarDataCb, (void *)g_lidars);
g_lidars->lidars_[handle].handle = handle;
g_lidars->lidars_[handle].connect_state = kConnectStateOff;
LidarDevice* p_lidar = &(g_lidars->lidars_[handle]);
p_lidar->handle = handle;
p_lidar->connect_state = kConnectStateOff;
p_lidar->config.enable_fan = true;
p_lidar->config.return_mode = kStrongestReturn;
p_lidar->config.coordinate = kCoordinateCartesian;
p_lidar->config.imu_rate = kImuFreq200Hz;
} else {
printf("Add lidar to connect is failed : %d %d \n", result, handle);
}
@@ -207,29 +227,54 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
p_lidar->connect_state = kConnectStateOn;
p_lidar->info = *info;
}
printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
} else if (type == kEventDisconnect) {
p_lidar->connect_state = kConnectStateOff;
printf("Lidar[%s] disconnect!\n", info->broadcast_code);
printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
p_lidar->info = *info;
printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
}
if (p_lidar->connect_state == kConnectStateOn) {
printf("Lidar[%s] status_code[%d] working state[%d] feature[%d]\n", \
p_lidar->info.broadcast_code,\
p_lidar->info.status.status_code,\
p_lidar->info.state,\
p_lidar->info.feature);
printf("Device Working State %d\n", p_lidar->info.state);
if (p_lidar->info.state == kLidarStateInit) {
printf("Device State Change Progress %u\n", p_lidar->info.status.progress);
} else {
printf("Device State Error Code 0X%08x\n", p_lidar->info.status.status_code.error_code);
}
printf("Device feature %d\n", p_lidar->info.feature);
SetErrorMessageCallback(handle, LdsLidar::LidarErrorStatusCb);
/** Config lidar parameter */
if (p_lidar->info.state == kLidarStateNormal) {
LidarStartSampling(handle, LdsLidar::StartSampleCb, g_lidars);
p_lidar->connect_state = kConnectStateSampling;
if (p_lidar->config.coordinate != 0) {
SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, g_lidars);
} else {
SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, g_lidars);
}
p_lidar->config.set_bits |= kConfigCoordinate;
if (kDeviceTypeLidarMid40 != info->type) {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\
LdsLidar::SetPointCloudReturnModeCb, g_lidars);
p_lidar->config.set_bits |= kConfigReturnMode;
}
if (kDeviceTypeLidarMid40 != info->type) {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\
LdsLidar::SetImuRatePushFrequencyCb, g_lidars);
p_lidar->config.set_bits |= kConfigImuRate;
}
p_lidar->connect_state = kConnectStateConfig;
}
}
}
/** Query the firmware version of Livox LiDAR. */
void LdsLidar::DeviceInformationCb(uint8_t status, uint8_t handle, \
DeviceInformationResponse *ack, void *clent_data) {
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle, \
DeviceInformationResponse *ack, void *client_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed : %d\n", status);
}
@@ -242,10 +287,114 @@ void LdsLidar::DeviceInformationCb(uint8_t status, uint8_t handle, \
}
}
/** Callback function of Lidar error message. */
void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message) {
static uint32_t error_message_count = 0;
if (message != NULL) {
++error_message_count;
if (0 == (error_message_count % 100)) {
printf("handle: %u\n", handle);
printf("temp_status : %u\n", message->lidar_error_code.temp_status);
printf("volt_status : %u\n", message->lidar_error_code.volt_status);
printf("motor_status : %u\n", message->lidar_error_code.motor_status);
printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);
printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);
printf("pps_status : %u\n", message->lidar_error_code.device_status);
printf("fan_status : %u\n", message->lidar_error_code.fan_status);
printf("self_heating : %u\n", message->lidar_error_code.self_heating);
printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
printf("system_status : %u\n", message->lidar_error_code.system_status);
}
}
}
void LdsLidar::ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
}
void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
printf("Set return mode success!\n");
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode)(p_lidar->config.return_mode),\
LdsLidar::SetPointCloudReturnModeCb, lds_lidar);
printf("Set return mode fail, try again!\n");
}
}
void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
printf("Set coordinate success!\n");
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
if (p_lidar->config.coordinate != 0) {
SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
} else {
SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
}
printf("Set coordinate fail, try again!\n");
}
}
void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice* p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
printf("Set imu rate success!\n");
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),\
LdsLidar::SetImuRatePushFrequencyCb, lds_lidar);
printf("Set imu rate fail, try again!\n");
}
}
/** Callback function of starting sampling. */
void LdsLidar::StartSampleCb(uint8_t status, uint8_t handle, \
uint8_t response, void *clent_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(clent_data);
void LdsLidar::StartSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
LdsLidar* lds_lidar = static_cast<LdsLidar *>(client_data);
if (handle >= kMaxLidarCount) {
return;
@@ -268,8 +417,8 @@ void LdsLidar::StartSampleCb(uint8_t status, uint8_t handle, \
}
/** Callback function of stopping sampling. */
void LdsLidar::StopSampleCb(uint8_t status, uint8_t handle, \
uint8_t response, void *clent_data) {
void LdsLidar::StopSampleCb(livox_status status, uint8_t handle, \
uint8_t response, void *client_data) {
}
/** Add broadcast code to whitelist */
sample_cc/lidar/lds_lidar.h
+36 -4
View File
@@ -37,13 +37,36 @@
typedef enum {
kConnectStateOff = 0,
kConnectStateOn = 1,
kConnectStateSampling = 2,
kConnectStateConfig = 2,
kConnectStateSampling = 3,
} LidarConnectState;
typedef enum {
kConfigFan = 1,
kConfigReturnMode = 2,
kConfigCoordinate = 4,
kConfigImuRate = 8
} LidarConfigCodeBit;
typedef enum {
kCoordinateCartesian = 0,
kCoordinateSpherical
} CoordinateType;
typedef struct {
bool enable_fan;
uint32_t return_mode;
uint32_t coordinate;
uint32_t imu_rate;
volatile uint32_t set_bits;
volatile uint32_t get_bits;
} UserConfig;
typedef struct {
uint8_t handle;
LidarConnectState connect_state;
DeviceInfo info;
UserConfig config;
} LidarDevice;
/**
@@ -70,10 +93,19 @@ class LdsLidar {
uint32_t data_num, void *client_data);
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
static void StartSampleCb(uint8_t status, uint8_t handle, uint8_t response, void *clent_data);
static void StopSampleCb(uint8_t status, uint8_t handle, uint8_t response, void *clent_data);
static void DeviceInformationCb(uint8_t status, uint8_t handle, \
static void StartSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void StopSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
static void DeviceInformationCb(livox_status status, uint8_t handle, \
DeviceInformationResponse *ack, void *clent_data);
static void LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message);
static void ControlFanCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void SetCoordinateCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
uint8_t response, void *clent_data);
int AddBroadcastCodeToWhitelist(const char* broadcast_code);
void AddLocalBroadcastCode(void);
sample_cc/lidar_utc_sync/CMakeLists.txt
+12
View File
@@ -0,0 +1,12 @@
cmake_minimum_required(VERSION 3.0)
if (UNIX)
set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-pthread")
endif (UNIX)
set(DEMO_NAME lidar_utc_sync)
add_executable(${DEMO_NAME} main.cpp lds_lidar.cpp synchro.cpp)
target_link_libraries(${DEMO_NAME}
PRIVATE
${PROJECT_NAME}_static
)
sample_cc/lidar_utc_sync/lds_lidar.cpp
+315
View File
@@ -0,0 +1,315 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 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 "lds_lidar.h"
#include <stdio.h>
#include <string.h>
#include <thread>
#include <memory>
#include <mutex>
std::mutex mtx;
/** Const varible ------------------------------------------------------------------------------- */
/** User add broadcast code here */
static const char* local_broadcast_code_list[] = {
"000000000000001",
};
/** For callback use only */
LdsLidar* g_lidars = nullptr;
/** Lds lidar function ---------------------------------------------------------------------------*/
LdsLidar::LdsLidar() {
auto_connect_mode_ = true;
whitelist_count_ = 0;
is_initialized_ = false;
lidar_count_ = 0;
memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
memset(lidars_, 0, sizeof(lidars_));
for (uint32_t i=0; i<kMaxLidarCount; i++) {
lidars_[i].handle = kMaxLidarCount;
/** Unallocated state */
lidars_[i].connect_state = kConnectStateOff;
}
}
LdsLidar::~LdsLidar() {
}
int LdsLidar::InitLdsLidar(std::vector<std::string>& broadcast_code_strs) {
if (is_initialized_) {
printf("LiDAR data source is already inited!\n");
return -1;
}
if (!Init()) {
Uninit();
printf("Livox-SDK init fail!\n");
return -1;
}
LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion);
printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
SetBroadcastCallback(LdsLidar::OnDeviceBroadcast);
SetDeviceStateUpdateCallback(LdsLidar::OnDeviceChange);
/** Add commandline input broadcast code */
for (auto input_str : broadcast_code_strs) {
LdsLidar::AddBroadcastCodeToWhitelist(input_str.c_str());
}
/** Add local broadcast code */
LdsLidar::AddLocalBroadcastCode();
if (whitelist_count_) {
LdsLidar::DisableAutoConnectMode();
printf("Disable auto connect mode!\n");
printf("List all broadcast code in whiltelist:\n");
for (uint32_t i=0; i<whitelist_count_; i++) {
printf("%s\n", broadcast_code_whitelist_[i]);
}
} else {
LdsLidar::EnableAutoConnectMode();
printf("No broadcast code was added to whitelist, swith to automatic connection mode!\n");
}
/** Start the device discovering routine. */
if (!Start()) {
Uninit();
printf("Livox-SDK init fail!\n");
return -1;
}
/** Add here, only for callback use */
if (g_lidars == nullptr) {
g_lidars = this;
}
is_initialized_= true;
printf("Livox-SDK init success!\n");
return 0;
}
int LdsLidar::DeInitLdsLidar(void) {
if (!is_initialized_) {
printf("LiDAR data source is not exit");
return -1;
}
Uninit();
printf("Livox SDK Deinit completely!\n");
return 0;
}
/** Static function in LdsLidar for callback or event process ------------------------------------*/
void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == nullptr) {
return;
}
if (info->dev_type == kDeviceTypeHub) {
printf("In lidar mode, couldn't connect a hub : %s\n", info->broadcast_code);
return;
}
std::unique_lock<std::mutex> lock(mtx);
if (g_lidars->IsAutoConnectMode()) {
printf("In automatic connection mode, will connect %s\n", info->broadcast_code);
} else {
if (!g_lidars->FindInWhitelist(info->broadcast_code)) {
printf("Not in the whitelist, please add %s to if want to connect!\n",\
info->broadcast_code);
return;
}
}
bool result = false;
uint8_t handle = 0;
result = AddLidarToConnect(info->broadcast_code, &handle);
if (result == kStatusSuccess && handle < kMaxLidarCount) {
g_lidars->lidars_[handle].handle = handle;
g_lidars->lidars_[handle].connect_state = kConnectStateOff;
} else {
printf("Add lidar to connect is failed : %d %d \n", result, handle);
}
}
void LdsLidar::SetRmcSyncTime(const char* rmc, uint16_t rmc_length) {
printf("Rmc: %s\n",rmc);
std::unique_lock<std::mutex> lock(mtx);
LidarDevice* p_lidar = nullptr;
for (uint8_t handle = 0; handle < kMaxLidarCount; handle++) {
p_lidar = &(g_lidars->lidars_[handle]);
if (p_lidar->connect_state != kConnectStateOff && p_lidar->info.state == kLidarStateNormal) {
livox_status status = LidarSetRmcSyncTime(handle, rmc, rmc_length, \
LidarSetRmcSyncTimeCb, g_lidars);
if (status != kStatusSuccess) {
printf("Lidar set GPRMC synchronization time error code: %d.\n",status);
}
}
}
}
/** Callback function of set synchronization time. */
void LdsLidar::LidarSetRmcSyncTimeCb(livox_status status, \
uint8_t handle,uint8_t response, void* client_data) {
if (handle >= kMaxLidarCount) {
return;
}
printf("OnLidarSetRmcSyncTimeCallback statue %d handle %d response %d. \n", status, handle, response);
}
/** Callback function of changing of device state. */
void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
if (info == nullptr) {
return;
}
uint8_t handle = info->handle;
if (handle >= kMaxLidarCount) {
return;
}
std::unique_lock<std::mutex> lock(mtx);
LidarDevice* p_lidar = &(g_lidars->lidars_[handle]);
if (type == kEventConnect) {
QueryDeviceInformation(handle, DeviceInformationCb, g_lidars);
if (p_lidar->connect_state == kConnectStateOff) {
p_lidar->connect_state = kConnectStateOn;
p_lidar->info = *info;
}
printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
} else if (type == kEventDisconnect) {
p_lidar->connect_state = kConnectStateOff;
printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
p_lidar->info = *info;
printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
}
if (p_lidar->connect_state == kConnectStateOn) {
printf("Device Working State %d\n", p_lidar->info.state);
if (p_lidar->info.state == kLidarStateInit) {
printf("Device State Change Progress %u\n", p_lidar->info.status.progress);
} else {
printf("Device State Error Code 0X%08x\n", p_lidar->info.status.status_code.error_code);
}
printf("Device feature %d\n", p_lidar->info.feature);
SetErrorMessageCallback(handle, LdsLidar::LidarErrorStatusCb);
}
}
/** Query the firmware version of Livox LiDAR. */
void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle, \
DeviceInformationResponse *ack, void *clent_data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed : %d\n", status);
}
if (ack) {
printf("firm ver: %d.%d.%d.%d\n",
ack->firmware_version[0],
ack->firmware_version[1],
ack->firmware_version[2],
ack->firmware_version[3]);
}
}
/** Callback function of Lidar error message. */
void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message) {
static uint32_t error_message_count = 0;
if (message != NULL) {
++error_message_count;
if (0 == (error_message_count % 100)) {
printf("handle: %u\n", handle);
printf("temp_status : %u\n", message->lidar_error_code.temp_status);
printf("volt_status : %u\n", message->lidar_error_code.volt_status);
printf("motor_status : %u\n", message->lidar_error_code.motor_status);
printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);
printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);
printf("pps_status : %u\n", message->lidar_error_code.device_status);
printf("fan_status : %u\n", message->lidar_error_code.fan_status);
printf("self_heating : %u\n", message->lidar_error_code.self_heating);
printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
printf("system_status : %u\n", message->lidar_error_code.system_status);
}
}
}
/** Add broadcast code to whitelist */
int LdsLidar::AddBroadcastCodeToWhitelist(const char* bd_code) {
if (!bd_code || (strlen(bd_code) > kBroadcastCodeSize) || \
(whitelist_count_ >= kMaxLidarCount)) {
return -1;
}
if (LdsLidar::FindInWhitelist(bd_code)) {
printf("%s is alrealy exist!\n", bd_code);
return -1;
}
strcpy(broadcast_code_whitelist_[whitelist_count_], bd_code);
++whitelist_count_;
return 0;
}
void LdsLidar::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])) && \
(nullptr == strstr(local_broadcast_code_list[i], invalid_bd.c_str()))) {
LdsLidar::AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]);
} else {
printf("Invalid local broadcast code : %s\n", local_broadcast_code_list[i]);
}
}
}
bool LdsLidar::FindInWhitelist(const char* bd_code) {
if (!bd_code) {
return false;
}
for (uint32_t i=0; i<whitelist_count_; i++) {
if (strncmp(bd_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0) {
return true;
}
}
return false;
}
sample_cc/lidar_utc_sync/lds_lidar.h
+96
View File
@@ -0,0 +1,96 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 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.
//
/** Livox LiDAR data source, data from dependent lidar */
#ifndef LDS_LIDAR_H_
#define LDS_LIDAR_H_
#include <memory>
#include <vector>
#include "livox_def.h"
#include "livox_sdk.h"
typedef enum {
kConnectStateOff = 0,
kConnectStateOn = 1,
kConnectStateSampling = 2,
} LidarConnectState;
typedef struct {
uint8_t handle;
LidarConnectState connect_state;
DeviceInfo info;
} LidarDevice;
/**
* LiDAR data source, data from dependent lidar.
*/
class LdsLidar {
public:
static LdsLidar& GetInstance() {
static LdsLidar lds_lidar;
return lds_lidar;
}
int InitLdsLidar(std::vector<std::string>& broadcast_code_strs);
int DeInitLdsLidar(void);
static void SetRmcSyncTime(const char* rmc, uint16_t rmc_length);
private:
LdsLidar();
LdsLidar(const LdsLidar&) = delete;
~LdsLidar();
LdsLidar& operator=(const LdsLidar&) = delete;
static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
static void LidarSetRmcSyncTimeCb(livox_status status, uint8_t handle,uint8_t response, void* client_data);
static void DeviceInformationCb(livox_status status, uint8_t handle, \
DeviceInformationResponse *ack, void *clent_data);
static void LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message);
int AddBroadcastCodeToWhitelist(const char* broadcast_code);
void AddLocalBroadcastCode(void);
bool FindInWhitelist(const char* broadcast_code);
void EnableAutoConnectMode(void) { auto_connect_mode_ = true; }
void DisableAutoConnectMode(void) { auto_connect_mode_ = false; }
bool IsAutoConnectMode(void) { return auto_connect_mode_; }
bool auto_connect_mode_;
uint32_t whitelist_count_;
volatile bool is_initialized_;
char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize];
uint32_t lidar_count_;
LidarDevice lidars_[kMaxLidarCount];
uint32_t data_recveive_count_[kMaxLidarCount];
};
#endif
sample_cc/lidar_utc_sync/main.cpp
+174
View File
@@ -0,0 +1,174 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 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 <stdio.h>
#include <stdlib.h>
#ifdef WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif
#include <string.h>
#include <apr_general.h>
#include <apr_getopt.h>
#include "lds_lidar.h"
#include "synchro.h"
/** Cmdline input broadcast code */
static std::vector<std::string> cmdline_broadcast_code;
/** Set the program options.
* You can input the registered device broadcast code and decide whether to save the log file.
*/
static int SetProgramOption(int argc, const char *argv[]) {
apr_status_t rv;
apr_pool_t *mp = NULL;
static const apr_getopt_option_t opt_option[] = {
/** Long-option, short-option, has-arg flag, description */
{ "code", 'c', 1, "Register device broadcast code" },
{ "log", 'l', 0, "Save the log file" },
{ "help", 'h', 0, "Show help" },
{ NULL, 0, 0, NULL },
};
apr_getopt_t *opt = NULL;
int optch = 0;
const char *optarg = NULL;
if (apr_initialize() != APR_SUCCESS) {
return -1;
}
if (apr_pool_create(&mp, NULL) != APR_SUCCESS) {
return -1;
}
rv = apr_getopt_init(&opt, mp, argc, argv);
if (rv != APR_SUCCESS) {
printf("Program options initialization failed.\n");
return -1;
}
/** Parse the all options based on opt_option[] */
bool is_help = false;
while ((rv = apr_getopt_long(opt, opt_option, &optch, &optarg)) == APR_SUCCESS) {
switch (optch) {
case 'c': {
printf("Register broadcast code: %s\n", optarg);
char *sn_list = (char *)malloc(sizeof(char)*(strlen(optarg) + 1));
strncpy(sn_list, optarg, sizeof(char)*(strlen(optarg) + 1));
char *sn_list_head = sn_list;
sn_list = strtok(sn_list, "&");
cmdline_broadcast_code.clear();
while (sn_list) {
cmdline_broadcast_code.push_back(sn_list);
sn_list = strtok(nullptr, "&");
}
free(sn_list_head);
sn_list_head = nullptr;
break;
}
case 'l': {
printf("Save the log file.\n");
SaveLoggerFile();
break;
}
case 'h': {
printf(
" [-c] Register device broadcast code\n"
" [-l] Save the log file\n"
" [-h] Show help\n"
);
is_help = true;
break;
}
default: {
break;
}
}
}
if (rv != APR_EOF) {
printf("Invalid options.\n");
}
apr_pool_destroy(mp);
mp = NULL;
apr_terminate();
if (is_help)
return 1;
return 0;
}
int main(int argc, const char *argv[]) {
/** Set the program options. */
if (SetProgramOption(argc, argv)) {
return 0;
}
LdsLidar& read_lidar = LdsLidar::GetInstance();
int ret = read_lidar.InitLdsLidar(cmdline_broadcast_code);
if (!ret) {
printf("Init lds lidar success!\n");
} else {
printf("Init lds lidar fail!\n");
return 0;
}
printf("Start discovering device.\n");
Synchro& synchro = Synchro::GetInstance();
#ifdef WIN32
synchro.SetPortName("COM3");
#else
synchro.SetPortName("/dev/ttyUSB0");
#endif
/** Set Synchro's Baudrate: 9600. */
synchro.SetBaudRate(BR9600);
/** Set Synchro's Parity: No parity (8N1). */
synchro.SetParity(P_8N1);
/** SyncTimer Callback will trigger when Synchro's GPRMC/GNRMC signal is ready. */
synchro.SetSyncTimerCallback(read_lidar.SetRmcSyncTime);
if (synchro.Start()) {
printf("Synchro start success");
} else {
printf("Synchro start failed");
return 0;
}
#ifdef WIN32
Sleep(100000);
#else
sleep(100);
#endif
synchro.Stop();
read_lidar.DeInitLdsLidar();
printf("Livox lidar demo end!\n");
}
sample_cc/lidar_utc_sync/synchro.cpp
+355
View File
@@ -0,0 +1,355 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 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>
#ifdef WIN32
#include <Windows.h>
#else
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <fcntl.h>
#endif
#ifdef WIN32
Synchro::Synchro() {
hfile_ = INVALID_HANDLE_VALUE;
is_quite_ = false;
rmc_buff_.resize(128,0);
}
#else
Synchro::Synchro() {
fd_ = 0;
is_quite_ = false;
rmc_buff_.resize(128,0);
}
#endif
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_ = port_name;
}
bool Synchro::Start() {
if (!Open()) {
return false;
}
is_quite_ = false;
listener_ = std::make_shared<std::thread>(&Synchro::IOLoop, this);
return true;
}
void Synchro::Stop() {
is_quite_ = true;
if (listener_) {
listener_->join();
listener_ = nullptr;
}
Close();
}
void Synchro::IOLoop() {
uint8_t buff[READ_BUF];
size_t size = 0;
while (!is_quite_) {
if (0 != (size = Read(buff, READ_BUF))) {
Decode(buff, size);
}
}
}
bool Synchro::ParseGps(uint8_t in_byte) {
if (cur_len_ < strlen("$GPRMC")) {
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("$GPRMC")) {
if (strncmp((const char*)&rmc_buff_[0], "$GPRMC", strlen("$GPRMC")) != 0 && \
strncmp((const char*)&rmc_buff_[0], "$GNRMC", strlen("$GNRMC")) != 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);
}
#ifdef WIN32
bool Synchro::Open() {
hfile_ = CreateFile(port_name_.c_str(),
GENERIC_READ,
0,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL);
if (hfile_ == INVALID_HANDLE_VALUE) {
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 (hfile_ != INVALID_HANDLE_VALUE) {
PurgeComm(hfile_, PURGE_RXCLEAR | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_TXABORT);
CloseHandle(hfile_);
}
hfile_ = INVALID_HANDLE_VALUE;
}
/* Sets up the port parameters */
int Synchro::Setup(enum BaudRate baud, enum Parity parity) {
static uint32_t baud_map[19] = {\
2400, 4800, 9600, 19200, 38400, 57600,115200, 230400,\
460800, 500000, 576000,921600,1152000, 1500000, 2000000,\
2500000, 3000000, 3500000, 4000000\
};
DCB dcb;
GetCommState(hfile_, &dcb);
dcb.BaudRate = DWORD(baud_map[baud]);
switch (parity) {
case P_8N1:
/* No parity (8N1) */
dcb.ByteSize = 8;
dcb.Parity = NOPARITY;
dcb.StopBits = ONESTOPBIT;
break;
case P_7E1:
/* Even parity (7E1) */
dcb.ByteSize = 7;
dcb.Parity = EVENPARITY;
dcb.StopBits = ONESTOPBIT;
break;
case P_7O1:
/* Odd parity (7O1) */
dcb.ByteSize = 7;
dcb.Parity = ODDPARITY;
dcb.StopBits = ONESTOPBIT;
break;
case P_7S1:
/* Space parity is setup the same as no parity (7S1) */
dcb.ByteSize = 7;
dcb.Parity = SPACEPARITY;
dcb.StopBits = ONESTOPBIT;
break;
default:
return -1;
}
SetCommState(hfile_, &dcb);
PurgeComm(hfile_, PURGE_RXCLEAR|PURGE_TXCLEAR|PURGE_RXABORT|PURGE_TXABORT);
return 0;
}
size_t Synchro::Read(uint8_t *buffer, size_t size) {
DWORD len = 0;
if (hfile_ != INVALID_HANDLE_VALUE) {
ReadFile(hfile_, buffer, size, &len, NULL);
return len;
} else {
return 0;
}
}
#else
bool Synchro::Open() {
fd_ = open(port_name_.c_str(), O_RDONLY | O_NOCTTY);
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,\
B460800, B500000, B576000,B921600,B1152000, B1500000, B2000000,\
B2500000, B3000000, B3500000, B4000000\
};
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(10000);
/* 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 */
options.c_cflag &= ~CBAUD;
baudrate = baud_map[baud];
options.c_cflag |= baudrate;
printf("[Baudrate]: %d %d\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) {
return read(fd_, buffer, size);
} else {
return 0;
}
}
#endif
sample_cc/lidar_utc_sync/synchro.h
+119
View File
@@ -0,0 +1,119 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 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.
//
#ifndef SYNCHRO_H_
#define SYNCHRO_H_
#include <stdint.h>
#include <memory>
#include <thread>
#include <vector>
#include <functional>
#ifdef WIN32
#include <Windows.h>
#endif
typedef void (*UtcTimerCallback)(const char* rmc, uint16_t rmc_length);
# define READ_BUF (256)
enum Parity {
P_8N1, /* No parity (8N1) */
P_7E1, /* Even parity (7E1)*/
P_7O1, /* Odd parity (7O1) */
P_7S1 /* Space parity is setup the same as no parity (7S1) */
};
enum BaudRate {
BR2400,
BR4800,
BR9600,
BR19200,
BR38400,
BR57600,
BR115200,
BR230400,
BR460800,
BR500000,
BR576000,
BR921600,
BR1152000,
BR1500000,
BR2000000,
BR2500000,
BR3000000,
BR3500000,
BR4000000,
};
class Synchro {
public:
Synchro();
~Synchro();
static Synchro& GetInstance() {
static Synchro synchro;
return synchro;
}
bool Start();
void Stop();
void SetBaudRate(BaudRate baudrate);
void SetPortName(std::string port_name);
void SetParity(Parity parity);
void SetSyncTimerCallback(UtcTimerCallback cb);
private:
int Setup(enum BaudRate baud, enum Parity parity);
void Close();
bool Open();
void IOLoop();
size_t Read(uint8_t *buffer, size_t size);
void Decode(uint8_t* buff, size_t size);
bool ParseGps(uint8_t in_byte);
void Clear();
#ifdef WIN32
HANDLE hfile_;
#else
int fd_;
#endif
enum BaudRate baudrate_;
enum Parity parity_;
std::string port_name_;
std::function<void(const char* rmc, uint16_t rmc_length)> sync_timer_cb_;
bool is_quite_;
std::shared_ptr<std::thread> listener_;
std::vector<char> rmc_buff_;
uint16_t cur_len_;
};
#endif
sample_cc/trouble_shooting/CMakeLists.txt
+8
View File
@@ -0,0 +1,8 @@
cmake_minimum_required(VERSION 3.0)
set(DEMO_NAME trouble_shooting)
add_executable(${DEMO_NAME} main.cpp)
target_link_libraries(${DEMO_NAME}
PRIVATE
${PROJECT_NAME}_static
)
sample_cc/trouble_shooting/main.cpp
+94
View File
@@ -0,0 +1,94 @@
//
// The MIT License (MIT)
//
// Copyright (c) 2019 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 <stdio.h>
#include <stdlib.h>
#ifdef WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif
#include <string>
#include <map>
#include "livox_sdk.h"
/** Connect all the broadcast device. */
std::map<std::string,std::string> broadcast_code_list;
/** Callback function when broadcast message received. */
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
return;
}
printf("Receive Broadcast Code %s\n", info->broadcast_code);
if (broadcast_code_list.find(info->ip) == broadcast_code_list.end()) {
broadcast_code_list[info->ip] = info->broadcast_code;
return;
}
if (broadcast_code_list[info->ip] == info->broadcast_code) {
return;
}
printf("broadcast_code: {%s} is conflicted with broadcast_code: {%s}, which ip is : {%s}\n", \
broadcast_code_list[info->ip].c_str(), info->broadcast_code, info->ip);
}
int main(int argc, const char *argv[]) {
printf("Livox SDK initializing.\n");
/** Initialize Livox-SDK. */
if (!Init()) {
return -1;
}
printf("Livox SDK has been initialized.\n");
LivoxSdkVersion _sdkversion;
GetLivoxSdkVersion(&_sdkversion);
printf("Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
/** Set the callback function receiving broadcast message from Livox LiDAR. */
SetBroadcastCallback(OnDeviceBroadcast);
/** Start the device discovering routine. */
printf("Start IP Conflict detection.\n");
if (!Start()) {
Uninit();
return -1;
}
printf("Start discovering device.\n");
#ifdef WIN32
Sleep(10000);
#else
sleep(10);
#endif
/** Uninitialize Livox-SDK. */
Uninit();
}