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/lidar/main.c
+83 -29
View File
@@ -62,22 +62,57 @@ char broadcast_code_list[kMaxLidarCount][kBroadcastCodeSize] = {
"000000000000004"
};*/
/** Receiving error message from Livox Lidar. */
void OnLidarErrorStatusCallback(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);
}
}
}
/** Receiving point cloud data from Livox LiDAR. */
void GetLidarData(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data) {
if (data) {
data_recveive_count[handle] += data_num;
if (data_recveive_count[handle] % 10000 == 0) {
printf("receive packet count %d %d\n", handle, data_recveive_count[handle]);
data_recveive_count[handle] ++ ;
if (data_recveive_count[handle] % 100 == 0) {
/** 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;
}
printf("data_type %d packet num %d\n", data->data_type, data_recveive_count[handle]);
}
}
}
/** Callback function of starting sampling. */
void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
printf("OnSampleCallback statue %d handle %d response %d \n", status, handle, response);
if (status == kStatusSuccess) {
if (response != 0) {
@@ -89,11 +124,11 @@ void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *da
}
/** Callback function of stopping sampling. */
void OnStopSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
void OnStopSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data) {
}
/** Query the firmware version of Livox LiDAR. */
void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
void OnDeviceInformation(livox_status status, uint8_t handle, DeviceInformationResponse *ack, void *data) {
if (status != kStatusSuccess) {
printf("Device Query Informations Failed %d\n", status);
}
@@ -106,38 +141,57 @@ void OnDeviceInformation(uint8_t status, uint8_t handle, DeviceInformationRespon
}
}
void LidarConnect(const DeviceInfo *info) {
uint8_t handle = info->handle;
QueryDeviceInformation(handle, OnDeviceInformation, NULL);
if (devices[handle].device_state == kDeviceStateDisconnect) {
devices[handle].device_state = kDeviceStateConnect;
devices[handle].info = *info;
}
}
void LidarDisConnect(const DeviceInfo *info) {
uint8_t handle = info->handle;
devices[handle].device_state = kDeviceStateDisconnect;
}
void LidarStateChange(const DeviceInfo *info) {
uint8_t handle = info->handle;
devices[handle].info = *info;
}
/** Callback function of changing of device state. */
void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
void OnDeviceInfoChange(const DeviceInfo *info, DeviceEvent type) {
if (info == NULL) {
return;
}
printf("OnDeviceChange broadcast code %s update type %d\n", info->broadcast_code, type);
uint8_t handle = info->handle;
if (handle >= kMaxLidarCount) {
return;
}
if (type == kEventConnect) {
QueryDeviceInformation(handle, OnDeviceInformation, NULL);
if (devices[handle].device_state == kDeviceStateDisconnect) {
devices[handle].device_state = kDeviceStateConnect;
devices[handle].info = *info;
}
LidarConnect(info);
printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
} else if (type == kEventDisconnect) {
devices[handle].device_state = kDeviceStateDisconnect;
LidarDisConnect(info);
printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
} else if (type == kEventStateChange) {
devices[handle].info = *info;
LidarStateChange(info);
printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
}
if (devices[handle].device_state == kDeviceStateConnect) {
printf("Device State error_code %d\n", devices[handle].info.status.status_code);
printf("Device State working state %d\n", devices[handle].info.state);
printf("Device Working State %d\n", devices[handle].info.state);
if (devices[handle].info.state == kLidarStateInit) {
printf("Device State Change Progress %u\n", devices[handle].info.status.progress);
} else {
printf("Device State Error Code 0X%08x\n", devices[handle].info.status.status_code.error_code);
}
printf("Device feature %d\n", devices[handle].info.feature);
SetErrorMessageCallback(handle, OnLidarErrorStatusCallback);
if (devices[handle].info.state == kLidarStateNormal) {
if (devices[handle].info.type == kDeviceTypeHub) {
HubStartSampling(OnSampleCallback, NULL);
} else {
LidarStartSampling(handle, OnSampleCallback, NULL);
}
LidarStartSampling(handle, OnSampleCallback, NULL);
devices[handle].device_state = kDeviceStateSampling;
}
}
@@ -147,7 +201,7 @@ void OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
* You need to add listening device broadcast code and set the point cloud data callback in this function.
*/
void OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
if (info == NULL) {
if (info == NULL || info->dev_type == kDeviceTypeHub) {
return;
}
@@ -186,9 +240,9 @@ int SetProgramOption(int argc, const char *argv[]) {
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" },
{ "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;
@@ -280,7 +334,7 @@ int main(int argc, const char *argv[]) {
/** Set the callback function called when device state change,
* which means connection/disconnection and changing of LiDAR state.
*/
SetDeviceStateUpdateCallback(OnDeviceChange);
SetDeviceStateUpdateCallback(OnDeviceInfoChange);
/** Start the device discovering routine. */
if (!Start()) {