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
sdk_core/src/command_handler/command_channel.cpp
+10 -2
View File
@@ -149,7 +149,12 @@ void CommandChannel::OnTimer(apr_time_t now) {
void CommandChannel::Uninit() {
if (sock_) {
loop_->RemoveDelegate(sock_, this);
apr_os_thread_t thread_id = apr_os_thread_current();
if (apr_os_thread_equal(loop_->GetThreadId(), thread_id)) {
loop_->RemoveDelegateSync(sock_);
} else {
loop_->RemoveDelegate(sock_, this);
}
loop_ = NULL;
apr_socket_close(sock_);
sock_ = NULL;
@@ -195,6 +200,9 @@ void CommandChannel::SendInternal(const Command &command) {
if (rv == APR_SUCCESS) {
rv = apr_socket_sendto(sock_, sa, 0, (const char *)buf, &apr_size);
}
if (rv != APR_SUCCESS) {
(*command.cb)(kStatusSendFailed, handle_, NULL);
}
apr_pool_destroy(subpool);
}
@@ -226,7 +234,7 @@ void CommandChannel::OnHeartbeatAck(const CommPacket &) {
}
void CommandChannel::DeviceDisconnect(uint8_t handle) {
DeviceRemove(handle);
DeviceRemove(handle,kEventDisconnect);
}
void CommandChannel::Send(const Command &command) {
sdk_core/src/command_handler/command_handler.cpp
+62 -16
View File
@@ -61,6 +61,32 @@ uint16_t GetCommandTimeout(uint8_t command_set, uint8_t command_id) {
return KDefaultTimeOut;
}
bool IsSubLidarException(const Command &command) {
if (device_manager().device_mode() == kDeviceModeLidar) {
return false;
}
if (!(command.packet.cmd_set == kCommandSetGeneral &&
command.packet.cmd_code == kCommandIDGeneralPushAbnormalState)) {
return false;
}
HubErrorCode* hub_error_code = static_cast<HubErrorCode *>((void *)command.packet.data);
if (hub_error_code->lidar_link_status == 0x01) {
return true;
}
return false;
}
bool IsMidDeviceIpResponse(const Command &command) {
return command.packet.cmd_set == kCommandSetGeneral &&
command.packet.cmd_code == kCommandIDGeneralGetDeviceIpInformation &&
command.packet.data_len != sizeof(GetDeviceIpModeResponse);
}
void OnSubLidarDisconnect() {
device_manager().UpdateDevices(DeviceInfo(), kEventHubConnectionChange);
}
bool CommandHandler::AddDevice(const DeviceInfo &info) {
if (impl_ == NULL) {
DeviceMode mode = static_cast<DeviceMode>(device_manager().device_mode());
@@ -109,14 +135,14 @@ void CommandHandler::OnCommand(uint8_t handle, const Command &command) {
}
}
bool CommandHandler::SendCommand(uint8_t handle,
uint8_t command_set,
uint8_t command_id,
uint8_t *data,
uint16_t length,
const shared_ptr<CommandCallback> &cb) {
livox_status CommandHandler::SendCommand(uint8_t handle,
uint8_t command_set,
uint8_t command_id,
uint8_t *data,
uint16_t length,
const shared_ptr<CommandCallback> &cb) {
if (impl_ == NULL) {
return false;
return kStatusHandlerImplNotExist;
}
Command cmd(handle,
@@ -128,24 +154,41 @@ bool CommandHandler::SendCommand(uint8_t handle,
length,
GetCommandTimeout(command_set, command_id),
cb);
bool result = impl_->SendCommand(handle, cmd);
livox_status result = impl_->SendCommand(handle, cmd);
return result;
}
bool CommandHandler::RegisterPush(uint8_t handle,
uint8_t command_set,
uint8_t command_id,
const shared_ptr<CommandCallback> &cb) {
livox_status CommandHandler::RegisterPush(uint8_t handle,
uint8_t command_set,
uint8_t command_id,
const shared_ptr<CommandCallback> &cb) {
lock_guard<mutex> lock(mutex_);
Command req(handle, kCommandTypeMsg, command_set, command_id, 0, NULL, 0, 0, cb);
message_registers_.insert(make_pair(MakeKey(command_set, command_id), req));
return true;
return kStatusSuccess;
}
void CommandHandler::OnCommandAck(uint8_t handle, const Command &command) {
if (command.cb != NULL) {
(*command.cb)(handle, command.packet.data);
if (command.cb == NULL) {
return;
}
if (command.packet.data == NULL) {
(*command.cb)(kStatusTimeout, handle, command.packet.data);
return;
}
if (IsMidDeviceIpResponse(command)) {
GetDeviceIpModeResponse response;
memcpy(&response, command.packet.data, command.packet.data_len);
uint8_t net_mask[4] = {255, 255, 255, 0};
memcpy(&response.net_mask, net_mask, 4);
uint8_t gw_addr[4] = {192, 168, 1, 1};
memcpy(&response.gw_addr, gw_addr, 4);
(*command.cb)(kStatusSuccess, handle, (uint8_t *)&response);
return;
}
(*command.cb)(kStatusSuccess, handle, command.packet.data);
}
void CommandHandler::OnCommandMsg(uint8_t handle, const Command &command) {
@@ -161,10 +204,13 @@ void CommandHandler::OnCommandMsg(uint8_t handle, const Command &command) {
}
}
}
if (IsSubLidarException(command)) {
OnSubLidarDisconnect();
}
for (list<Command>::iterator ite = commands.begin(); ite != commands.end(); ++ite) {
if (ite->cb) {
(*ite->cb)(handle, command.packet.data);
(*ite->cb)(kStatusSuccess, handle, command.packet.data);
}
}
}
sdk_core/src/command_handler/command_handler.h
+11 -11
View File
@@ -46,17 +46,17 @@ class CommandHandler : public noncopyable {
bool AddDevice(const DeviceInfo &info);
void RemoveDevice(uint8_t handle);
bool SendCommand(uint8_t handle,
uint8_t command_set,
uint8_t command_id,
uint8_t *data,
uint16_t length,
const boost::shared_ptr<CommandCallback> &cb);
livox_status SendCommand(uint8_t handle,
uint8_t command_set,
uint8_t command_id,
uint8_t *data,
uint16_t length,
const boost::shared_ptr<CommandCallback> &cb);
bool RegisterPush(uint8_t handle,
uint8_t command_set,
uint8_t command_id,
const boost::shared_ptr<CommandCallback> &cb);
livox_status RegisterPush(uint8_t handle,
uint8_t command_set,
uint8_t command_id,
const boost::shared_ptr<CommandCallback> &cb);
void OnCommand(uint8_t handle, const Command &command);
@@ -84,7 +84,7 @@ class CommandHandlerImpl : public CommandChannelDelegate {
virtual bool AddDevice(const DeviceInfo &info) = 0;
virtual bool RemoveDevice(uint8_t handle) = 0;
virtual bool SendCommand(uint8_t handle, const Command &command) = 0;
virtual livox_status SendCommand(uint8_t handle, const Command &command) = 0;
virtual void OnCommand(uint8_t handle, const Command &command) {
if (handler_) {
sdk_core/src/command_handler/command_impl.cpp
+564 -177
View File
@@ -31,6 +31,9 @@
#include "livox_def.h"
#include "livox_sdk.h"
# define HMS_PARSE_BUF (128)
# define YY_PARSE_BUF (128)
using std::pair;
using std::vector;
using namespace livox;
@@ -43,7 +46,7 @@ void SetBroadcastCallback(DeviceBroadcastCallback cb) {
device_manager().SetDeviceBroadcastCallback(cb);
}
uint8_t AddHubToConnect(const char *broadcast_code, uint8_t *handle) {
livox_status AddHubToConnect(const char *broadcast_code, uint8_t *handle) {
bool result = device_manager().AddListeningDevice(broadcast_code, kDeviceModeHub, *handle);
if (result) {
return kStatusSuccess;
@@ -52,7 +55,7 @@ uint8_t AddHubToConnect(const char *broadcast_code, uint8_t *handle) {
}
}
uint8_t AddLidarToConnect(const char *broadcast_code, uint8_t *handle) {
livox_status AddLidarToConnect(const char *broadcast_code, uint8_t *handle) {
bool result = device_manager().AddListeningDevice(broadcast_code, kDeviceModeLidar, *handle);
if (result) {
return kStatusSuccess;
@@ -61,7 +64,7 @@ uint8_t AddLidarToConnect(const char *broadcast_code, uint8_t *handle) {
}
}
uint8_t GetConnectedDevices(DeviceInfo *devices, uint8_t *size) {
livox_status GetConnectedDevices(DeviceInfo *devices, uint8_t *size) {
if (devices == NULL || size == NULL) {
return kStatusFailure;
}
@@ -79,284 +82,668 @@ void SetDataCallback(uint8_t handle, DataCallback cb, void *client_data) {
data_handler().AddDataListener(handle, cb, client_data);
}
uint8_t DeviceSampleControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) {
livox_status DeviceSampleControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *client_data) {
uint8_t req = enable;
bool result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralControlSample,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralControlSample,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
uint8_t LidarStartSampling(uint8_t handle, CommonCommandCallback cb, void *data) {
livox_status LidarFanControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
uint8_t req = static_cast<uint8_t>(enable);
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarControlFan,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
livox_status SetDeviceIp(uint8_t handle, SetDeviceIpExtendModeRequest *req, CommonCommandCallback cb, void *client_data) {
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralConfigureStaticDynamicIp,
(uint8_t*)req,
sizeof(*req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
bool ChecksumRmc(const char* buff_begin, const char* buff_end) {
//checksum $GPRMC/$GNRMC format : *hh
const uint8_t head_size = strlen("$");
const uint8_t tail_size = strlen("hh");
char tail[3] = { 0 };
uint32_t tail_num = 0;
uint8_t crc = 0;
buff_begin += head_size;
for ( ; buff_begin < buff_end; buff_begin++) {
if (*buff_begin == '*') {
if (buff_begin + tail_size <= buff_end) {
strncpy(tail, &buff_begin[1], tail_size);
sscanf(tail, "%x", &tail_num);
crc ^= (uint8_t)tail_num;
if (crc == 0) {
return true;
}
break;
}
}
crc ^= *buff_begin;
}
return false;
}
bool ParseRmcTime(const char* rmc, uint16_t rmc_len, LidarSetUtcSyncTimeRequest* utc_time_req) {
const char* rmc_begin = strstr(rmc, "$GPRMC");
if (rmc_begin == NULL) {
rmc_begin = strstr(rmc, "$GNRMC");
}
if (rmc_begin == NULL) {
return false;
}
if (!ChecksumRmc(rmc_begin, rmc + rmc_len)) {
return false;
}
char utc_hms_buff[HMS_PARSE_BUF] = { 0 };
char utc_yy_buff[YY_PARSE_BUF] = { 0 };
int hour = 0, minute = 0, second = 0, millisecond = 0;
memset(utc_time_req, 0, sizeof(LidarSetUtcSyncTimeRequest));
if (4 != sscanf(rmc_begin,
"$G%*[NP]RMC,%[^,],%*C,%*f,%*C,%*f,%*C,%*f,%*f,%2hhu%2hhu%[^,],%*f,",
&utc_hms_buff[0],
&(utc_time_req->day),
&(utc_time_req->month),
&utc_yy_buff[0])) {
return false;
}
int time_buff_size = strlen(utc_yy_buff);
uint8_t temp_time = 0;
switch (time_buff_size) {
case sizeof("yyyy")-1:
if (strncmp(utc_yy_buff, "2000", strlen("2000")) < 0) {
return false;
}
if (1 != sscanf(utc_yy_buff, "%*C%*C%2hhu", &(utc_time_req->year))) {
return false;
}
temp_time = utc_time_req->day;
utc_time_req->day = utc_time_req->month;
utc_time_req->month = temp_time;
break;
case sizeof("yy")-1:
if (1 != sscanf(utc_yy_buff, "%2hhu", &(utc_time_req->year))) {
return false;
}
break;
default:
return false;
}
time_buff_size = strlen(utc_hms_buff);
switch (time_buff_size) {
case sizeof("hhmmss")-1:
if (3 != sscanf(utc_hms_buff, "%2d%2d%2d", &hour, &minute, &second)) {
return false;
}
break;
case sizeof("hhmmss.s")-1:
case sizeof("hhmmss.ss")-1:
case sizeof("hhmmss.sss")-1:
if (4 != sscanf(utc_hms_buff, "%2d%2d%2d.%2d", &hour, &minute, &second, &millisecond)) {
return false;
}
break;
default:
return false;
}
utc_time_req->mircrosecond = (minute * 60 * 1000 + second * 1000 + millisecond) * 1000;
return true;
}
livox_status LidarStartSampling(uint8_t handle, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar) {
return kStatusNotSupported;
}
return DeviceSampleControl(handle, true, cb, data);
return DeviceSampleControl(handle, true, cb, client_data);
}
uint8_t LidarStopSampling(uint8_t handle, CommonCommandCallback cb, void *data) {
livox_status LidarStopSampling(uint8_t handle, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar) {
return kStatusNotSupported;
}
return DeviceSampleControl(handle, false, cb, data);
return DeviceSampleControl(handle, false, cb, client_data);
}
uint8_t HubStartSampling(CommonCommandCallback cb, void *client_data) {
livox_status HubStartSampling(CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
return DeviceSampleControl(kHubDefaultHandle, true, cb, client_data);
}
uint8_t HubStopSampling(CommonCommandCallback cb, void *client_data) {
livox_status HubStopSampling(CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
return DeviceSampleControl(kHubDefaultHandle, false, cb, client_data);
}
uint8_t HubGetLidarHandle(uint8_t slot, uint8_t id) {
livox_status HubGetLidarHandle(uint8_t slot, uint8_t id) {
return (slot - 1) * 3 + id - 1;
}
uint8_t QueryDeviceInformation(uint8_t handle, DeviceInformationCallback cb, void *data) {
bool result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralDeviceInfo,
NULL,
0,
MakeCommandCallback<DeviceInformationResponse>(cb, data));
return result ? kStatusSuccess : kStatusFailure;
livox_status QueryDeviceInformation(uint8_t handle, DeviceInformationCallback cb, void *client_data) {
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralDeviceInfo,
NULL,
0,
MakeCommandCallback<DeviceInformationResponse>(cb, client_data));
return result;
}
uint8_t SetCartesianCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
livox_status DisconnectDevice(uint8_t handle, CommonCommandCallback cb, void *client_data) {
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralDisconnect,
NULL,
0,
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
livox_status SetCartesianCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
uint8_t req = 0;
bool result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralCoordinateSystem,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralCoordinateSystem,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
uint8_t SetSphericalCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
livox_status SetSphericalCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
uint8_t req = 1;
bool result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralCoordinateSystem,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralCoordinateSystem,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
uint8_t SetErrorMessageCallback(uint8_t handle, ErrorMessageCallback cb) {
bool result = command_handler().RegisterPush(
livox_status SetErrorMessageCallback(uint8_t handle, ErrorMessageCallback cb) {
livox_status result = command_handler().RegisterPush(
handle, kCommandSetGeneral, kCommandIDGeneralPushAbnormalState, MakeMessageCallback<ErrorMessage>(cb));
return result ? kStatusSuccess : kStatusFailure;
return result;
}
uint8_t SetStaticDynamicIP(uint8_t handle,
SetDeviceIPModeRequest *req,
CommonCommandCallback cb,
void *client_data) {
bool result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralConfigureStaticDynamicIP,
(uint8_t *)req,
sizeof(*req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status SetStaticDynamicIP(uint8_t handle,
SetDeviceIPModeRequest *req,
CommonCommandCallback cb,
void *client_data) {
if(device_manager().device_mode() == kDeviceModeLidar
&& !device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
SetDeviceIpExtendModeRequest ext_req;
ext_req.ip_mode = req->ip_mode;
ext_req.ip_addr = req->ip_addr;
uint8_t net_mask[4] = {255, 255, 255, 0};
memcpy(&ext_req.net_mask, net_mask, 4);
uint8_t gw_addr[4] = {192, 168, 1, 1};
memcpy(&ext_req.gw_addr, gw_addr, 4);
return SetDeviceIp(handle, &ext_req, cb, client_data);
}
uint8_t GetDeviceIPInformation(uint8_t handle, GetDeviceIPInformationCallback cb, void *client_data) {
bool result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralGetDeviceIPInformation,
NULL,
0,
MakeCommandCallback<GetDeviceIPModeResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status SetDynamicIp(uint8_t handle, CommonCommandCallback cb, void *client_data) {
SetDeviceIpExtendModeRequest req;
req.ip_mode = kLidarDynamicIpMode;
return SetDeviceIp(handle, &req, cb, client_data);
}
uint8_t LidarSetMode(uint8_t handle, LidarMode mode, CommonCommandCallback cb, void *client_data) {
livox_status SetStaticIp(uint8_t handle,
SetStaticDeviceIpModeRequest *req,
CommonCommandCallback cb,
void *client_data) {
SetDeviceIpExtendModeRequest static_ip_req = { kLidarStaticIpMode,req->ip_addr,req->net_mask,req->gw_addr};
return SetDeviceIp(handle, &static_ip_req, cb, client_data);
}
livox_status GetDeviceIpInformation(uint8_t handle, GetDeviceIpInformationCallback cb, void *client_data) {
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralGetDeviceIpInformation,
NULL,
0,
MakeCommandCallback<GetDeviceIpModeResponse>(cb, client_data));
return result;
}
livox_status RebootDevice(uint8_t handle, uint16_t timeout, CommonCommandCallback cb, void * client_data) {
if(device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralRebootDevice,
(uint8_t *)(&timeout),
sizeof(timeout),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
livox_status LidarSetMode(uint8_t handle, LidarMode mode, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar) {
return kStatusNotSupported;
}
uint8_t req = static_cast<uint8_t>(mode);
bool result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarSetMode,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarSetMode,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
uint8_t LidarSetExtrinsicParameter(uint8_t handle,
LidarSetExtrinsicParameterRequest *req,
CommonCommandCallback cb,
void *client_data) {
livox_status LidarSetExtrinsicParameter(uint8_t handle,
LidarSetExtrinsicParameterRequest *req,
CommonCommandCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarSetExtrinsicParameter,
(uint8_t *)req,
sizeof(*req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarSetExtrinsicParameter,
(uint8_t *)req,
sizeof(*req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
uint8_t LidarGetExtrinsicParameter(uint8_t handle, LidarGetExtrinsicParameterCallback cb, void *client_data) {
livox_status LidarGetExtrinsicParameter(uint8_t handle, LidarGetExtrinsicParameterCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarGetExtrinsicParameter,
NULL,
0,
MakeCommandCallback<LidarGetExtrinsicParameterResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarGetExtrinsicParameter,
NULL,
0,
MakeCommandCallback<LidarGetExtrinsicParameterResponse>(cb, client_data));
return result;
}
uint8_t LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) {
livox_status LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| !device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
uint8_t req = static_cast<uint8_t>(enable);
bool result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarControlRainFogSuppression,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarControlRainFogSuppression,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
uint8_t HubQueryLidarInformation(HubQueryLidarInformationCallback cb, void *client_data) {
livox_status LidarTurnOnFan(uint8_t handle, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
return LidarFanControl(handle, true, cb, client_data);
}
livox_status LidarTurnOffFan(uint8_t handle, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
return LidarFanControl(handle, false, cb, client_data);
}
livox_status LidarGetFanState(uint8_t handle, LidarGetFanStateCallback cb, void * data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarGetFanState,
NULL,
0,
MakeCommandCallback<LidarGetFanStateResponse>(cb, data));
return result;
}
livox_status LidarSetPointCloudReturnMode(uint8_t handle, PointCloudReturnMode mode, CommonCommandCallback cb, void * data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
uint8_t req = static_cast<uint8_t>(mode);
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarSetPointCloudReturnMode,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, data));
return result;
}
livox_status LidarGetPointCloudReturnMode(uint8_t handle, LidarGetPointCloudReturnModeCallback cb, void * client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarGetPointCloudReturnMode,
NULL,
0,
MakeCommandCallback<LidarGetPointCloudReturnModeResponse>(cb, client_data));
return result;
}
livox_status LidarSetImuPushFrequency(uint8_t handle, ImuFreq freq, CommonCommandCallback cb, void * client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
uint8_t req = static_cast<uint8_t>(freq);
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarSetImuPushFrequency,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
livox_status LidarGetImuPushFrequency(uint8_t handle, LidarGetImuPushFrequencyCallback cb, void * data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarGetImuPushFrequency,
NULL,
0,
MakeCommandCallback<LidarGetImuPushFrequencyResponse>(cb, data));
return result;
}
livox_status HubQueryLidarInformation(HubQueryLidarInformationCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubQueryLidarInformation,
NULL,
0,
MakeCommandCallback<HubQueryLidarInformationResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubQueryLidarInformation,
NULL,
0,
MakeCommandCallback<HubQueryLidarInformationResponse>(cb, client_data));
return result;
}
uint8_t HubSetMode(HubSetModeRequest *req, uint16_t length, HubSetModeCallback cb, void *client_data) {
livox_status HubSetMode(HubSetModeRequest *req, uint16_t length, HubSetModeCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubSetMode,
(uint8_t *)req,
length,
MakeCommandCallback<HubSetModeResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubSetMode,
(uint8_t *)req,
length,
MakeCommandCallback<HubSetModeResponse>(cb, client_data));
return result;
}
uint8_t HubControlSlotPower(HubControlSlotPowerRequest *req, CommonCommandCallback cb, void *client_data) {
livox_status HubControlSlotPower(HubControlSlotPowerRequest *req, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubControlSlotPower,
(uint8_t *)req,
sizeof(*req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubControlSlotPower,
(uint8_t *)req,
sizeof(*req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
uint8_t HubSetExtrinsicParameter(HubSetExtrinsicParameterRequest *req,
uint16_t length,
HubSetExtrinsicParameterCallback cb,
void *client_data) {
livox_status HubSetExtrinsicParameter(HubSetExtrinsicParameterRequest *req,
uint16_t length,
HubSetExtrinsicParameterCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubSetExtrinsicParameter,
(uint8_t *)req,
length,
MakeCommandCallback<HubSetExtrinsicParameterResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubSetExtrinsicParameter,
(uint8_t *)req,
length,
MakeCommandCallback<HubSetExtrinsicParameterResponse>(cb, client_data));
return result;
}
uint8_t HubGetExtrinsicParameter(HubGetExtrinsicParameterCallback cb, void *client_data) {
livox_status HubGetExtrinsicParameter(HubGetExtrinsicParameterCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubGetExtrinsicParameter,
NULL,
0,
MakeCommandCallback<HubGetExtrinsicParameterResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubGetExtrinsicParameter,
NULL,
0,
MakeCommandCallback<HubGetExtrinsicParameterResponse>(cb, client_data));
return result;
}
uint8_t HubQueryLidarStatus(HubQueryLidarStatusCallback cb, void *client_data) {
livox_status HubQueryLidarStatus(HubQueryLidarStatusCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubQueryLidarDeviceStatus,
NULL,
0,
MakeCommandCallback<HubQueryLidarStatusResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubQueryLidarDeviceStatus,
NULL,
0,
MakeCommandCallback<HubQueryLidarStatusResponse>(cb, client_data));
return result;
}
uint8_t HubExtrinsicParameterCalculation(bool enable, CommonCommandCallback cb, void *client_data) {
livox_status HubExtrinsicParameterCalculation(bool enable, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
uint8_t req = static_cast<uint8_t>(enable);
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubExtrinsicParameterCalculation,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubExtrinsicParameterCalculation,
&req,
sizeof(req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
uint8_t HubRainFogSuppress(HubRainFogSuppressRequest *req,
uint16_t length,
HubRainFogSuppressCallback cb,
livox_status HubRainFogSuppress(HubRainFogSuppressRequest *req,
uint16_t length,
HubRainFogSuppressCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubRainFogSuppression,
(uint8_t *)req,
length,
MakeCommandCallback<HubRainFogSuppressResponse>(cb, client_data));
return result;
}
livox_status HubQuerySlotPowerStatus(HubQuerySlotPowerStatusCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubQuerySlotPowerStatus,
NULL,
0,
MakeCommandCallback<HubQuerySlotPowerStatusResponse>(cb, client_data));
return result;
}
livox_status HubFanControl(HubFanControlRequest *req,
uint16_t length,
HubFanControlCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubRainFogSuppression,
(uint8_t *)req,
length,
MakeCommandCallback<HubRainFogSuppressResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubControlFan,
(uint8_t *)req,
length,
MakeCommandCallback<HubFanControlResponse>(cb, client_data));
return result;
}
uint8_t HubQuerySlotPowerStatus(HubQuerySlotPowerStatusCallback cb, void *client_data) {
livox_status HubGetFanState(HubGetFanStateRequest *req,
uint16_t length,
HubGetFanStateCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
bool result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubQuerySlotPowerStatus,
NULL,
0,
MakeCommandCallback<HubQuerySlotPowerStatusResponse>(cb, client_data));
return result ? kStatusSuccess : kStatusFailure;
}
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubGetFanState,
(uint8_t *)req,
length,
MakeCommandCallback<HubGetFanStateResponse>(cb, client_data));
return result;
}
livox_status HubSetPointCloudReturnMode(HubSetPointCloudReturnModeRequest *req,
uint16_t length,
HubSetPointCloudReturnModeCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubSetPointCloudReturnMode,
(uint8_t *)req,
length,
MakeCommandCallback<HubSetPointCloudReturnModeResponse>(cb, client_data));
return result;
}
livox_status HubGetPointCloudReturnMode(HubGetPointCloudReturnModeRequest *req,
uint16_t length,
HubGetPointCloudReturnModeCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubGetPointCloudReturnMode,
(uint8_t *)req,
length,
MakeCommandCallback<HubGetPointCloudReturnModeResponse>(cb, client_data));
return result;
}
livox_status HubSetImuPushFrequency(HubSetImuPushFrequencyRequest *req,
uint16_t length,
HubSetImuPushFrequencyCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubSetImuPushFrequency,
(uint8_t *)req,
length,
MakeCommandCallback<HubSetImuPushFrequencyResponse>(cb, client_data));
return result;
}
livox_status HubGetImuPushFrequency(HubGetImuPushFrequencyRequest *req,
uint16_t length,
HubGetImuPushFrequencyCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeHub) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(kHubDefaultHandle,
kCommandSetHub,
kCommandIDHubGetImuPushFrequency,
(uint8_t *)req,
length,
MakeCommandCallback<HubGetImuPushFrequencyResponse>(cb, client_data));
return result;
}
livox_status LidarSetUtcSyncTime(uint8_t handle,
LidarSetUtcSyncTimeRequest* req,
CommonCommandCallback cb ,
void *client_data) {
livox_status result = command_handler().SendCommand(handle,
kCommandSetLidar,
kCommandIDLidarSetSyncTime,
(uint8_t *)req,
sizeof(*req),
MakeCommandCallback<uint8_t>(cb, client_data));
return result;
}
livox_status LidarSetRmcSyncTime(uint8_t handle,
const char* rmc,
uint16_t rmc_length,
CommonCommandCallback cb,
void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
return kStatusNotSupported;
}
LidarSetUtcSyncTimeRequest utc_time_req;
if (!ParseRmcTime(rmc, rmc_length, &utc_time_req)) {
return kStatusFailure;
}
return LidarSetUtcSyncTime(handle, &utc_time_req, cb, client_data);
}
sdk_core/src/command_handler/command_impl.h
+85 -3
View File
@@ -75,11 +75,15 @@ typedef enum {
/**
* General command set, set the IP of the a device.
*/
kCommandIDGeneralConfigureStaticDynamicIP = 8,
kCommandIDGeneralConfigureStaticDynamicIp = 8,
/**
* General command set, get the IP of the a device.
*/
kCommandIDGeneralGetDeviceIPInformation = 9,
kCommandIDGeneralGetDeviceIpInformation = 9,
/**
* General command set, reboot a device.
*/
kCommandIDGeneralRebootDevice = 0x0a,
/**
* ******************************************************
@@ -97,6 +101,7 @@ static const uint16_t GeneralCommandTimeout[kCommandIDGeneralCommandCount] = {KD
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut};
/** Enum that represents the command id. */
@@ -117,7 +122,34 @@ typedef enum {
* Lidar command set, enable or disable the rain/fog suppression of a LiDAR.
*/
kCommandIDLidarControlRainFogSuppression = 3,
/**
* Lidar command set, turn on\off fan of a LiDAR.
*/
kCommandIDLidarControlFan = 4,
/**
* Lidar command set, get fan state of a LiDAR.
*/
kCommandIDLidarGetFanState = 5,
/**
* Lidar command set, set point cloud return mode of a LiDAR.
*/
kCommandIDLidarSetPointCloudReturnMode = 6,
/**
* Lidar command set, get point cloud return mode of a LiDAR.
*/
kCommandIDLidarGetPointCloudReturnMode = 7,
/**
* Lidar command set, set IMU push frequency of a LiDAR.
*/
kCommandIDLidarSetImuPushFrequency = 8,
/**
* Lidar command set, get IMU push frequency of a LiDAR.
*/
kCommandIDLidarGetImuPushFrequency = 9,
/**
* Lidar command set, set synchronization time of a LiDAR.
*/
kCommandIDLidarSetSyncTime = 0x0a,
/**
* ******************************************************
* Don't add command id after kCommandIDLidarCommandCount.
@@ -126,6 +158,13 @@ typedef enum {
} LidarCommandID;
static const uint16_t LidarCommandTimeout[kCommandIDLidarCommandCount] = {KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut};
@@ -168,6 +207,30 @@ typedef enum {
* Hub command set, get the power supply state of each hub slot.
*/
kCommandIDHubQuerySlotPowerStatus = 8,
/**
* Hub command set, turn on\off fan of the connected Livox LiDAR.
*/
kCommandIDHubControlFan = 9,
/**
* Hub command set, get the fan state of the connected Livox LiDAR.
*/
kCommandIDHubGetFanState = 0x0a,
/**
* Hub command set, set point cloud return mode of the connected Livox LiDAR.
*/
kCommandIDHubSetPointCloudReturnMode = 0x0b,
/**
* Hub command set, get point cloud return mode of the connected Livox LiDAR.
*/
kCommandIDHubGetPointCloudReturnMode = 0x0c,
/**
* Hub command set, set IMU push frequency of the connected Livox LiDAR.
*/
kCommandIDHubSetImuPushFrequency = 0x0d,
/**
* Hub command set, get IMU push frequency of the connected Livox LiDAR.
*/
kCommandIDHubGetImuPushFrequency = 0x0e,
/**
* ******************************************************
@@ -177,6 +240,12 @@ typedef enum {
} HubCommandID;
static const uint16_t HubCommandTimeout[kCommandIDHubCommandCount] = {KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
@@ -197,5 +266,18 @@ typedef enum {
/** message type, which is sent at a specified frequency. */
kCommandTypeMsg = 2
} CommandType;
#pragma pack(1)
/**
* The request body of the command for setting device's IP mode.
*/
typedef struct {
uint8_t ip_mode; /**< IP address mode: 0 for dynamic IP address, 1 for static IP address. */
uint32_t ip_addr; /**< IP address. */
uint32_t net_mask; /**< Subnet mask. */
uint32_t gw_addr; /**< Gateway address. */
} SetDeviceIpExtendModeRequest;
#pragma pack()
} // namespace livox
#endif // LIVOX_SDK_COMMAND_IMPL_H
sdk_core/src/command_handler/hub_command_handler.cpp
+3 -3
View File
@@ -46,12 +46,12 @@ bool HubCommandHandlerImpl::AddDevice(const DeviceInfo &info) {
return true;
}
bool HubCommandHandlerImpl::SendCommand(uint8_t, const Command &command) {
livox_status HubCommandHandlerImpl::SendCommand(uint8_t, const Command &command) {
if (channel_ == NULL) {
return false;
return kStatusChannelNotExist;
}
channel_->SendAsync(command);
return true;
return kStatusSuccess;
}
bool HubCommandHandlerImpl::RemoveDevice(uint8_t) {
sdk_core/src/command_handler/hub_command_handler.h
+1 -1
View File
@@ -36,7 +36,7 @@ class HubCommandHandlerImpl : public CommandHandlerImpl {
void Uninit();
bool AddDevice(const DeviceInfo &info);
bool RemoveDevice(uint8_t handle);
bool SendCommand(uint8_t handle, const Command &command);
livox_status SendCommand(uint8_t handle, const Command &command);
private:
apr_pool_t *mem_pool_;
sdk_core/src/command_handler/lidar_command_handler.cpp
+11 -6
View File
@@ -42,20 +42,25 @@ bool LidarCommandHandlerImpl::AddDevice(const DeviceInfo &info) {
return true;
}
bool LidarCommandHandlerImpl::SendCommand(uint8_t handle, const Command &command) {
livox_status LidarCommandHandlerImpl::SendCommand(uint8_t handle, const Command &command) {
CommandChannel *channel = NULL;
bool found = false;
for (list<DeviceItem>::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) {
if (ite->info.handle == handle) {
found = true;
channel = ite->channel.get();
break;
}
}
if (channel) {
channel->SendAsync(command);
return true;
if (!found) {
return kStatusInvalidHandle;
}
return false;
if (!channel) {
return kStatusChannelNotExist;
}
channel->SendAsync(command);
return kStatusSuccess;
}
bool LidarCommandHandlerImpl::RemoveDevice(uint8_t handle) {
sdk_core/src/command_handler/lidar_command_handler.h
+1 -1
View File
@@ -38,7 +38,7 @@ class LidarCommandHandlerImpl : public CommandHandlerImpl {
bool AddDevice(const DeviceInfo &info);
bool RemoveDevice(uint8_t handle);
bool SendCommand(uint8_t handle, const Command &command);
livox_status SendCommand(uint8_t handle, const Command &command);
private:
typedef struct {