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 206c15522f
commit 0b62082029
1442 changed files with 52220 additions and 16430 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sdk_core/src/device_discovery.cpp
+37 -22
View File
@@ -66,7 +66,7 @@ bool DeviceDiscovery::Start(IOLoop *loop) {
return false;
}
loop_ = loop;
sock_ = util::CreateBindSocket(kListenPort, mem_pool_);
sock_ = util::CreateBindSocket(kListenPort, mem_pool_, true);
if (sock_ == NULL) {
LOG_ERROR("DeviceDiscovery Create Socket Failed");
return false;
@@ -84,7 +84,7 @@ void DeviceDiscovery::OnData(apr_socket_t *sock, void *) {
comm_port_->UpdateCacheWrIdx(size);
if (rv != APR_SUCCESS) {
LOG_ERROR(" Receive Failed {}", PrintAPRStatus(rv));
LOG_WARN(" Receive Failed {}", PrintAPRStatus(rv));
return;
}
CommPacket packet;
@@ -137,6 +137,7 @@ void DeviceDiscovery::OnTimer(apr_time_t now) {
void DeviceDiscovery::Uninit() {
if (sock_) {
loop_->RemoveDelegate(sock_, this);
apr_socket_close(sock_);
sock_ = NULL;
}
@@ -156,24 +157,10 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
return;
}
BroadcastDeviceInfo *device_info = (BroadcastDeviceInfo *)packet.data;
string broadcast_code = device_info->broadcast_code;
LOG_INFO(" Boradcast broadcast code: {}", broadcast_code);
device_manager().BroadcastDevices(device_info);
DeviceInfo lidar_info;
bool found = device_manager().FindDevice(broadcast_code, lidar_info);
if (!found || device_manager().IsDeviceConnected(lidar_info.handle)) {
return;
}
++port_count;
strncpy(lidar_info.broadcast_code, broadcast_code.c_str(), sizeof(lidar_info.broadcast_code));
lidar_info.cmd_port = kListenPort + kCmdPortOffset + port_count;
lidar_info.data_port = kListenPort + kDataPortOffset + port_count;
lidar_info.type = device_info->dev_type;
lidar_info.state = kLidarStateUnknown;
lidar_info.feature = kLidarFeatureNone;
lidar_info.status.status_code = 0;
BroadcastDeviceInfo device_info;
memcpy((void*)(&device_info),(void*)(packet.data),(sizeof(BroadcastDeviceInfo)-sizeof(device_info.ip)));
string broadcast_code = device_info.broadcast_code;
LOG_INFO(" Broadcast broadcast code: {}", broadcast_code);
char ip[16];
memset(&ip, 0, sizeof(ip));
@@ -182,6 +169,31 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
LOG_ERROR(PrintAPRStatus(rv));
return;
}
strncpy(device_info.ip, ip, sizeof(device_info.ip));
device_manager().BroadcastDevices(&device_info);
DeviceInfo lidar_info;
bool found = device_manager().FindDevice(broadcast_code, lidar_info);
if (!found) {
LOG_INFO("Broadcast code : {} not add to connect", broadcast_code);
}
if (!found || device_manager().IsDeviceConnected(lidar_info.handle)) {
return;
}
++port_count;
strncpy(lidar_info.broadcast_code, broadcast_code.c_str(), sizeof(lidar_info.broadcast_code));
lidar_info.cmd_port = kListenPort + kCmdPortOffset + port_count;
lidar_info.data_port = kListenPort + kDataPortOffset + port_count;
lidar_info.sensor_port = kListenPort + kSensorPortOffset + port_count;
lidar_info.type = device_info.dev_type;
lidar_info.state = kLidarStateUnknown;
lidar_info.feature = kLidarFeatureNone;
lidar_info.status.progress = 0;
strncpy(lidar_info.ip, ip, sizeof(lidar_info.ip));
apr_pool_t *pool = NULL;
rv = apr_pool_create(&pool, mem_pool_);
@@ -206,11 +218,12 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
do {
HandshakeRequest handshake_req;
uint32_t local_ip = 0;
if (util::FindLocalIP(addr->sa.sin, local_ip) == false) {
if (util::FindLocalIp(addr->sa.sin, local_ip) == false) {
result = false;
LOG_INFO("LocalIp and DeviceIp are not in same subnet");
break;
}
LOG_INFO(" LocalIP: {}", inet_ntoa(*(struct in_addr *)&local_ip));
LOG_INFO("LocalIP: {}", inet_ntoa(*(struct in_addr *)&local_ip));
LOG_INFO("Command Port: {}", lidar_info.cmd_port);
LOG_INFO("Data Port: {}", lidar_info.data_port);
CommPacket packet;
@@ -218,6 +231,7 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
handshake_req.ip_addr = local_ip;
handshake_req.cmd_port = lidar_info.cmd_port;
handshake_req.data_port = lidar_info.data_port;
handshake_req.sensor_port = lidar_info.sensor_port;
packet.packet_type = kCommandTypeAck;
packet.seq_num = CommandChannel::GenerateSeq();
packet.cmd_set = kCommandSetGeneral;
@@ -245,6 +259,7 @@ void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr
}
}
DeviceDiscovery &device_discovery() {
static DeviceDiscovery discovery;
return discovery;