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

feat:support Mid70 and Avia

Browse Source
This commit is contained in:
Livox-SDK
2020-10-26 20:25:42 +08:00
parent 9f316ced0a
commit eb19d42ce4
23 changed files with 441 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sample/hub/main.c
+4
View File
@@ -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);
}
sample/hub_lvx_file/lvx_file.cpp
+41 -22
View File
@@ -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;
}
}
sample/lidar/main.c
+4
View File
@@ -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]);
}
sample/lidar_lvx_file/lvx_file.cpp
+41 -21
View File
@@ -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;
}
}
sample/lidar_lvx_file/main.cpp
+1
View File
@@ -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();