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
sdk_core/src/command_handler/command_channel.cpp
+15 -13
View File
@@ -37,6 +37,7 @@ using std::make_pair;
using std::map;
using std::pair;
using std::string;
using std::chrono::steady_clock;
namespace livox {
@@ -52,9 +53,9 @@ CommandChannel::CommandChannel(apr_port_t port,
loop_(NULL),
callback_(cb),
comm_port_(new CommPort),
heartbeat_time_(0),
heartbeat_time_(),
remote_ip_(remote_ip),
last_heartbeat_(0) {}
last_heartbeat_() {}
bool CommandChannel::Bind(IOLoop *loop) {
if (loop == NULL) {
@@ -67,6 +68,7 @@ bool CommandChannel::Bind(IOLoop *loop) {
}
loop_->AddDelegate(sock_, this);
last_heartbeat_ = steady_clock::now();
return true;
}
@@ -118,11 +120,11 @@ void CommandChannel::SendAsync(const Command &command) {
}
}
void CommandChannel::OnTimer(apr_time_t now) {
void CommandChannel::OnTimer(TimePoint now) {
list<Command> timeout_commands;
map<uint16_t, pair<Command, apr_time_t> >::iterator ite = commands_.begin();
map<uint16_t, pair<Command, TimePoint> >::iterator ite = commands_.begin();
while (ite != commands_.end()) {
pair<Command, apr_time_t> &command_pair = ite->second;
pair<Command, TimePoint> &command_pair = ite->second;
if (now > command_pair.second) {
timeout_commands.push_back(command_pair.first);
commands_.erase(ite++);
@@ -132,7 +134,7 @@ void CommandChannel::OnTimer(apr_time_t now) {
}
for (list<Command>::iterator ite = timeout_commands.begin(); ite != timeout_commands.end(); ++ite) {
LOG_WARN("Apr time now: {}, Command Timeout: Set {}, Id {}, Seq {}", PrintAPRTime(apr_time_now()),
LOG_WARN("Command Timeout: Set {}, Id {}, Seq {}",
(uint16_t)ite->packet.cmd_set, ite->packet.cmd_code, ite->packet.seq_num);
if (callback_) {
ite->packet.packet_type = kCommandTypeAck;
@@ -140,7 +142,7 @@ void CommandChannel::OnTimer(apr_time_t now) {
}
}
if ((last_heartbeat_ != 0) && (now - last_heartbeat_ > apr_time_from_sec(3))) {
if (now - last_heartbeat_ > std::chrono::seconds(3)) {
DeviceDisconnect(handle_);
} else {
HeartBeat(now);
@@ -165,13 +167,13 @@ void CommandChannel::Uninit() {
}
commands_.clear();
last_heartbeat_ = 0;
heartbeat_time_ = 0;
last_heartbeat_ = {};
heartbeat_time_ = {};
remote_ip_ = "";
}
void CommandChannel::HeartBeat(apr_time_t t) {
if (heartbeat_time_ == 0 || (t - heartbeat_time_) > apr_time_from_msec(kHeartbeatTimer)) {
void CommandChannel::HeartBeat(TimePoint t) {
if (heartbeat_time_ == TimePoint() || (t - heartbeat_time_) > kHeartbeatTimer) {
heartbeat_time_ = t;
Command command(handle_,
@@ -230,7 +232,7 @@ Command CommandChannel::DeepCopy(const Command &cmd) {
}
void CommandChannel::OnHeartbeatAck(const CommPacket &) {
last_heartbeat_ = apr_time_now();
last_heartbeat_ = steady_clock::now();
}
void CommandChannel::DeviceDisconnect(uint8_t handle) {
@@ -240,7 +242,7 @@ void CommandChannel::DeviceDisconnect(uint8_t handle) {
void CommandChannel::Send(const Command &command) {
LOG_INFO(" Send Command: Set {} Id {} Seq {}", (uint16_t)command.packet.cmd_set, command.packet.cmd_code, command.packet.seq_num);
SendInternal(command);
commands_[command.packet.seq_num] = make_pair(command, apr_time_now() + apr_time_from_msec(command.time_out));
commands_[command.packet.seq_num] = make_pair(command, steady_clock::now() + std::chrono::milliseconds(command.time_out));
Command &cmd = commands_[command.packet.seq_num].first;
if (cmd.packet.data != NULL) {
delete[] cmd.packet.data;
sdk_core/src/command_handler/command_channel.h
+9 -6
View File
@@ -28,6 +28,7 @@
#include <map>
#include <list>
#include <string>
#include <chrono>
#include "apr_network_io.h"
#include "base/io_loop.h"
#include "comm/comm_port.h"
@@ -71,6 +72,8 @@ class CommandChannelDelegate {
*/
class CommandChannel : public IOLoop::IOLoopDelegate {
public:
typedef std::chrono::steady_clock::time_point TimePoint;
CommandChannel(apr_port_t port,
uint8_t handle,
const std::string &remote_ip,
@@ -95,31 +98,31 @@ class CommandChannel : public IOLoop::IOLoopDelegate {
void SendAsync(const Command &command);
void OnData(apr_socket_t *, void *);
void OnTimer(apr_time_t now);
void OnTimer(TimePoint now);
static uint16_t GenerateSeq();
private:
void Send(const Command &cmd);
void HeartBeat(apr_time_t t);
void HeartBeat(TimePoint t);
void SendInternal(const Command &command);
Command DeepCopy(const Command &cmd);
void OnHeartbeatAck(const CommPacket &packet);
void DeviceDisconnect(uint8_t handle);
private:
static const int kHeartbeatTimer = 800;
const std::chrono::milliseconds kHeartbeatTimer = std::chrono::milliseconds(800);
uint8_t handle_;
apr_port_t port_;
apr_socket_t *sock_;
apr_pool_t *mem_pool_;
IOLoop *loop_;
CommandChannelDelegate *callback_;
std::map<uint16_t, std::pair<Command, apr_time_t> > commands_;
std::map<uint16_t, std::pair<Command, TimePoint> > commands_;
std::unique_ptr<CommPort> comm_port_;
apr_time_t heartbeat_time_;
TimePoint heartbeat_time_;
std::string remote_ip_;
apr_time_t last_heartbeat_;
TimePoint last_heartbeat_;
};
} // namespace livox
sdk_core/src/command_handler/command_impl.cpp
+118 -25
View File
@@ -158,7 +158,7 @@ bool ParseRmcTime(const char* rmc, uint16_t rmc_len, LidarSetUtcSyncTimeRequest*
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;
int hour = 0, minute = 0, second = 0;
memset(utc_time_req, 0, sizeof(LidarSetUtcSyncTimeRequest));
@@ -204,23 +204,10 @@ bool ParseRmcTime(const char* rmc, uint16_t rmc_len, LidarSetUtcSyncTimeRequest*
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;
if (3 != sscanf(utc_hms_buff, "%2d%2d%2d", &hour, &minute, &second)) {
return false;
}
utc_time_req->hour = hour;
utc_time_req->mircrosecond = (minute * 60 * 1000 + second * 1000) * 1000;
return true;
@@ -391,7 +378,8 @@ livox_status RebootDevice(uint8_t handle, uint16_t timeout, CommonCommandCallbac
}
livox_status LidarEnableHighSensitivity(uint8_t handle, SetDeviceParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarTele(handle)) {
if (!device_manager().IsLidarTele(handle)
&& !device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
@@ -407,7 +395,8 @@ livox_status LidarEnableHighSensitivity(uint8_t handle, SetDeviceParametersCallb
}
livox_status LidarDisableHighSensitivity(uint8_t handle, SetDeviceParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarTele(handle)) {
if (!device_manager().IsLidarTele(handle)
&& !device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
uint8_t req_buff[kMaxCommandBufferSize] = {0};
@@ -422,7 +411,8 @@ livox_status LidarDisableHighSensitivity(uint8_t handle, SetDeviceParametersCall
}
livox_status LidarGetHighSensitivityState(uint8_t handle, GetDeviceParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarTele(handle)) {
if (!device_manager().IsLidarTele(handle)
&& !device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
@@ -432,6 +422,101 @@ livox_status LidarGetHighSensitivityState(uint8_t handle, GetDeviceParametersCal
return GetDeiveParameter(handle, &req, cb, client_data);
}
livox_status LidarSetSlotNum(uint8_t handle, uint8_t slot, SetDeviceParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarMid70(handle)
&& !device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
KeyValueParam * kv = (KeyValueParam *)&req_buff[0];
kv->key = static_cast<uint16_t>(kKeySlotNum);
kv->length = 1;
kv->value[0] = slot;
req_len = sizeof(KeyValueParam);
return SetDeviceParameters(handle, req_buff, req_len, cb, client_data);
}
livox_status LidarGetSlotNum(uint8_t handle, GetDeviceParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarMid70(handle)
&& !device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
GetDeviceParameterRequest req;
req.param_num = 1;
req.key[0] = static_cast<uint16_t>(kKeySlotNum);
return GetDeiveParameter(handle, &req, cb, client_data);
}
livox_status LidarGetScanPattern(uint8_t handle, GetDeviceParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
GetDeviceParameterRequest req;
req.param_num = 1;
req.key[0] = static_cast<uint16_t>(kKeyScanPattern);
return GetDeiveParameter(handle, &req, cb, client_data);
}
livox_status LidarSetScanPattern(uint8_t handle, LidarScanPattern pattern, SetDeviceParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
KeyValueParam * kv = (KeyValueParam *)&req_buff[0];
kv->key = static_cast<uint16_t>(kKeyScanPattern);
kv->length = 1;
kv->value[0] = static_cast<uint8_t>(pattern);
req_len = sizeof(KeyValueParam);
return SetDeviceParameters(handle, req_buff, req_len, cb, client_data);
}
livox_status DeviceResetAllParameters(uint8_t handle, DeviceResetParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarAvia(handle)
&& !device_manager().IsLidarMid70(handle)
&& !device_manager().IsLidarTele(handle)) {
return kStatusNotSupported;
}
ResetDeviceParameterRequest req = {};
req.flag = 0; //Reset all keys
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralResetDeviceParam,
(uint8_t *)&req,
sizeof(req),
MakeCommandCallback<DeviceParameterResponse>(cb, client_data));
return result;
}
livox_status DeviceResetParameters(uint8_t handle, DeviceParamKeyName * keys, uint8_t keys_num, DeviceResetParametersCallback cb, void *client_data) {
if (!device_manager().IsLidarAvia(handle)
&& !device_manager().IsLidarMid70(handle)
&& !device_manager().IsLidarTele(handle)) {
return kStatusNotSupported;
}
uint8_t req_buff[kMaxCommandBufferSize] = {0};
uint16_t req_len = 0;
ResetDeviceParameterRequest * req = (ResetDeviceParameterRequest *)&req_buff[0];
req->flag = 1; //Reset some keys
req->key_num = keys_num;
for (uint8_t i = 0; i < keys_num; i++) {
req->key[i] = static_cast<uint16_t>(keys[i]);
}
req_len = sizeof(ResetDeviceParameterRequest) + (keys_num - 1) * sizeof(uint16_t);
livox_status result = command_handler().SendCommand(handle,
kCommandSetGeneral,
kCommandIDGeneralResetDeviceParam,
(uint8_t *)req,
req_len,
MakeCommandCallback<DeviceParameterResponse>(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;
@@ -492,7 +577,9 @@ livox_status LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCall
livox_status LidarTurnOnFan(uint8_t handle, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
|| device_manager().IsLidarMid40(handle)
|| device_manager().IsLidarMid70(handle)
|| device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
return LidarFanControl(handle, true, cb, client_data);
@@ -500,7 +587,9 @@ livox_status LidarTurnOnFan(uint8_t handle, CommonCommandCallback cb, void *clie
livox_status LidarTurnOffFan(uint8_t handle, CommonCommandCallback cb, void *client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
|| device_manager().IsLidarMid40(handle)
|| device_manager().IsLidarMid70(handle)
|| device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
return LidarFanControl(handle, false, cb, client_data);
@@ -508,7 +597,9 @@ livox_status LidarTurnOffFan(uint8_t handle, CommonCommandCallback cb, void *cli
livox_status LidarGetFanState(uint8_t handle, LidarGetFanStateCallback cb, void * data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
|| device_manager().IsLidarMid40(handle)
|| device_manager().IsLidarMid70(handle)
|| device_manager().IsLidarAvia(handle)) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(handle,
@@ -551,7 +642,8 @@ livox_status LidarGetPointCloudReturnMode(uint8_t handle, LidarGetPointCloudRetu
livox_status LidarSetImuPushFrequency(uint8_t handle, ImuFreq freq, CommonCommandCallback cb, void * client_data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
|| device_manager().IsLidarMid40(handle)
|| device_manager().IsLidarMid70(handle)) {
return kStatusNotSupported;
}
uint8_t req = static_cast<uint8_t>(freq);
@@ -566,7 +658,8 @@ livox_status LidarSetImuPushFrequency(uint8_t handle, ImuFreq freq, CommonComman
livox_status LidarGetImuPushFrequency(uint8_t handle, LidarGetImuPushFrequencyCallback cb, void * data) {
if (device_manager().device_mode() != kDeviceModeLidar
|| device_manager().IsLidarMid40(handle)) {
|| device_manager().IsLidarMid40(handle)
|| device_manager().IsLidarMid70(handle)) {
return kStatusNotSupported;
}
livox_status result = command_handler().SendCommand(handle,
sdk_core/src/command_handler/command_impl.h
+1
View File
@@ -115,6 +115,7 @@ static const uint16_t GeneralCommandTimeout[kCommandIDGeneralCommandCount] = {KD
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,
KDefaultTimeOut,};
/** Enum that represents the command id. */