diff --git a/sdk_core/CMakeLists.txt b/sdk_core/CMakeLists.txt new file mode 100644 index 0000000..3b5d7c3 --- /dev/null +++ b/sdk_core/CMakeLists.txt @@ -0,0 +1,86 @@ +cmake_minimum_required(VERSION 3.0) + +set(SDK_LIBRARY ${PROJECT_NAME}_static) +add_library(${SDK_LIBRARY} STATIC "") + +find_package(PkgConfig) +pkg_check_modules(APR apr-1) +if (APR_FOUND) + target_include_directories(${SDK_LIBRARY} + PRIVATE + ${APR_INCLUDE_DIRS}) + target_link_libraries(${SDK_LIBRARY} + PRIVATE + ${APR_LIBRARIES}) +elseif () + message("Can't find Apache Runtime Library, please install.") +endif () + + +find_package(Boost 1.54 REQUIRED COMPONENTS system) +if (Boost_FOUND AND Boost_VERSION LESS 106900) + target_link_libraries(${SDK_LIBRARY} + PRIVATE + ${Boost_LIBRARIES}) +endif () + +target_include_directories(${SDK_LIBRARY} + PUBLIC + include + include/third_party/FastCRC + PRIVATE + src) + +set_target_properties(${SDK_LIBRARY} PROPERTIES PUBLIC_HEADER "include/livox_def.h;include/livox_sdk.h") + +target_compile_options(${SDK_LIBRARY} + PRIVATE $<$:-Wall -Werror -Wno-c++11-long-long> + PRIVATE $<$:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long> + PRIVATE $<$:-Wno-unknown-pragmas -Wall -Werror -Wno-c++11-long-long>) + +target_sources(${SDK_LIBRARY} + PRIVATE + src/third_party/FastCRC/FastCRC_tables.hpp + src/third_party/FastCRC/FastCRCsw.cpp + src/base/io_loop.h + src/base/io_loop.cpp + src/base/thread_base.h + src/base/thread_base.cpp + src/base/io_thread.h + src/base/io_thread.cpp + src/base/network_util.h + src/base/network_util.cpp + src/base/logging.h + src/base/noncopyable.h + src/base/util.cpp + src/livox_sdk.cpp + src/device_discovery.h + src/device_discovery.cpp + src/device_manager.h + src/device_manager.cpp + src/comm/comm_port.cpp + src/comm/sdk_protocol.h + src/comm/sdk_protocol.cpp + src/data_handler/data_handler.h + src/data_handler/data_handler.cpp + src/data_handler/hub_data_handler.h + src/data_handler/hub_data_handler.cpp + src/data_handler/lidar_data_handler.h + src/data_handler/lidar_data_handler.cpp + src/command_handler/command_handler.h + src/command_handler/command_handler.cpp + src/command_handler/command_channel.h + src/command_handler/command_channel.cpp + src/command_handler/hub_command_handler.h + src/command_handler/hub_command_handler.cpp + src/command_handler/lidar_command_handler.h + src/command_handler/lidar_command_handler.cpp + src/command_handler/command_impl.cpp + src/command_handler/command_impl.h) + + +install(TARGETS ${SDK_LIBRARY} + PUBLIC_HEADER DESTINATION include + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) + diff --git a/sdk_core/include/comm/comm_port.h b/sdk_core/include/comm/comm_port.h new file mode 100644 index 0000000..4568ba6 --- /dev/null +++ b/sdk_core/include/comm/comm_port.h @@ -0,0 +1,72 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef COMM_COMM_PORT_H_ +#define COMM_COMM_PORT_H_ + +#include +#include "protocol.h" + +namespace livox { +const uint32_t kCacheSize = 8192; +const uint32_t kMoveCacheLimit = 1536; + +typedef struct { + uint8_t buf[kCacheSize]; + uint32_t rd_idx; + uint32_t wr_idx; + uint32_t size; +} PortCache; + +class CommPort { + public: + CommPort(); + + ~CommPort(); + + int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet); + + int32_t ParseCommStream(CommPacket *o_pack); + + uint8_t *FetchCacheFreeSpace(uint32_t *o_len); + + int32_t UpdateCacheWrIdx(uint32_t used_size); + + uint16_t GetAndUpdateSeqNum(); + + private: + uint32_t GetCacheTailSize(); + + uint32_t GetValidDataSize(); + + void UpdateCache(void); + + PortCache cache_; + Protocol *protocol_; + uint32_t parse_step_; + uint16_t seq_num_; +}; + +} // namespace livox +#endif // COMM_COMM_PORT_H_ diff --git a/sdk_core/include/comm/protocol.h b/sdk_core/include/comm/protocol.h new file mode 100644 index 0000000..f598308 --- /dev/null +++ b/sdk_core/include/comm/protocol.h @@ -0,0 +1,82 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef COMM_PROTOCOL_H_ +#define COMM_PROTOCOL_H_ + +#include + +namespace livox { +typedef struct CommPacket CommPacket; + +typedef int (*RequestPackCb)(CommPacket *packet); + +typedef enum { kRequestPack, kAckPack, kMsgPack } PacketType; + +typedef enum { kLidarSdk, kRsvd1, kProtocolUndef } ProtocolType; + +typedef enum { kNoNeed, kNeedAck, kDelayAck } NeedAckType; + +typedef enum { kParseSuccess, kParseFail } ParseResult; + +typedef struct CommPacket { + uint8_t packet_type; + uint8_t protocol; + uint8_t protocol_version; + uint8_t cmd_set; + uint32_t cmd_code; + uint32_t sender; + uint32_t sub_sender; + uint32_t receiver; + uint32_t sub_receiver; + uint32_t seq_num; + uint8_t *data; + uint16_t data_len; + uint32_t padding; + // RequestPackCb *ack_request_cb; + // uint32_t retry_times; + // uint32_t timeout; +} CommPacket; + +class Protocol { + public: + virtual ~Protocol(){}; + + virtual int32_t ParsePacket(uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) = 0; + + virtual int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet) = 0; + + virtual uint32_t GetPreambleLen() = 0; + + virtual uint32_t GetPacketWrapperLen() = 0; + + virtual uint32_t GetPacketLen(uint8_t *buf) = 0; + + virtual int32_t CheckPreamble(uint8_t *buf) = 0; + + virtual int32_t CheckPacket(uint8_t *buf) = 0; +}; + +} // namespace livox +#endif // COMM_PROTOCOL_H_ diff --git a/sdk_core/include/livox_def.h b/sdk_core/include/livox_def.h new file mode 100644 index 0000000..b621ac1 --- /dev/null +++ b/sdk_core/include/livox_def.h @@ -0,0 +1,383 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_DEF_H_ +#define LIVOX_DEF_H_ + +#include + +#define kMaxLidarCount 32 + +/** Device type. */ +typedef enum { + kDeviceTypeHub = 0, /**< Livox Hub. */ + kDeviceTypeLidarMid40 = 1, /**< Mid-40. */ + kDeviceTypeLidarTele = 2, /**< Tele. */ + kDeviceTypeLidarHorizon = 3 /**< Horizon. */ +} DeviceType; + +/** Lidar state. */ +typedef enum { + kLidarStateInit = 0, /**< Initialization state. */ + kLidarStateNormal = 1, /**< Normal work state. */ + kLidarStatePowerSaving = 2, /**< Power-saving state. */ + kLidarStateStandBy = 3, /**< Standby state. */ + kLidarStateError = 4, /**< Error state. */ + kLidarStateUnknown = 5 +} LidarState; + +/** Lidar mode. */ +typedef enum { + kLidarModeNormal = 1, /**< Normal mode. */ + kLidarModePowerSaving = 2, /**< Power-saving mode. */ + kLidarModeStandby = 3 /**< Standby mode. */ +} LidarMode; + +/** Lidar feature. */ +typedef enum { + kLidarFeatureNone = 0, /**< No feature. */ + kLidarFeatureRainFog = 1 /**< Rain and fog feature. */ +} LidarFeature; + +/** Function return value definition. */ +typedef enum { + kStatusSuccess = 0, /**< Success. */ + kStatusFailure = 1, /**< Failure. */ + kStatusNotConnected = 2, /**< Requested device is not connected. */ + kStatusNotSupported = 3, /**< Operation is not supported on this device. */ + kStatusTimeout = 4, /**< Operation timeouts. */ + kStatusNotEnoughMemory = 5 /**< No enough memory. */ +} ResponseStatus; + +/** Device update type, indicating the change of device connection or working state. */ +typedef enum { + kEventConnect = 0, /**< Device is connected. */ + kEventDisconnect = 1, /**< Device is removed. */ + kEventStateChange = 2 /**< Device working state changes or an error occurs. */ +} DeviceEvent; + +/** Timestamp sync mode define. */ +typedef enum { + kTimestampTypeNoSync = 0, /**< No sync signal mode. */ + kTimestampTypePtp = 1, /**< 1588v2.0 PTP sync mode. */ + kTimestampTypeRsvd = 2, /**< Reserved use. */ + kTimestampTypePpsGps = 3, /**< pps+gps sync mode. */ + kTimestampTypePps = 4, /**< pps only sync mode. */ + kTimestampTypeUnknown = 5 /**< Unknown mode. */ +} TimestampType; + +#pragma pack(1) + +#define kBroadcastCodeSize 16 + +/** Cartesian coordinate format. */ +typedef struct { + int32_t x; /**< X axis, Unit:mm */ + int32_t y; /**< Y axis, Unit:mm */ + int32_t z; /**< Z axis, Unit:mm */ + uint8_t reflectivity; /**< Reflectivity */ +} LivoxRawPoint; + +/** standard point cloud format */ +typedef struct { + float x; /**< X axis, Unit:m */ + float y; /**< X axis, Unit:m */ + float z; /**< X axis, Unit:m */ + uint8_t reflectivity; /**< Reflectivity */ +} LivoxPoint; + +/** Point cloud packet. */ +typedef struct { + uint8_t version; /**< Packet protocol version. */ + uint8_t slot; /**< Slot number used for connecting LiDAR. */ + uint8_t id; /**< LiDAR id. */ + uint8_t rsvd; /**< Reserved. */ + uint32_t err_code; /**< Device error status indicator information. */ + uint8_t timestamp_type; /**< Timestamp type. */ + /** Point cloud coordinate format, 1 for spherical coordinate data, 0 for cartesian coordinate data. */ + uint8_t data_type; + uint8_t timestamp[8]; /**< Nanosecond or UTC format timestamp. */ + uint8_t data[1]; /**< Point cloud data. */ +} LivoxEthPacket; + +/** Information of LiDAR work state. */ +typedef union { + uint32_t status_code; /**< LiDAR work state status code. */ + uint32_t progress; /**< LiDAR work state switching progress. */ +} StatusUnion; + +/** Information of the connected LiDAR or hub. */ +typedef struct { + char broadcast_code[kBroadcastCodeSize]; /**< Device broadcast code, null-terminated string, 15 characters at most. */ + uint8_t handle; /**< Device handle. */ + uint8_t slot; /**< Slot number used for connecting LiDAR. */ + uint8_t id; /**< LiDAR id. */ + uint32_t type; /**< Device type, refer to \ref DeviceType. */ + uint16_t data_port; /**< Point cloud data UDP port. */ + uint16_t cmd_port; /**< Control command UDP port. */ + char ip[16]; /**< IP address. */ + LidarState state; /**< LiDAR state. */ + LidarFeature feature; /**< LiDAR feature. */ + StatusUnion status; /** LiDAR work state status. */ +} DeviceInfo; + +/** The information of broadcast device. */ +typedef struct { + char broadcast_code[kBroadcastCodeSize]; /**< Device broadcast code, null-terminated string, 15 characters at most. */ + uint8_t dev_type; /**< Device type, refer to \ref DeviceType. */ + uint16_t reserved; /**< Reserved. */ +} BroadcastDeviceInfo; + +/** The information of LiDAR units that are connected to the Livox Hub. */ +typedef struct { + char broadcast_code[kBroadcastCodeSize]; /**< Device broadcast code, null-terminated string, 15 characters at most. */ + uint8_t dev_type; /**< Device type, refer to \ref DeviceType. */ + uint8_t version[4]; /**< Firmware version. */ + uint8_t slot; /**< Slot number used for connecting LiDAR units. */ + uint8_t id; /**< Device id. */ +} ConnectedLidarInfo; + +/** LiDAR mode configuration information. */ +typedef struct { + char broadcast_code[kBroadcastCodeSize]; /**< Device broadcast code, null-terminated string, 15 characters at most. */ + uint8_t state; /**< LiDAR state. */ +} LidarModeRequestItem; + +typedef struct { + uint8_t ret_code; /**< Return code. */ + char broadcast_code[kBroadcastCodeSize]; /**< Device broadcast code. */ +} ReturnCode; + +typedef struct { + char broadcast_code[kBroadcastCodeSize]; /**< Device broadcast code. */ +} DeviceBroadcastCode; + +typedef struct { + char broadcast_code[kBroadcastCodeSize]; /**< Device broadcast code. */ + uint8_t feature; /**< Close or open the rain and fog feature. */ +} RainFogSuppressRequestItem; + +/** LiDAR configuration information. */ +typedef struct { + char broadcast_code[kBroadcastCodeSize]; /**< Device broadcast code. */ + float roll; /**< Roll angle, unit: degree. */ + float pitch; /**< Pitch angle, unit: degree. */ + float yaw; /**< Yaw angle, unit: degree. */ + int32_t x; /**< X translation, unit: mm. */ + int32_t y; /**< Y translation, unit: mm. */ + int32_t z; /**< Z translation, unit: mm. */ +} ExtrinsicParameterRequestItem; + +/** LiDAR extrinsic parameters. */ +typedef struct { + uint8_t ret_code; /**< Return code. */ + char broadcast_code[kBroadcastCodeSize]; /**< Broadcast code. */ + float roll; /**< Roll angle, unit: degree. */ + float pitch; /**< Pitch angle, unit: degree. */ + float yaw; /**< Yaw angle, unit: degree. */ + int32_t x; /**< X translation, unit: mm. */ + int32_t y; /**< Y translation, unit: mm. */ + int32_t z; /**< Z translation, unit: mm. */ +} ExtrinsicParameterResponseItem; + +typedef struct { + char broadcast_code[kBroadcastCodeSize]; /**< Broadcast code. */ + uint8_t state; /**< LiDAR state. */ + uint8_t feature; /**< LiDAR feature. */ + StatusUnion error_union; /** LiDAR work state. */ +} LidarStateItem; + +/** + * The request body for the command of handshake. + */ +typedef struct { + uint32_t ip_addr; /**< IP address of the device. */ + uint16_t data_port; /**< UDP port of the data connection. */ + uint16_t cmd_port; /**< UDP port of the command connection. */ +} HandshakeRequest; + +/** + * The response body of querying device information. + */ +typedef struct { + uint8_t ret_code; /**< Return code. */ + uint8_t firmware_version[4]; /**< Firmware version. */ +} DeviceInformationResponse; + +/** + * The response body of getting device error status. + */ +typedef struct { + uint32_t error_code; /**< Error code. */ +} ErrorMessage; + +/** + * 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. */ +} SetDeviceIPModeRequest; + +/** + * The response body of getting device's IP mode. + */ +typedef struct { + uint8_t ret_code; /**< Return code. */ + uint8_t ip_mode; /**< IP address mode: 0 for dynamic IP address, 1 for static IP address. */ + uint32_t ip_addr; /**< IP address. */ +} GetDeviceIPModeResponse; + +/** + * The body of heartbeat response. + */ +typedef struct { + uint8_t ret_code; /**< Return code. */ + uint8_t state; /**< Working state. */ + uint8_t feature; /**< LiDAR feature. */ + StatusUnion error_union; /**< LiDAR work state. */ +} HeartbeatResponse; + +/** + * The request body for the command of setting LiDAR's parameters. + */ +typedef struct { + float roll; /**< Roll angle, unit: degree. */ + float pitch; /**< Pitch angle, unit: degree. */ + float yaw; /**< Yaw angle, unit: degree. */ + int32_t x; /**< X translation, unit: mm. */ + int32_t y; /**< Y translation, unit: mm. */ + int32_t z; /**< Z translation, unit: mm. */ +} LidarSetExtrinsicParameterRequest; + +/** + * The response body of getting LiDAR's parameters. + */ +typedef struct { + uint8_t ret_code; + float roll; /**< Roll angle, unit: degree. */ + float pitch; /**< Pitch angle, unit: degree. */ + float yaw; /**< Yaw angle, unit: degree. */ + int32_t x; /**< X translation, unit: mm. */ + int32_t y; /**< Y translation, unit: mm. */ + int32_t z; /**< Z translation, unit: mm. */ +} LidarGetExtrinsicParameterResponse; + +/** + * The response body of querying the information of LiDAR units connected to the Livox Hub. + */ +typedef struct { + uint8_t ret_code; /**< Return code from device. */ + uint8_t count; /**< Count of device_info_list. */ + ConnectedLidarInfo device_info_list[1]; /**< Connected lidars information. */ +} HubQueryLidarInformationResponse; + +/** + * The request body of setting LiDAR units working mode. + */ +typedef struct { + uint8_t count; /**< Number of LiDAR units to set. */ + LidarModeRequestItem config_list[1]; /**< LiDAR mode configuration list. */ +} HubSetModeRequest; + +/** + * The response of setting LiDAR units working mode. + */ +typedef struct { + uint8_t ret_code; /**< Return code from device. */ + uint8_t count; /**< Count of ret_state_list. */ + ReturnCode ret_state_list[1]; /**< Return status list. */ +} HubSetModeResponse; + +/** + * The request body of toggling the power supply of the slot. + */ +typedef struct { + uint8_t slot; /**< Slot of the hub. */ + uint8_t state; /**< Status of toggling the power supply. */ +} HubControlSlotPowerRequest; + +/** + * The request body of setting the Livox Hub's parameters. + */ +typedef struct { + uint8_t count; /**< Count of cfg_param_list. */ + ExtrinsicParameterRequestItem parameter_list[1]; /**< Configuration parameter list. */ +} HubSetExtrinsicParameterRequest; + +/** + * The response body of setting the Livox Hub's parameters. + */ +typedef struct { + uint8_t ret_code; /**< Return code. */ + uint8_t count; /**< Count of ret_code_list. */ + ReturnCode ret_code_list[1]; /**< Return code */ +} HubSetExtrinsicParameterResponse; + +/** + * The request body of getting the Livox Hub's parameters. + */ +typedef struct { + uint8_t count; /**< Count of code_list. */ + DeviceBroadcastCode code_list[1]; /**< Broadcast code list. */ +} HubGetExtrinsicParameterRequest; + +/** + * The response body of getting the Livox Hub's parameters. + */ +typedef struct { + uint8_t ret_code; /**< Return code. */ + uint8_t count; /**< Number of LiDAR units connected to the Livox Hub. */ + ExtrinsicParameterResponseItem parameter_list[1]; /**< Postion parameters of connected LiDAR unit(s). */ +} HubGetExtrinsicParameterResponse; + +/** + * The response body of getting sub LiDAR's state. + */ +typedef struct { + uint8_t ret_code; /**< Return code. */ + uint8_t count; /**< Number of LiDAR connected to the Livox Hub. */ + LidarStateItem state_list[1]; /**< Device information of connected LiDAR units. */ +} HubQueryLidarStatusResponse; + +/** + * The request body of toggling the LiDAR units rain and fog mode. + */ +typedef struct { + uint8_t count; /**< Number of LiDAR units connected to the Livox Hub. */ + RainFogSuppressRequestItem lidar_cfg_list[1]; /**< Command data of connected LiDAR units. */ +} HubRainFogSuppressRequest; + +/** + * The response body of toggling the LiDAR units rain and fog mode. + */ +typedef struct { + uint8_t ret_code; /**< Return code. */ + uint8_t count; /**< Count of ret_state_list. */ + ReturnCode ret_state_list[1]; /**< Return code */ +} HubRainFogSuppressResponse; + +#pragma pack() + +#endif // LIVOX_DEF_H_ diff --git a/sdk_core/include/livox_sdk.h b/sdk_core/include/livox_sdk.h new file mode 100644 index 0000000..1d38de4 --- /dev/null +++ b/sdk_core/include/livox_sdk.h @@ -0,0 +1,488 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_SDK_H_ +#define LIVOX_SDK_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "livox_def.h" + +/** + * Initialize the SDK. + * @return true if successfully initialized, otherwise false. + */ +bool Init(); + +/** + * Start the device scanning routine which runs on a separate thread. + * @return true if successfully started, otherwise false. + */ +bool Start(); + +/** + * Uninitialize the SDK. + */ +void Uninit(); + +/** + * @c SetBroadcastCallback response callback function. + * @param info information of the broadcast device, becomes invalid after the function returns. + */ +typedef void (*DeviceBroadcastCallback)(const BroadcastDeviceInfo *info); + +/** + * Set the callback of listening device broadcast message. When broadcast message is received from Livox Hub/LiDAR, cb + * is called. + * @param cb callback for device broadcast. + */ +void SetBroadcastCallback(DeviceBroadcastCallback cb); + +/** + * @c SetDeviceStateUpdateCallback response callback function. + * @param device information of the connected device. + * @param type the update type that indicates connection/disconnection of the device or change of working state. + */ +typedef void (*DeviceStateUpdateCallback)(const DeviceInfo *device, DeviceEvent type); + +/** + * @brief Add a callback for device connection or working state changing event. + * @note Livox SDK supports two hardware connection modes. 1: Directly connecting to the LiDAR device; 2. Connecting to + * the LiDAR device(s) via the Livox Hub. In the first mode, connection/disconnection of every LiDAR unit is reported by + * this callback. In the second mode, only connection/disconnection of the Livox Hub is reported by this callback. If + * you want to get information of the LiDAR unit(s) connected to hub, see \ref HubQueryLidarInformation. + * @note 3 conditions can trigger this callback: + * 1. Connection and disconnection of device. + * 2. A change of device working state. + * 3. An error occurs. + * @param cb callback for device connection/disconnection. + */ +void SetDeviceStateUpdateCallback(DeviceStateUpdateCallback cb); + +/** + * Add a broadcast code to the connecting list and only devices with broadcast code in this list will be connected. The + * broadcast code is unique for every device. + * @param broadcast_code device's broadcast code. + * @param handle device handle. For Livox Hub, the handle is always 31; for LiDAR units connected to the Livox Hub, + * the corresponding handle is (slot-1)*3+id-1. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t AddHubToConnect(const char *broadcast_code, uint8_t *handle); + +/** + * Add a broadcast code to the connecting list and only devices with broadcast code in this list will be connected. The + * broadcast code is unique for every device. + * @param broadcast_code device's broadcast code. + * @param handle device handle. The handle is the same as the order calling AddLidarToConnect starting from 0. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t AddLidarToConnect(const char *broadcast_code, uint8_t *handle); + +/** + * Get all connected devices' information. + * @param devices list of connected devices' information. + * @param size number of devices connected. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t GetConnectedDevices(DeviceInfo *devices, uint8_t *size); + +/** + * Function type of callback that queries device's information. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*DeviceInformationCallback)(uint8_t status, + uint8_t handle, + DeviceInformationResponse *response, + void *client_data); + +/** + * Command to query device's information. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t QueryDeviceInformation(uint8_t handle, DeviceInformationCallback cb, void *client_data); + +/** + * Callback function for receiving point cloud data. + * @param handle device handle. + * @param data device's data. + * @param data_num number of points in data. + */ +typedef void (*DataCallback)(uint8_t handle, LivoxEthPacket *data, uint32_t data_num); + +/** + * Set the callback to receive point cloud data. Only one callback is supported for a specific device. Set the point + * cloud data callback before beginning sampling. + * @param cb callback to receive point cloud data. + */ +void SetDataCallback(uint8_t handle, DataCallback cb); + +/** + * Function type of callback with 1 byte of response. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*CommonCommandCallback)(uint8_t status, uint8_t handle, uint8_t response, void *client_data); + +/** + * Start hub sampling. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t HubStartSampling(CommonCommandCallback cb, void *client_data); + +/** + * Stop the Livox Hub's sampling. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t HubStopSampling(CommonCommandCallback cb, void *client_data); + +/** + * Get the LiDAR unit handle used in the Livox Hub data callback function from slot and id. + * @param slot Livox Hub's slot. + * @param id Livox Hub's id. + * @return LiDAR unit handle. + */ +uint8_t HubGetLidarHandle(uint8_t slot, uint8_t id); + +/** + * Command to change point cloud coordinate system to cartesian coordinate. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t SetCartesianCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data); + +/** + * Change point cloud coordinate system to spherical coordinate. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t SetSphericalCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data); + +/** + * Callback of the error status message. + * @param handle device handle. + * @param response response from the device. + */ +typedef void (*ErrorMessageCallback)(uint8_t handle, ErrorMessage *message); + +/** + * Add error status callback for the device. + * @param handle device handle. + * @param cb callback for the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t SetErrorMessageCallback(uint8_t handle, ErrorMessageCallback cb); + +/** + * Set device's IP mode. + * @param handle device handle. + * @param req request sent to device. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t SetStaticDynamicIP(uint8_t handle, + SetDeviceIPModeRequest *req, + CommonCommandCallback cb, + void *client_data); + +/** + * Callback function that gets device's IP information. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*GetDeviceIPInformationCallback)(uint8_t status, + uint8_t handle, + GetDeviceIPModeResponse *response, + void *client_data); + +/** + * Get device's IP mode. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t GetDeviceIPInformation(uint8_t handle, GetDeviceIPInformationCallback cb, void *client_data); + +/** + * @c HubQueryLidarInformation response callback function. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*HubQueryLidarInformationCallback)(uint8_t status, + uint8_t handle, + HubQueryLidarInformationResponse *response, + void *client_data); + +/** + * Query the information of LiDARs connected to the hub. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t HubQueryLidarInformation(HubQueryLidarInformationCallback cb, void *client_data); + +/** + * @c HubSetMode response callback function. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*HubSetModeCallback)(uint8_t status, uint8_t handle, HubSetModeResponse *response, void *client_data); + +/** + * Set the mode of LiDAR unit connected to the Livox Hub. + * @param req mode configuration of LiDAR units. + * @param length length of req. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t HubSetMode(HubSetModeRequest *req, uint16_t length, HubSetModeCallback cb, void *client_data); + +/** + * @c HubQueryLidarStatus response callback function. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*HubQueryLidarStatusCallback)(uint8_t status, uint8_t handle, HubQueryLidarStatusResponse *response, void *client_data); + +/** + * Get the state of LiDAR units connected to the Livox Hub. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t HubQueryLidarStatus(HubQueryLidarStatusCallback cb, void *client_data); + +/** + * Toggle the power supply of designated slots. + * @param req request whether to enable or disable the power of designated slots. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ + +uint8_t HubControlSlotPower(HubControlSlotPowerRequest *req, CommonCommandCallback cb, void *client_data); + +/** + * @c HubSetExtrinsicParameter response callback function. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*HubSetExtrinsicParameterCallback)(uint8_t status, + uint8_t handle, + HubSetExtrinsicParameterResponse *response, + void *client_data); + +/** + * Set extrinsic parameters of LiDAR units connected to the Livox Hub. + * @param req the parameters to write. + * @param length the request's length. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t HubSetExtrinsicParameter(HubSetExtrinsicParameterRequest *req, + uint16_t length, + HubSetExtrinsicParameterCallback cb, + void *client_data); + +/** + * @c HubGetExtrinsicParameter response callback function. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*HubGetExtrinsicParameterCallback)(uint8_t status, + uint8_t handle, + HubGetExtrinsicParameterResponse *response, + void *client_data); + +/** + * Get extrinsic parameters of LiDAR units connected to the Livox Hub. + * @param req the LiDAR units broadcast code list. + * @param length the request's length. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t HubGetExtrinsicParameter(HubGetExtrinsicParameterRequest *req, + uint16_t length, + HubGetExtrinsicParameterCallback cb, + void *client_data); + +/** + * Turn on or off the calculation of extrinsic parameters. + * @param enable the request whether enable or disable calculating the extrinsic parameters. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ + +uint8_t HubExtrinsicParameterCalculation(bool enable, CommonCommandCallback cb, void *client_data); + +/** + * @c HubRainFogSuppress response callback function. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*HubRainFogSuppressCallback)(uint8_t status, + uint8_t handle, + HubRainFogSuppressResponse *response, + void *client_data); + +/** + * Toggling the rain and fog mode for lidars connected to the hub. + * @param req the request whether open or close the rain and fog mode. + * @param length the request's length. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t HubRainFogSuppress(HubRainFogSuppressRequest *req, + uint16_t length, + HubRainFogSuppressCallback cb, + void *client_data); + +/** + * Start LiDAR sampling. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t LidarStartSampling(uint8_t handle, CommonCommandCallback cb, void *client_data); + +/** + * Stop LiDAR sampling. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t LidarStopSampling(uint8_t handle, CommonCommandCallback cb, void *client_data); + +/** + * Set LiDAR mode. + * @note Successful callback function status only means LiDAR successfully starting the changing process of mode. + * You need to observe the actually change of mode in DeviceStateUpdateCallback function. + * @param handle device handle. + * @param mode the mode to change. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t LidarSetMode(uint8_t handle, LidarMode mode, CommonCommandCallback cb, void *client_data); + +/** + * Set LiDAR extrinsic parameters. + * @param handle device handle. + * @param param the parameters to write. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t LidarSetExtrinsicParameter(uint8_t handle, + LidarSetExtrinsicParameterRequest *req, + CommonCommandCallback cb, + void *client_data); + +/** + * @c LidarGetExtrinsicParameter response callback function. + * @param status kStatusSuccess on successful return, kStatusTimeout on timeout, see \ref ResponseStatus for other + * error code. + * @param handle device handle. + * @param response response from the device. + * @param client_data user data associated with the command. + */ +typedef void (*LidarGetExtrinsicParameterCallback)(uint8_t status, + uint8_t handle, + LidarGetExtrinsicParameterResponse *response, + void *client_data); + +/** + * Get LiDAR extrinsic parameters. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t LidarGetExtrinsicParameter(uint8_t handle, LidarGetExtrinsicParameterCallback cb, void *client_data); + +/** + * Enable and disable the rain/fog suppression. + * @param handle device handle. + * @param cb callback for the command. + * @param client_data user data associated with the command. + * @return kStatusSuccess on successful return, see \ref ResponseStatus for other error code. + */ +uint8_t LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCallback cb, void *client_data); + +#ifdef __cplusplus +} +#endif + +#endif // LIVOX_SDK_H_ diff --git a/sdk_core/include/third_party/FastCRC/FastCRC.h b/sdk_core/include/third_party/FastCRC/FastCRC.h new file mode 100644 index 0000000..d796171 --- /dev/null +++ b/sdk_core/include/third_party/FastCRC/FastCRC.h @@ -0,0 +1,79 @@ +/* FastCRC library code is placed under the MIT license + * Copyright (c) 2014,2015 Frank Bosing + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + + +// Teensy 3.0, Teensy 3.1: +// See K20P64M72SF1RM.pdf (Kinetis), Pages 638 - 641 for documentation of CRC Device +// See KINETIS_4N30D.pdf for Errata (Errata ID 2776) +// +// So, ALL HW-calculations are done as 32 bit. +// +// +// +// Thanks to: +// - Catalogue of parametrised CRC algorithms, CRC RevEng +// http://reveng.sourceforge.net/crc-catalogue/ +// +// - Danjel McGougan (CRC-Table-Generator) +// + +// +// modify from FastCRC library @ 2018/11/20 +// + +#ifndef FASTCRC_FASTCRC_H_ +#define FASTCRC_FASTCRC_H_ + +#include + + +// ================= 16-BIT CRC =================== + +class FastCRC16 { +public: + FastCRC16(uint16_t seed); + + // change function name from mcrf4xx_upd to mcrf4xx + uint16_t mcrf4xx_calc(const uint8_t *data,const uint16_t datalen); // Equivalent to _crc_ccitt_update() in crc16.h from avr_libc + +private: + uint16_t seed_; + +}; + +// ================= 32-BIT CRC =================== + +class FastCRC32 { +public: + FastCRC32(uint32_t seed); + + // change function name from crc32_upd to crc32 + uint32_t crc32_calc(const uint8_t *data, uint16_t len); // Call for subsequent calculations with previous seed + +private: + uint32_t seed_; +}; + +#endif // FASTCRC_FASTCRC_H_ + diff --git a/sdk_core/src/base/command_callback.h b/sdk_core/src/base/command_callback.h new file mode 100644 index 0000000..3573bd5 --- /dev/null +++ b/sdk_core/src/base/command_callback.h @@ -0,0 +1,245 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_COMMAND_CALLBACK_H +#define LIVOX_COMMAND_CALLBACK_H + +#include +#include +#include "livox_sdk.h" + +namespace livox { + +class CommandCallback { + public: + virtual ~CommandCallback() {} + virtual void operator()(uint8_t handle, void *data) = 0; +}; + +template +class MemberFunctionCallback : public CommandCallback { + public: + typedef void (T::*MemFn)(uint8_t status, uint8_t handle, ResponseType *data); + + MemberFunctionCallback(T *cls, MemFn func) : this_(cls), func_(func) {} + void operator()(uint8_t handle, void *data) { + if (this_) { + (this_->*func_)((data == NULL) ? kStatusTimeout : kStatusSuccess, handle, static_cast(data)); + } + } + + private: + T *this_; + MemFn func_; +}; + +template +class MemberFunctionCallback : public CommandCallback { + public: + typedef void (T::*MemFn)(uint8_t status, uint8_t handle, uint8_t response); + + MemberFunctionCallback(T *cls, MemFn func) : this_(cls), func_(func) {} + void operator()(uint8_t handle, void *data) { + if (this_) { + (this_->*func_)((data == NULL) ? kStatusTimeout : kStatusSuccess, + handle, + static_cast(reinterpret_cast(data))); + } + } + + private: + T *this_; + MemFn func_; +}; + +template +boost::shared_ptr MakeCommandCallback(T *cls, + typename MemberFunctionCallback::MemFn func) { + boost::shared_ptr cb(new MemberFunctionCallback(cls, func)); + return cb; +} + +template +class FunctionStatusCallback : public CommandCallback { + public: + typedef void (*Fn)(uint8_t status, uint8_t handle, T *data, void *client_data); + + public: + FunctionStatusCallback(Fn func, void *client_data) : func_(func), client_data_(client_data) {} + void operator()(uint8_t handle, void *data) { + if (func_) { + (*func_)((data == NULL) ? kStatusTimeout : kStatusSuccess, handle, static_cast(data), client_data_); + } + } + + private: + Fn func_; + void *client_data_; +}; + +template <> +class FunctionStatusCallback : public CommandCallback { + public: + typedef void (*Fn)(uint8_t status, uint8_t handle, uint8_t data, void *client_data); + + public: + FunctionStatusCallback(Fn func, void *client_data) : func_(func), client_data_(client_data) {} + void operator()(uint8_t handle, void *data) { + if (func_) { + if (data == NULL) { + (*func_)(kStatusTimeout, handle, 0, client_data_); + } else { + (*func_)(kStatusSuccess, handle, *(uint8_t *)data, client_data_); + } + } + } + + private: + Fn func_; + void *client_data_; +}; + +template +boost::shared_ptr MakeCommandCallback(typename FunctionStatusCallback::Fn func, void *client_data) { + boost::shared_ptr cb(new FunctionStatusCallback(func, client_data)); + return cb; +} + +template +class MessageCallback : public CommandCallback { + public: + typedef void (*Fn)(uint8_t handle, T *data); + + public: + MessageCallback(const Fn &func) : func_(func) {} + void operator()(uint8_t handle, void *data) { + if (func_) { + (*func_)(handle, static_cast(data)); + } + } + + private: + Fn func_; +}; + +template +class BoostFunctionMessageCallback : public CommandCallback { + public: + typedef boost::function Fn; + + public: + BoostFunctionMessageCallback(const Fn &func) : func_(func) {} + void operator()(uint8_t handle, void *data) { + if (func_) { + func_(handle, static_cast(data)); + } + } + + private: + Fn func_; +}; + +template +boost::shared_ptr MakeMessageCallback(typename MessageCallback::Fn func) { + boost::shared_ptr cb(new MessageCallback(func)); + return cb; +} + +template +boost::shared_ptr MakeMessageCallback(typename BoostFunctionMessageCallback::Fn func) { + boost::shared_ptr cb(new BoostFunctionMessageCallback(func)); + return cb; +} + +template +class MemberMessageCallback : public CommandCallback { + public: + typedef void (T::*MemFn)(uint8_t handle, ResponseType *data); + + MemberMessageCallback(T *cls, MemFn func) : this_(cls), func_(func) {} + void operator()(uint8_t handle, void *data) { + if (this_) { + (this_->*func_)(handle, (ResponseType *)data); + } + } + + private: + T *this_; + MemFn func_; +}; + +template +boost::shared_ptr MakeMemberMessageCallback( + T *_this, + typename MemberMessageCallback::MemFn func) { + boost::shared_ptr cb(new MemberMessageCallback(_this, func)); + return cb; +} + +template +class BoostFunctionCallback : public CommandCallback { + public: + typedef boost::function Fn; + BoostFunctionCallback(const Fn &func) : func_(func) {} + void operator()(uint8_t handle, void *data) { + if (func_) { + if (data == NULL) { + func_(kStatusTimeout, handle, NULL); + } else { + func_(kStatusSuccess, handle, (ResponseType *)data); + } + } + } + + private: + Fn func_; +}; + +template <> +class BoostFunctionCallback : public CommandCallback { + public: + typedef boost::function Fn; + BoostFunctionCallback(const Fn &func) : func_(func) {} + void operator()(uint8_t handle, void *data) { + if (func_) { + if (data == NULL) { + func_(kStatusTimeout, handle, 0); + } else { + func_(kStatusSuccess, handle, static_cast(reinterpret_cast(data))); + } + } + } + + private: + Fn func_; +}; + +template +boost::shared_ptr MakeCommandCallback(const typename BoostFunctionCallback::Fn &func) { + boost::shared_ptr cb(new BoostFunctionCallback(func)); + return cb; +} + +} // namespace livox +#endif // LIVOX_COMMAND_CALLBACK_H diff --git a/sdk_core/src/base/io_loop.cpp b/sdk_core/src/base/io_loop.cpp new file mode 100644 index 0000000..1dc1129 --- /dev/null +++ b/sdk_core/src/base/io_loop.cpp @@ -0,0 +1,182 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "io_loop.h" +#include +#include +#include +#include +#include "logging.h" + +using boost::lock_guard; +using boost::mutex; +using std::vector; + +namespace livox { + +bool IOLoop::Init() { + if (mem_pool_ == NULL) { + return false; + } + apr_status_t rv = + apr_pollset_create(&pollset_, kMaxPollCount, mem_pool_, APR_POLLSET_THREADSAFE | APR_POLLSET_WAKEABLE); + if (rv != APR_SUCCESS) { + LOG_ERROR << PrintAPRStatus(rv) << std::endl; + return false; + } + + return true; +} + +void IOLoop::Uninit() { + if (pollset_) { + apr_pollset_destroy(pollset_); + pollset_ = NULL; + } + + for (DelegatesType::const_iterator ite = delegates_.begin(); ite != delegates_.end(); ++ite) { + ClientData *data = ite->second; + if (data) { + delete data; + } + } + + delegates_.clear(); + mem_pool_ = NULL; +} + +void IOLoop::AddDelegate(apr_socket_t *sock, IOLoop::IOLoopDelegate *delegate, void *data) { + PostTask(boost::bind(&IOLoop::AddDelegateAsync, this, sock, delegate, data)); +} + +void IOLoop::RemoveDelegate(apr_socket_t *sock, IOLoopDelegate *) { + PostTask(boost::bind(&IOLoop::RemoveDelegateAsync, this, sock)); +} + +void IOLoop::Loop() { + apr_int32_t num = 0; + const apr_pollfd_t *ret_pfd = NULL; + apr_status_t rv = apr_pollset_poll(pollset_, apr_time_from_msec(50), &num, &ret_pfd); + + if (rv == APR_SUCCESS) { + for (int i = 0; i < num; i++) { + ClientData *data = static_cast(ret_pfd[i].client_data); + ; + if (data) { + IOLoopDelegate *delegate = data->first; + if (delegate) { + delegate->OnData(ret_pfd->desc.s, data->second); + } + } + } + } + + if (enable_wake_ && APR_STATUS_IS_EINTR(rv)) { + DelegatesType delegates = delegates_; + for (DelegatesType::const_iterator ite = delegates.begin(); ite != delegates.end(); ++ite) { + ClientData *data = ite->second; + if (data) { + IOLoopDelegate *delegate = data->first; + if (delegate) { + delegate->OnWake(); + } + } + } + } + + if (enable_timer_) { + apr_time_t t = apr_time_now(); + if (last_timeout_ == 0 || t - last_timeout_ > apr_time_from_msec(50)) { + last_timeout_ = t; + // copy delegates_ in case delegate is removed in callback. + DelegatesType delegates = delegates_; + for (DelegatesType::const_iterator ite = delegates.begin(); ite != delegates.end(); ++ite) { + ClientData *data = ite->second; + if (data) { + IOLoopDelegate *delegate = data->first; + if (delegate) { + delegate->OnTimer(t); + } + } + } + } + } + + vector tasks; + { + lock_guard lock(mutex_); + tasks.swap(pending_tasks_); + } + + for (vector::iterator ite = tasks.begin(); ite != tasks.end(); ++ite) { + IOLoopTask &task = *ite; + task(); + } + + if (rv != APR_SUCCESS && !APR_STATUS_IS_TIMEUP(rv) && !APR_STATUS_IS_EINTR(rv)) { + LOG_ERROR << PrintAPRStatus(rv) << std::endl; + } +} + +bool IOLoop::Wakeup() { + apr_status_t rv = apr_pollset_wakeup(pollset_); + if (rv != APR_SUCCESS) { + LOG_ERROR << PrintAPRStatus(rv) << std::endl; + return false; + } + return true; +} + +void IOLoop::PostTask(const IOLoopTask &task) { + { + lock_guard lock(mutex_); + pending_tasks_.push_back(task); + } + Wakeup(); +} + +void IOLoop::AddDelegateAsync(apr_socket_t *sock, IOLoop::IOLoopDelegate *delegate, void *data) { + ClientData *p = new ClientData(delegate, data); + apr_pollfd_t pfd = {mem_pool_, APR_POLL_SOCKET, APR_POLLIN, 0, {NULL}, p}; + pfd.desc.s = sock; + apr_pollset_add(pollset_, &pfd); + delegates_[sock] = p; +} + +void IOLoop::RemoveDelegateAsync(apr_socket_t *sock) { + apr_pollfd_t pfd = {mem_pool_, APR_POLL_SOCKET, 0, 0, {NULL}, NULL}; + pfd.desc.s = sock; + apr_pollset_remove(pollset_, &pfd); + + DelegatesType::iterator ite = delegates_.find(sock); + if (ite != delegates_.end()) { + ClientData *p = ite->second; + if (p) { + delete p; + } + delegates_.erase(ite); + } +} + +} // namespace livox diff --git a/sdk_core/src/base/io_loop.h b/sdk_core/src/base/io_loop.h new file mode 100644 index 0000000..504a73d --- /dev/null +++ b/sdk_core/src/base/io_loop.h @@ -0,0 +1,87 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_IO_LOOP_H_ +#define LIVOX_IO_LOOP_H_ + +#include +#include +#include +#include +#include +#include "apr_general.h" +#include "apr_network_io.h" +#include "apr_poll.h" +#include "apr_thread_proc.h" +#include "command_callback.h" +#include "noncopyable.h" +#include "thread_base.h" +#include "util.h" + +namespace livox { + +class IOLoop : public noncopyable { + public: + class IOLoopDelegate { + public: + virtual void OnData(apr_socket_t *, void *) {} + virtual void OnTimer(apr_time_t) {} + virtual void OnWake() {} + }; + + typedef boost::function IOLoopTask; + + public: + explicit IOLoop(apr_pool_t *mem_pool, bool enable_timer = true, bool enable_wake = true) + : pollset_(NULL), mem_pool_(mem_pool), last_timeout_(0), enable_timer_(enable_timer), enable_wake_(enable_wake){}; + + bool Init(); + void Uninit(); + void AddDelegate(apr_socket_t *sock, IOLoopDelegate *delegate, void *data = NULL); + void RemoveDelegate(apr_socket_t *sock, IOLoopDelegate *delegate); + void Loop(); + bool Wakeup(); + void PostTask(const IOLoopTask &task); + + private: + void AddDelegateAsync(apr_socket_t *sock, IOLoopDelegate *delegate, void *data); + void RemoveDelegateAsync(apr_socket_t *sock); + + private: + static const apr_uint32_t kMaxPollCount = 48; + typedef std::pair ClientData; + typedef boost::unordered_map DelegatesType; + DelegatesType delegates_; + apr_pollset_t *pollset_; + apr_pool_t *mem_pool_; + apr_time_t last_timeout_; + boost::mutex mutex_; + bool enable_timer_; + bool enable_wake_; + std::vector pending_tasks_; +}; + +} // namespace livox + +#endif // LIVOX_IO_LOOP_H_ diff --git a/sdk_core/src/base/io_thread.cpp b/sdk_core/src/base/io_thread.cpp new file mode 100644 index 0000000..6db00b3 --- /dev/null +++ b/sdk_core/src/base/io_thread.cpp @@ -0,0 +1,60 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "io_thread.h" +#include "apr_pools.h" + +namespace livox { + +void IOThread::Join() { + ThreadBase::Join(); +} + +void IOThread::ThreadFunc() { + if (loop_ == NULL) { + return; + } + while (!IsQuit()) { + loop_->Loop(); + } +} + +bool IOThread::Init(bool enable_timer, bool enable_wake) { + if (!ThreadBase::Init()) { + return false; + } + + loop_.reset(new IOLoop(pool_, enable_timer, enable_wake)); + return loop_->Init(); +} + +void IOThread::Uninit() { + if (loop_) { + loop_->Uninit(); + loop_.reset(NULL); + } + ThreadBase::Uninit(); +} + +} // namespace livox diff --git a/sdk_core/src/base/io_thread.h b/sdk_core/src/base/io_thread.h new file mode 100644 index 0000000..ae9dd0e --- /dev/null +++ b/sdk_core/src/base/io_thread.h @@ -0,0 +1,47 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_IO_THREAD_H_ +#define LIVOX_IO_THREAD_H_ +#include +#include "io_loop.h" +#include "thread_base.h" + +namespace livox { + +class IOThread : public ThreadBase { + public: + IOThread() : loop_(NULL) {} + bool Init(bool enable_timer = true, bool enable_wake = true); + void Join(); + void Uninit(); + + IOLoop *loop() { return loop_.get(); } + void ThreadFunc(); + + private: + boost::scoped_ptr loop_; +}; +} // namespace livox +#endif // LIVOX_IO_THREAD_H_ diff --git a/sdk_core/src/base/logging.h b/sdk_core/src/base/logging.h new file mode 100644 index 0000000..fe9d65a --- /dev/null +++ b/sdk_core/src/base/logging.h @@ -0,0 +1,39 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_LOGGING_H_ +#define LIVOX_LOGGING_H_ +#include +#include + +#define __FILENAME__ (strrchr(__FILE__, '/') ? (strrchr(__FILE__, '/') + 1) : __FILE__) + +#define LOG_TRACE std::cout << "TRACE " << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << " " +#define LOG_DEBUG std::cout << "DEBUG " << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << " " +#define LOG_INFO std::cout << "INFO " << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << " " +#define LOG_WARN std::cout << "WARN " << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << " " +#define LOG_ERROR std::cout << "ERROR " << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << " " +#define LOG_FATAL std::cout << "FATAL " << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << " " + +#endif // LIVOX_LOGGING_H_ diff --git a/sdk_core/src/base/network_util.cpp b/sdk_core/src/base/network_util.cpp new file mode 100644 index 0000000..75f9932 --- /dev/null +++ b/sdk_core/src/base/network_util.cpp @@ -0,0 +1,90 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "network_util.h" +#include +namespace livox { +namespace util { +apr_socket_t *CreateBindSocket(uint16_t port, apr_pool_t *mem_pool, bool nonblock) { + apr_sockaddr_t *sa = NULL; + apr_socket_t *s = NULL; + apr_status_t rv = APR_SUCCESS; + + rv = apr_sockaddr_info_get(&sa, NULL, APR_INET, port, 0, mem_pool); + if (rv != APR_SUCCESS) { + return NULL; + } + + rv = apr_socket_create(&s, sa->family, SOCK_DGRAM, APR_PROTO_UDP, mem_pool); + if (rv != APR_SUCCESS) { + return NULL; + } + + rv = apr_socket_bind(s, sa); + if (rv != APR_SUCCESS) { + apr_socket_close(s); + s = NULL; + return NULL; + } + + if (nonblock) { + apr_socket_opt_set(s, APR_SO_NONBLOCK, 1); + apr_socket_timeout_set(s, 0); + } + return s; +} + +bool FindLocalIP(const struct sockaddr_in &client_addr, uint32_t &local_ip) { + struct ifaddrs *if_addrs = NULL, *addrs = NULL; + if (getifaddrs(&if_addrs) == -1) { + return false; + } + addrs = if_addrs; + bool found = false; + while (if_addrs != NULL) { + if ((if_addrs->ifa_addr != NULL) && (if_addrs->ifa_addr->sa_family == AF_INET)) // check it is IP4 + { + // is a connected IP4 Address + struct sockaddr_in *ifu_localaddr = (struct sockaddr_in *)if_addrs->ifa_addr; + struct sockaddr_in *ifu_netmask = (struct sockaddr_in *)if_addrs->ifa_netmask; + if (ifu_localaddr->sin_addr.s_addr != htonl(INADDR_ANY)) { + if ((ifu_localaddr->sin_addr.s_addr & ifu_netmask->sin_addr.s_addr) == + (client_addr.sin_addr.s_addr & ifu_netmask->sin_addr.s_addr)) { + local_ip = ifu_localaddr->sin_addr.s_addr; + found = true; + break; + } + } + } + if_addrs = if_addrs->ifa_next; + } + + if (addrs) { + freeifaddrs(addrs); + } + return found; +} + +} // namespace util +} // namespace livox diff --git a/sdk_core/src/base/network_util.h b/sdk_core/src/base/network_util.h new file mode 100644 index 0000000..54ee1ed --- /dev/null +++ b/sdk_core/src/base/network_util.h @@ -0,0 +1,38 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_NETWORK_UTIL_H_ +#define LIVOX_NETWORK_UTIL_H_ +#include +#include + +namespace livox { +namespace util { + +apr_socket_t *CreateBindSocket(uint16_t port, apr_pool_t *mem_pool, bool nonblock = true); +bool FindLocalIP(const struct sockaddr_in &client_addr, uint32_t &local_ip); + +} // namespace util +} // namespace livox +#endif // LIVOX_NETWORK_UTIL_H_ diff --git a/sdk_core/src/base/noncopyable.h b/sdk_core/src/base/noncopyable.h new file mode 100644 index 0000000..db4728c --- /dev/null +++ b/sdk_core/src/base/noncopyable.h @@ -0,0 +1,40 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_NONCOPYABLE_H_ +#define LIVOX_NONCOPYABLE_H_ + +namespace livox { +class noncopyable { + protected: + noncopyable() {} + ~noncopyable() {} + + private: + noncopyable(const noncopyable &); + noncopyable &operator=(const noncopyable &); +}; + +} // namespace livox +#endif // LIVOX_NONCOPYABLE_H_ diff --git a/sdk_core/src/base/thread_base.cpp b/sdk_core/src/base/thread_base.cpp new file mode 100644 index 0000000..e185a26 --- /dev/null +++ b/sdk_core/src/base/thread_base.cpp @@ -0,0 +1,71 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "thread_base.h" + +namespace livox { +static void *APR_THREAD_FUNC ClassThreadHelperFunc(apr_thread_t *thd, void *data) { + ThreadBase *caller = static_cast(data); + if (caller == NULL) { + return NULL; + } + caller->ThreadFunc(); + apr_thread_exit(thd, APR_SUCCESS); + return NULL; +} + +ThreadBase::ThreadBase() : thread_(NULL), quit_(false), pool_(NULL) {} + +bool ThreadBase::Start() { + quit_ = false; + apr_threadattr_t *thread_attr = NULL; + apr_status_t rv = APR_SUCCESS; + rv = apr_threadattr_create(&thread_attr, pool_); + if (rv != APR_SUCCESS) { + return false; + } + rv = apr_thread_create(&thread_, thread_attr, ClassThreadHelperFunc, this, pool_); + return rv == APR_SUCCESS; +} + +void ThreadBase::Join() { + apr_status_t rv = APR_SUCCESS; + if (thread_) { + apr_thread_join(&rv, thread_); + thread_ = NULL; + } +} + +bool ThreadBase::Init() { + return apr_pool_create(&pool_, NULL) == APR_SUCCESS; +} + +void ThreadBase::Uninit() { + if (pool_) { + apr_pool_destroy(pool_); + pool_ = NULL; + } +} + +} // namespace livox diff --git a/sdk_core/src/base/thread_base.h b/sdk_core/src/base/thread_base.h new file mode 100644 index 0000000..b871096 --- /dev/null +++ b/sdk_core/src/base/thread_base.h @@ -0,0 +1,54 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_THREAD_BASE_H_ +#define LIVOX_THREAD_BASE_H_ +#include +#include +#include +#include "noncopyable.h" + +namespace livox { + +class ThreadBase : public noncopyable { + public: + ThreadBase(); + virtual ~ThreadBase() {} + virtual void ThreadFunc() = 0; + bool Init(); + virtual void Uninit(); + virtual bool Start(); + virtual void Join(); + void Quit() { quit_ = true; } + bool IsQuit() { return quit_; } + + protected: + apr_thread_t *thread_; + boost::atomic_bool quit_; + apr_pool_t *pool_; +}; + +} // namespace livox + +#endif // LIVOX_THREAD_BASE_H_ diff --git a/sdk_core/src/base/util.cpp b/sdk_core/src/base/util.cpp new file mode 100644 index 0000000..4315f31 --- /dev/null +++ b/sdk_core/src/base/util.cpp @@ -0,0 +1,52 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "util.h" +#include +#include +using std::string; +namespace livox { + +string PrintAPRStatus(apr_status_t s) { + char buf[128]; + apr_strerror(s, buf, sizeof(buf)); + return buf; +} +string PrintAPRTime(apr_time_t t) { + char cbuf[45 + 1]; + apr_size_t sz = 45; + apr_time_exp_t xt; + apr_time_exp_gmt(&xt, t); + int milli = t % 1000; + apr_strftime(cbuf, &sz, 45, " %T.", &xt); + string s(cbuf); + + char buffer[33]; + sprintf(buffer, "%d", milli); + + s += buffer; + return s; +} + +} // namespace livox diff --git a/sdk_core/src/base/util.h b/sdk_core/src/base/util.h new file mode 100644 index 0000000..7cde2d5 --- /dev/null +++ b/sdk_core/src/base/util.h @@ -0,0 +1,37 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_UTIL_H_ +#define LIVOX_UTIL_H_ +#include +#include "apr_general.h" +#include "apr_time.h" + +namespace livox { + +std::string PrintAPRStatus(apr_status_t s); +std::string PrintAPRTime(apr_time_t t); + +} // namespace livox +#endif // LIVOX_UTIL_H_ diff --git a/sdk_core/src/comm/comm_port.cpp b/sdk_core/src/comm/comm_port.cpp new file mode 100644 index 0000000..7192ddd --- /dev/null +++ b/sdk_core/src/comm/comm_port.cpp @@ -0,0 +1,164 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "comm/comm_port.h" +#include +#include +#include + +#include "sdk_protocol.h" + +namespace livox { +const uint32_t kSearchPacketPreamble = 0; +const uint32_t kGetPacketData = 1; + +CommPort::CommPort() { + protocol_ = new SdkProtocol(0x4c49, 0x564f580a); + cache_.wr_idx = 0; + cache_.rd_idx = 0; + cache_.size = kCacheSize; + parse_step_ = kSearchPacketPreamble; + seq_num_ = 0; +} + +CommPort::~CommPort() { + if (protocol_) { + delete protocol_; + } +} + +uint8_t *CommPort::FetchCacheFreeSpace(uint32_t *o_len) { + UpdateCache(); + + if (cache_.wr_idx < cache_.size) { + *o_len = cache_.size - cache_.wr_idx; + return &cache_.buf[cache_.wr_idx]; + } else { + *o_len = 0; + return NULL; + } +} + +int32_t CommPort::UpdateCacheWrIdx(uint32_t used_size) { + if ((cache_.wr_idx + used_size) < cache_.size) { + cache_.wr_idx += used_size; + return 0; + } else { + return -1; + } +} + +uint32_t CommPort::GetCacheTailSize() { + if (cache_.wr_idx < cache_.size) { + return cache_.size - cache_.wr_idx; + } else { + return 0; + } +} + +uint32_t CommPort::GetValidDataSize() { + if (cache_.wr_idx > cache_.rd_idx) { + return cache_.wr_idx - cache_.rd_idx; + } else { + return 0; + } +} + +void CommPort::UpdateCache(void) { + if (GetCacheTailSize() < kMoveCacheLimit) { // move unused data to cache head + uint32_t valid_data_size = GetValidDataSize(); + + if (valid_data_size) { + memmove(cache_.buf, &cache_.buf[cache_.rd_idx], valid_data_size); + cache_.rd_idx = 0; + cache_.wr_idx = valid_data_size; + } else if (cache_.rd_idx) { + cache_.rd_idx = 0; + cache_.wr_idx = 0; + } + } +} + +int32_t CommPort::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet) { + return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet); +} + +int32_t CommPort::ParseCommStream(CommPacket *o_pack) { +#define CUR_PACK_LEN (protocol_->GetPacketLen(&cache_.buf[cache_.rd_idx])) +#define CUR_CACHE_POS (&cache_.buf[cache_.rd_idx]) + + while (GetValidDataSize() > protocol_->GetPreambleLen()) { + // printf("unpack step : %d %d %d\r\n", parse_step_, cache_.wr_idx, cache_.rd_idx); + if (kSearchPacketPreamble == parse_step_) { + if (!protocol_->CheckPreamble(CUR_CACHE_POS)) { + parse_step_ = kGetPacketData; + } else { + ++cache_.rd_idx; + } + } else if (kGetPacketData == parse_step_) { + if (GetValidDataSize() >= CUR_PACK_LEN && CUR_PACK_LEN) { + parse_step_ = kSearchPacketPreamble; + if (!protocol_->CheckPacket(CUR_CACHE_POS)) { + int32_t ret = protocol_->ParsePacket(CUR_CACHE_POS, GetValidDataSize(), o_pack); + cache_.rd_idx += CUR_PACK_LEN; + if (kParseSuccess == ret) { + return ret; + } + } else { + cache_.rd_idx += protocol_->GetPreambleLen(); + } + } else { + if (CUR_PACK_LEN > cache_.size) { // always > cache_.size, + cache_.rd_idx = 0; + cache_.wr_idx = 0; + parse_step_ = kSearchPacketPreamble; + } + + break; + } + } else { + parse_step_ = kSearchPacketPreamble; + } + } + + return kParseFail; +} + +uint16_t CommPort::GetAndUpdateSeqNum() { + // add lock here for thread safe + uint16_t seq = seq_num_; + seq_num_++; + + return seq; +} + +// only for pack function test use +// uint8_t test_buf[1024]; +// uint32_t o_len = 0; +// comm_port_->Pack(test_buf, sizeof(test_buf), &o_len, packet); +// for (uint32_t print_i = 0; print_i < o_len; print_i++) { +// printf("%.2x_", test_buf[print_i]); +//} +// printf("\r\n"); +} // namespace livox diff --git a/sdk_core/src/comm/sdk_protocol.cpp b/sdk_core/src/comm/sdk_protocol.cpp new file mode 100644 index 0000000..e3b1fff --- /dev/null +++ b/sdk_core/src/comm/sdk_protocol.cpp @@ -0,0 +1,128 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "sdk_protocol.h" + +#include +#include +namespace livox { +const uint8_t kSdkProtocolSof = 0xAA; +const uint32_t kSdkPacketCrcSize = 4; // crc32 +const uint32_t kSdkPacketPreambleCrcSize = 2; // crc16 + +SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32) : crc16_(seed16), crc32_(seed32) {} + +SdkProtocol::~SdkProtocol() {} + +int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet) { + SdkPacket *sdk_packet = (SdkPacket *)o_buf; + + if (kLidarSdk != i_packet.protocol) { + return -1; + } + + sdk_packet->sof = kSdkProtocolSof; + sdk_packet->length = i_packet.data_len + GetPacketWrapperLen(); + + if (sdk_packet->length > o_buf_size) { + return -1; + } + + sdk_packet->version = kSdkVer0; + sdk_packet->packet_type = i_packet.packet_type; + sdk_packet->seq_num = i_packet.seq_num & 0xFFFF; + sdk_packet->preamble_crc = crc16_.mcrf4xx_calc(o_buf, GetPreambleLen() - kSdkPacketPreambleCrcSize); + sdk_packet->cmd_set = i_packet.cmd_set; + sdk_packet->cmd_id = i_packet.cmd_code; + + memcpy(sdk_packet->data, i_packet.data, i_packet.data_len); + + uint32_t crc = crc32_.crc32_calc(o_buf, sdk_packet->length - kSdkPacketCrcSize); + o_buf[sdk_packet->length - 4] = crc & 0xFF; + o_buf[sdk_packet->length - 3] = (crc >> 8) & 0xFF; + o_buf[sdk_packet->length - 2] = (crc >> 16) & 0xFF; + o_buf[sdk_packet->length - 1] = (crc >> 24) & 0xFF; + + *o_len = sdk_packet->length; + + return 0; +} + +int32_t SdkProtocol::ParsePacket(uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet) { + SdkPacket *sdk_packet = (SdkPacket *)i_buf; + + if (i_len < GetPacketWrapperLen()) { + return -1; // packet lenth error + } + + memset((void *)o_packet, 0, sizeof(CommPacket)); + + o_packet->packet_type = sdk_packet->packet_type; + o_packet->protocol = kLidarSdk; + o_packet->protocol_version = sdk_packet->version; + + o_packet->seq_num = sdk_packet->seq_num; + o_packet->cmd_set = sdk_packet->cmd_set; + o_packet->cmd_code = sdk_packet->cmd_id; + o_packet->data_len = sdk_packet->length - GetPacketWrapperLen(); + o_packet->data = sdk_packet->data; + + return 0; +} + +uint32_t SdkProtocol::GetPreambleLen() { + return sizeof(SdkPreamble); +} + +uint32_t SdkProtocol::GetPacketWrapperLen() { + return sizeof(SdkPacket) - 1 + kSdkPacketCrcSize; +} + +uint32_t SdkProtocol::GetPacketLen(uint8_t *buf) { + SdkPreamble *preamble = (SdkPreamble *)buf; + return preamble->length; +} + +int32_t SdkProtocol::CheckPreamble(uint8_t *buf) { + SdkPreamble *preamble = (SdkPreamble *)buf; + + if ((preamble->sof == kSdkProtocolSof) && (crc16_.mcrf4xx_calc(buf, GetPreambleLen()) == 0)) { + return 0; + } else { + return -1; + } +} + +int32_t SdkProtocol::CheckPacket(uint8_t *buf) { + SdkPacket *sdk_packet = (SdkPacket *)buf; + uint32_t crc = ((uint32_t)(buf[sdk_packet->length - 4])) | (((uint32_t)(buf[sdk_packet->length - 3])) << 8) | + (((uint32_t)(buf[sdk_packet->length - 2])) << 16) | (((uint32_t)(buf[sdk_packet->length - 1])) << 24); + + if (crc32_.crc32_calc(buf, sdk_packet->length - kSdkPacketCrcSize) == crc) { + return 0; + } else { + return -1; + } +} +} // namespace livox diff --git a/sdk_core/src/comm/sdk_protocol.h b/sdk_core/src/comm/sdk_protocol.h new file mode 100644 index 0000000..10e2a36 --- /dev/null +++ b/sdk_core/src/comm/sdk_protocol.h @@ -0,0 +1,84 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_SDK_PROTOCOL_H_ +#define LIVOX_SDK_PROTOCOL_H_ + +#include +#include "../include/comm/protocol.h" +#include "../include/third_party/FastCRC/FastCRC.h" +namespace livox { +typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion; + +#pragma pack(1) + +typedef struct { + uint8_t sof; + uint8_t version; + uint16_t length; + uint8_t packet_type; + uint16_t seq_num; + uint16_t preamble_crc; +} SdkPreamble; + +typedef struct { + uint8_t sof; + uint8_t version; + uint16_t length; + uint8_t packet_type; + uint16_t seq_num; + uint16_t preamble_crc; + uint8_t cmd_set; + uint8_t cmd_id; + uint8_t data[1]; +} SdkPacket; + +#pragma pack() + +class SdkProtocol : public Protocol { + public: + SdkProtocol(uint16_t seed16, uint32_t seed32); + + ~SdkProtocol(); + + int32_t ParsePacket(uint8_t *i_buf, uint32_t i_len, CommPacket *o_packet); + + int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len, const CommPacket &i_packet); + + uint32_t GetPreambleLen(); + + uint32_t GetPacketWrapperLen(); + + uint32_t GetPacketLen(uint8_t *buf); + + int32_t CheckPreamble(uint8_t *buf); + + int32_t CheckPacket(uint8_t *buf); + + private: + FastCRC16 crc16_; + FastCRC32 crc32_; +}; +} // namespace livox +#endif // LIVOX_SDK_PROTOCOL_H_ diff --git a/sdk_core/src/command_handler/command_channel.cpp b/sdk_core/src/command_handler/command_channel.cpp new file mode 100644 index 0000000..55057f0 --- /dev/null +++ b/sdk_core/src/command_handler/command_channel.cpp @@ -0,0 +1,242 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "command_channel.h" +#include +#include "base/logging.h" +#include "base/network_util.h" +#include "command_impl.h" +#include "device_manager.h" +#include "livox_def.h" + +using boost::atomic_uint16_t; +using boost::bind; +using std::list; +using std::make_pair; +using std::map; +using std::pair; +using std::string; + +namespace livox { + +CommandChannel::CommandChannel(apr_port_t port, + uint8_t handle, + const string &remote_ip, + CommandChannelDelegate *cb, + apr_pool_t *pool) + : handle_(handle), + port_(port), + sock_(NULL), + mem_pool_(pool), + loop_(NULL), + callback_(cb), + comm_port_(new CommPort), + heartbeat_time_(0), + remote_ip_(remote_ip), + last_heartbeat_(0) {} + +bool CommandChannel::Bind(IOLoop *loop) { + if (loop == NULL) { + return false; + } + loop_ = loop; + sock_ = util::CreateBindSocket(port_, mem_pool_); + if (sock_ == NULL) { + return false; + } + + loop_->AddDelegate(sock_, this); + return true; +} + +void CommandChannel::OnData(apr_socket_t *, void *) { + apr_sockaddr_t addr; + uint32_t buf_size = 0; + uint8_t *cache_buf = comm_port_->FetchCacheFreeSpace(&buf_size); + apr_size_t size = buf_size; + apr_status_t rv = apr_socket_recvfrom(&addr, sock_, 0, reinterpret_cast(cache_buf), &size); + comm_port_->UpdateCacheWrIdx(size); + if (rv != APR_SUCCESS) { + LOG_ERROR << PrintAPRStatus(rv) << std::endl; + return; + } + + CommPacket packet; + memset(&packet, 0, sizeof(packet)); + + while ((kParseSuccess == comm_port_->ParseCommStream(&packet))) { + if (packet.packet_type == kCommandTypeAck) { + uint16_t seq = packet.seq_num; + if (commands_.find(seq) != commands_.end()) { + Command command = commands_[seq].first; + command.packet = packet; + if (callback_) { + callback_->OnCommand(handle_, command); + } + commands_.erase(seq); + } else if (packet.cmd_set == kCommandSetGeneral && packet.cmd_code == kCommandIDGeneralHeartbeat) { + OnHeartbeatAck(packet); + if (callback_) { + callback_->OnHeartbeatStateUpdate(handle_, *(reinterpret_cast(packet.data))); + } + } + } else if (packet.packet_type == kCommandTypeMsg) { + if (callback_) { + Command command; + command.packet = packet; + callback_->OnCommand(handle_, command); + } + } + } +} + +void CommandChannel::SendAsync(const Command &command) { + Command cmd = DeepCopy(command); + if (loop_) { + loop_->PostTask(bind(&CommandChannel::Send, this, cmd)); + } +} + +void CommandChannel::OnTimer(apr_time_t now) { + list timeout_commands; + map >::iterator ite = commands_.begin(); + while (ite != commands_.end()) { + pair &command_pair = ite->second; + if (now > command_pair.second) { + timeout_commands.push_back(command_pair.first); + commands_.erase(ite++); + } else { + ++ite; + } + } + + for (list::iterator ite = timeout_commands.begin(); ite != timeout_commands.end(); ++ite) { + LOG_WARN << PrintAPRTime(apr_time_now()) << " Command Timeout: Set " << (uint16_t)ite->packet.cmd_set << " Id " + << ite->packet.cmd_code << " Seq " << ite->packet.seq_num << std::endl; + if (callback_) { + ite->packet.packet_type = kCommandTypeAck; + callback_->OnCommand(handle_, *ite); + } + } + + if ((last_heartbeat_ != 0) && (now - last_heartbeat_ > apr_time_from_sec(3))) { + DeviceDisconnect(handle_); + } else { + HeartBeat(now); + } +} + +void CommandChannel::Uninit() { + if (sock_) { + loop_->RemoveDelegate(sock_, this); + loop_ = NULL; + apr_socket_close(sock_); + sock_ = NULL; + } + callback_ = NULL; + if (comm_port_) { + comm_port_.reset(NULL); + } + + commands_.clear(); + last_heartbeat_ = 0; + heartbeat_time_ = 0; + remote_ip_ = ""; +} + +void CommandChannel::HeartBeat(apr_time_t t) { + if (heartbeat_time_ == 0 || (t - heartbeat_time_) > apr_time_from_msec(kHeartbeatTimer)) { + heartbeat_time_ = t; + + Command command(handle_, + kCommandTypeCmd, + kCommandSetGeneral, + kCommandIDGeneralHeartbeat, + GenerateSeq(), + NULL, + 0, + 0, + boost::shared_ptr()); + SendInternal(command); + } +} + +void CommandChannel::SendInternal(const Command &command) { + apr_pool_t *subpool = NULL; + apr_pool_create(&subpool, mem_pool_); + uint8_t *buf = (uint8_t *)apr_palloc(subpool, kMaxCommandBufferSize); + uint32_t size = 0; + comm_port_->Pack(buf, kMaxCommandBufferSize, &size, command.packet); + apr_size_t apr_size = size; + apr_status_t rv = APR_SUCCESS; + apr_sockaddr_t *sa = NULL; + rv = apr_sockaddr_info_get(&sa, remote_ip_.c_str(), APR_INET, 65000, 0, subpool); + if (rv == APR_SUCCESS) { + rv = apr_socket_sendto(sock_, sa, 0, (const char *)buf, &apr_size); + } + apr_pool_destroy(subpool); +} + +uint16_t CommandChannel::GenerateSeq() { + static atomic_uint16_t seq(1); + uint16_t value = seq.load(); + uint16_t desired = 0; + do { + if (value == UINT16_MAX) { + desired = 1; + } else { + desired = value + 1; + } + } while (!seq.compare_exchange_weak(value, desired)); + return desired; +} + +Command CommandChannel::DeepCopy(const Command &cmd) { + Command result_cmd(cmd); + if (result_cmd.packet.data != NULL) { + result_cmd.packet.data = new uint8_t[result_cmd.packet.data_len]; + memcpy(result_cmd.packet.data, cmd.packet.data, result_cmd.packet.data_len); + } + return result_cmd; +} + +void CommandChannel::OnHeartbeatAck(const CommPacket &) { + last_heartbeat_ = apr_time_now(); +} + +void CommandChannel::DeviceDisconnect(uint8_t handle) { + DeviceRemove(handle); +} + +void CommandChannel::Send(const Command &command) { + SendInternal(command); + commands_[command.packet.seq_num] = make_pair(command, apr_time_now() + apr_time_from_msec(command.time_out)); + Command &cmd = commands_[command.packet.seq_num].first; + if (cmd.packet.data != NULL) { + delete cmd.packet.data; + cmd.packet.data = NULL; + cmd.packet.data_len = 0; + } +} +} // namespace livox diff --git a/sdk_core/src/command_handler/command_channel.h b/sdk_core/src/command_handler/command_channel.h new file mode 100644 index 0000000..b7cfd4b --- /dev/null +++ b/sdk_core/src/command_handler/command_channel.h @@ -0,0 +1,126 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_COMMAND_CHANNEL_H_ +#define LIVOX_COMMAND_CHANNEL_H_ +#include +#include +#include +#include "apr_network_io.h" +#include "base/io_loop.h" +#include "comm/comm_port.h" + +namespace livox { + +typedef struct TagCommand { + uint8_t handle; + CommPacket packet; + boost::shared_ptr cb; + uint32_t time_out; + TagCommand() : packet(), time_out(0) {} + TagCommand(uint8_t _handle, + uint8_t _cmd_type, + uint8_t _cmd_set, + uint8_t _cmd_code, + uint16_t _seq_num, + uint8_t *data, + uint16_t length, + uint32_t _time_out, + const boost::shared_ptr &_cb) + : handle(_handle), packet(), cb(_cb) { + packet.packet_type = _cmd_type; + packet.cmd_set = _cmd_set; + packet.cmd_code = _cmd_code; + packet.seq_num = _seq_num; + packet.data = data; + packet.data_len = length; + time_out = _time_out; + } +} Command; + +class CommandChannelDelegate { + public: + virtual void OnCommand(uint8_t handle, const Command &command) = 0; + virtual void OnHeartbeatStateUpdate(uint8_t handle, const HeartbeatResponse &state) = 0; +}; + +/** + * CommandChannel implements the sending/receiving commands with a specific device. + */ +class CommandChannel : public IOLoop::IOLoopDelegate { + public: + CommandChannel(apr_port_t port, + uint8_t handle, + const std::string &remote_ip, + CommandChannelDelegate *cb, + apr_pool_t *pool); + virtual ~CommandChannel() { Uninit(); } + + /** Uninitialize CommandChannel. */ + void Uninit(); + + /** + * Bind a CommandChannel with a IOLoop. + * @param loop the IOLoop to bind. + * @return true on successfully. + */ + bool Bind(IOLoop *loop); + + /** + * Send a command asynchronously. + * @param command the command to send. + */ + void SendAsync(const Command &command); + + void OnData(apr_socket_t *, void *); + void OnTimer(apr_time_t now); + + static uint16_t GenerateSeq(); + + private: + void Send(const Command &cmd); + void HeartBeat(apr_time_t 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; + uint8_t handle_; + apr_port_t port_; + apr_socket_t *sock_; + apr_pool_t *mem_pool_; + IOLoop *loop_; + CommandChannelDelegate *callback_; + std::map > commands_; + boost::scoped_ptr comm_port_; + apr_time_t heartbeat_time_; + std::string remote_ip_; + apr_time_t last_heartbeat_; +}; + +} // namespace livox + +#endif // LIVOX_COMMAND_CHANNEL_H_ diff --git a/sdk_core/src/command_handler/command_handler.cpp b/sdk_core/src/command_handler/command_handler.cpp new file mode 100644 index 0000000..1917d68 --- /dev/null +++ b/sdk_core/src/command_handler/command_handler.cpp @@ -0,0 +1,179 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "command_handler.h" +#include +#include +#include "command_impl.h" +#include "device_manager.h" +#include "hub_command_handler.h" +#include "lidar_command_handler.h" + +using boost::lock_guard; +using boost::mutex; +using boost::shared_ptr; +using std::list; +using std::make_pair; +using std::multimap; +using std::pair; + +namespace livox { + +CommandHandler &command_handler() { + static CommandHandler handler; + return handler; +} + +uint16_t GetCommandTimeout(uint8_t command_set, uint8_t command_id) { + switch (command_set) { + case kCommandSetGeneral: + assert(command_id < sizeof(GeneralCommandTimeout)); + return GeneralCommandTimeout[command_id]; + case kCommandSetLidar: + assert(command_id < sizeof(LidarCommandTimeout)); + return LidarCommandTimeout[command_id]; + case kCommandSetHub: + assert(command_id < sizeof(HubCommandTimeout)); + return HubCommandTimeout[command_id]; + } + return KDefaultTimeOut; +} + +bool CommandHandler::AddDevice(const DeviceInfo &info) { + if (impl_ == NULL) { + DeviceMode mode = static_cast(device_manager().device_mode()); + if (mode == kDeviceModeHub) { + impl_.reset(new HubCommandHandlerImpl(this, mem_pool_, loop_)); + } else if (mode == kDeviceModeLidar) { + impl_.reset(new LidarCommandHandlerImpl(this, mem_pool_, loop_)); + } + } + if (impl_ == NULL) { + return false; + } + + return impl_->AddDevice(info); +} + +bool CommandHandler::Init(IOLoop *loop) { + apr_status_t rv = apr_pool_create(&mem_pool_, NULL); + if (rv != APR_SUCCESS) { + return false; + } + + loop_ = loop; + return true; +} + +void CommandHandler::Uninit() { + loop_ = NULL; + if (impl_) { + impl_.reset(NULL); + } + + if (mem_pool_) { + apr_pool_destroy(mem_pool_); + mem_pool_ = NULL; + } +} + +void CommandHandler::OnCommand(uint8_t handle, const Command &command) { + if (command.packet.packet_type == kCommandTypeAck) { + OnCommandAck(handle, command); + } else if (command.packet.packet_type == kCommandTypeMsg) { + OnCommandMsg(handle, command); + } +} + +bool CommandHandler::SendCommand(uint8_t handle, + uint8_t command_set, + uint8_t command_id, + uint8_t *data, + uint16_t length, + const shared_ptr &cb) { + if (impl_ == NULL) { + return false; + } + + Command cmd(handle, + kCommandTypeCmd, + command_set, + command_id, + CommandChannel::GenerateSeq(), + data, + length, + GetCommandTimeout(command_set, command_id), + cb); + bool result = impl_->SendCommand(handle, cmd); + return result; +} + +bool CommandHandler::RegisterPush(uint8_t handle, + uint8_t command_set, + uint8_t command_id, + const shared_ptr &cb) { + lock_guard 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; +} + +void CommandHandler::OnCommandAck(uint8_t handle, const Command &command) { + if (command.cb != NULL) { + (*command.cb)(handle, command.packet.data); + } +} + +void CommandHandler::OnCommandMsg(uint8_t handle, const Command &command) { + list commands; + { + lock_guard lock(mutex_); + pair::iterator, multimap::iterator> ret; + ret = message_registers_.equal_range(MakeKey(command.packet.cmd_set, command.packet.cmd_code)); + + for (multimap::iterator ite = ret.first; ite != ret.second; ++ite) { + if (ite->second.handle == handle) { + commands.push_back(ite->second); + } + } + } + + for (list::iterator ite = commands.begin(); ite != commands.end(); ++ite) { + if (ite->cb) { + (*ite->cb)(handle, command.packet.data); + } + } +} + +void CommandHandler::RemoveDevice(uint8_t handle) { + if (impl_) { + impl_->RemoveDevice(handle); + }; +} + +void CommandHandler::OnHeartbeatStateUpdate(uint8_t handle, const HeartbeatResponse &state) { + device_manager().UpdateDeviceState(handle, state); +} + +} // namespace livox diff --git a/sdk_core/src/command_handler/command_handler.h b/sdk_core/src/command_handler/command_handler.h new file mode 100644 index 0000000..279c255 --- /dev/null +++ b/sdk_core/src/command_handler/command_handler.h @@ -0,0 +1,109 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_COMMAND_HANDLER_H_ +#define LIVOX_COMMAND_HANDLER_H_ + +#include +#include +#include "base/command_callback.h" +#include "base/util.h" +#include "command_channel.h" +#include "device_discovery.h" +#include "livox_sdk.h" + +namespace livox { +class CommandHandlerImpl; + +class CommandHandler : public noncopyable { + public: + explicit CommandHandler() : mem_pool_(NULL), loop_(NULL) {} + + bool Init(IOLoop *loop); + void Uninit(); + + 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 &cb); + + bool RegisterPush(uint8_t handle, + uint8_t command_set, + uint8_t command_id, + const boost::shared_ptr &cb); + + void OnCommand(uint8_t handle, const Command &command); + + void OnHeartbeatStateUpdate(uint8_t handle, const HeartbeatResponse &state); + + private: + inline uint16_t MakeKey(uint8_t command_set, uint8_t command_id) { return (command_set << 8) | command_id; } + + void OnCommandAck(uint8_t handle, const Command &command); + void OnCommandMsg(uint8_t handle, const Command &command); + + private: + std::multimap message_registers_; + apr_pool_t *mem_pool_; + boost::scoped_ptr impl_; + boost::mutex mutex_; + IOLoop *loop_; +}; + +class CommandHandlerImpl : public CommandChannelDelegate { + public: + CommandHandlerImpl(CommandHandler *handler) : handler_(handler) {} + virtual ~CommandHandlerImpl() { Uninit(); } + virtual void Uninit(){}; + 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 void OnCommand(uint8_t handle, const Command &command) { + if (handler_) { + handler_->OnCommand(handle, command); + } + } + + void OnHeartbeatStateUpdate(uint8_t handle, const HeartbeatResponse &state) { + if (handler_) { + handler_->OnHeartbeatStateUpdate(handle, state); + } + } + + private: + CommandHandler *handler_; +}; + +CommandHandler &command_handler(); + +} // namespace livox + +#endif // LIVOX_COMMAND_HANDLER_H_ diff --git a/sdk_core/src/command_handler/command_impl.cpp b/sdk_core/src/command_handler/command_impl.cpp new file mode 100644 index 0000000..eed8f36 --- /dev/null +++ b/sdk_core/src/command_handler/command_impl.cpp @@ -0,0 +1,352 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include + +#include "command_handler.h" +#include "command_impl.h" +#include "data_handler/data_handler.h" +#include "device_manager.h" +#include "livox_def.h" +#include "livox_sdk.h" + +using std::pair; +using std::vector; +using namespace livox; + +void SetDeviceStateUpdateCallback(DeviceStateUpdateCallback cb) { + device_manager().SetDeviceConnectedCallback(cb); +} + +void SetBroadcastCallback(DeviceBroadcastCallback cb) { + device_manager().SetDeviceBroadcastCallback(cb); +} + +uint8_t AddHubToConnect(const char *broadcast_code, uint8_t *handle) { + bool result = device_manager().AddListeningDevice(broadcast_code, kDeviceModeHub, *handle); + if (result) { + return kStatusSuccess; + } else { + return kStatusFailure; + } +} + +uint8_t AddLidarToConnect(const char *broadcast_code, uint8_t *handle) { + bool result = device_manager().AddListeningDevice(broadcast_code, kDeviceModeLidar, *handle); + if (result) { + return kStatusSuccess; + } else { + return kStatusFailure; + } +} + +uint8_t GetConnectedDevices(DeviceInfo *devices, uint8_t *size) { + if (devices == NULL || size == NULL) { + return kStatusFailure; + } + vector device; + device_manager().GetConnectedDevices(device); + uint16_t final_size = (*size >= device.size()) ? device.size() : *size; + for (int i = 0; i < final_size; i++) { + devices[i] = device[i]; + } + *size = final_size; + return kStatusSuccess; +} + +void SetDataCallback(uint8_t handle, DataCallback cb) { + data_handler().AddDataListener(handle, cb); +} + +uint8_t DeviceSampleControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) { + uint8_t req = enable; + bool result = command_handler().SendCommand(handle, + kCommandSetGeneral, + kCommandIDGeneralControlSample, + &req, + sizeof(req), + MakeCommandCallback(cb, data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t LidarStartSampling(uint8_t handle, CommonCommandCallback cb, void *data) { + if (device_manager().device_mode() != kDeviceModeLidar) { + return kStatusNotSupported; + } + return DeviceSampleControl(handle, true, cb, data); +} + +uint8_t LidarStopSampling(uint8_t handle, CommonCommandCallback cb, void *data) { + if (device_manager().device_mode() != kDeviceModeLidar) { + return kStatusNotSupported; + } + return DeviceSampleControl(handle, false, cb, data); +} + +uint8_t 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) { + if (device_manager().device_mode() != kDeviceModeHub) { + return kStatusNotSupported; + } + return DeviceSampleControl(kHubDefaultHandle, false, cb, client_data); +} + +uint8_t 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(cb, data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t SetErrorMessageCallback(uint8_t handle, ErrorMessageCallback cb) { + bool result = command_handler().RegisterPush( + handle, kCommandSetGeneral, kCommandIDGeneralPushAbnormalState, MakeMessageCallback(cb)); + return result ? kStatusSuccess : kStatusFailure; +} + +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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t GetDeviceIPInformation(uint8_t handle, GetDeviceIPInformationCallback cb, void *client_data) { + bool result = command_handler().SendCommand(handle, + kCommandSetGeneral, + kCommandIDGeneralGetDeviceIPInformation, + NULL, + 0, + MakeCommandCallback(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t LidarSetMode(uint8_t handle, LidarMode mode, CommonCommandCallback cb, void *client_data) { + if (device_manager().device_mode() != kDeviceModeLidar) { + return kStatusNotSupported; + } + uint8_t req = static_cast(mode); + bool result = command_handler().SendCommand(handle, + kCommandSetLidar, + kCommandIDLidarSetMode, + &req, + sizeof(req), + MakeCommandCallback(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) { + uint8_t req = static_cast(enable); + bool result = command_handler().SendCommand(handle, + kCommandSetLidar, + kCommandIDLidarControlRainFogSuppression, + &req, + sizeof(req), + MakeCommandCallback(cb, data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t HubGetExtrinsicParameter(HubGetExtrinsicParameterRequest *req, + uint16_t length, + HubGetExtrinsicParameterCallback cb, + void *client_data) { + if (device_manager().device_mode() != kDeviceModeHub) { + return kStatusNotSupported; + } + bool result = command_handler().SendCommand(kHubDefaultHandle, + kCommandSetHub, + kCommandIDHubGetExtrinsicParameter, + (uint8_t *)req, + length, + MakeCommandCallback(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t HubExtrinsicParameterCalculation(bool enable, CommonCommandCallback cb, void *client_data) { + if (device_manager().device_mode() != kDeviceModeHub) { + return kStatusNotSupported; + } + uint8_t req = static_cast(enable); + bool result = command_handler().SendCommand(kHubDefaultHandle, + kCommandSetHub, + kCommandIDHubExtrinsicParameterCalculation, + &req, + sizeof(req), + MakeCommandCallback(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} + +uint8_t HubRainFogSuppress(HubRainFogSuppressRequest *req, + uint16_t length, + HubRainFogSuppressCallback 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(cb, client_data)); + return result ? kStatusSuccess : kStatusFailure; +} diff --git a/sdk_core/src/command_handler/command_impl.h b/sdk_core/src/command_handler/command_impl.h new file mode 100644 index 0000000..8514a61 --- /dev/null +++ b/sdk_core/src/command_handler/command_impl.h @@ -0,0 +1,196 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_SDK_COMMAND_IMPL_H +#define LIVOX_SDK_COMMAND_IMPL_H + +namespace livox { + +/** The maximum buffer size of command */ +static const uint32_t kMaxCommandBufferSize = 1536; +static const uint16_t KDefaultTimeOut = 500; + +/** Enum that represents the index of command set. */ +typedef enum { + kCommandSetGeneral = 0, /**< general command set. */ + kCommandSetLidar, /**< LiDAR command set. */ + kCommandSetHub /**< hub command set. */ +} CommandSet; + +/** Enum that represents the command id. */ +typedef enum { + /** General command set, broadcast command. */ + kCommandIDGeneralBroadcast = 0, + /** + * General command set, handshake command. When received broadcast from a Livox LiDAR or Hub, a handshake command + * should be sent back to the same port to negotiate connection with the device. + */ + kCommandIDGeneralHandshake = 1, + /** + * General command set, query the information of device. + */ + kCommandIDGeneralDeviceInfo = 2, + /** + * General command set, heartbeat command. after connected, a heartbeat command should be sent at least every second. + */ + kCommandIDGeneralHeartbeat = 3, + /** + * General command set, enable or disable the sampling. + * + */ + kCommandIDGeneralControlSample = 4, + /** + * General command set, change the coordinate of point cloud data. + */ + kCommandIDGeneralCoordinateSystem = 5, + /** + * General command set, disconnect the device. + */ + kCommandIDGeneralDisconnect = 6, + /** + * General command set, a message command from a connected device to notify exceptions, in 10Hz. + */ + kCommandIDGeneralPushAbnormalState = 7, + /** + * General command set, set the IP of the a device. + */ + kCommandIDGeneralConfigureStaticDynamicIP = 8, + /** + * General command set, get the IP of the a device. + */ + kCommandIDGeneralGetDeviceIPInformation = 9, + + /** + * ****************************************************** + * Don't add command id after kCommandIDGeneralCommandCount. + */ + kCommandIDGeneralCommandCount +} GeneralCommandID; + +static const uint16_t GeneralCommandTimeout[kCommandIDGeneralCommandCount] = {KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut}; + +/** Enum that represents the command id. */ +typedef enum { + /** + * Lidar command set, set the working mode and sub working mode of a LiDAR. + */ + kCommandIDLidarSetMode = 0, + /** + * Lidar command set, set the parameters of a LiDAR. + */ + kCommandIDLidarSetExtrinsicParameter = 1, + /** + * Lidar command set, get the parameters of a LiDAR. + */ + kCommandIDLidarGetExtrinsicParameter = 2, + /** + * Lidar command set, enable or disable the rain/fog suppression of a LiDAR. + */ + kCommandIDLidarControlRainFogSuppression = 3, + + /** + * ****************************************************** + * Don't add command id after kCommandIDLidarCommandCount. + */ + kCommandIDLidarCommandCount +} LidarCommandID; + +static const uint16_t LidarCommandTimeout[kCommandIDLidarCommandCount] = {KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut}; + +/** Enum that represents the command id. */ +typedef enum { + /** + * Hub command set, get the information of connected Livox LiDAR. + */ + kCommandIDHubQueryLidarInformation = 0, + /** + * Hub command set, set the working mode of connected Livox LiDAR. + */ + kCommandIDHubSetMode = 1, + /** + * Hub command set, enable or disable the power supply of the hub slot. + */ + kCommandIDHubControlSlotPower = 2, + /** + * Hub command set, set the parameters of connected Livox LiDAR. + */ + kCommandIDHubSetExtrinsicParameter = 3, + /** + * Hub command set, get the parameters of connected Livox LiDAR. + */ + kCommandIDHubGetExtrinsicParameter = 4, + /** + * Hub command set, get the working state of the connected Livox LiDAR. + */ + kCommandIDHubQueryLidarDeviceStatus = 5, + /** + * Hub command set, enable or disable calculating the parameters. + */ + kCommandIDHubExtrinsicParameterCalculation = 6, + /** + * Hub command set, open or close the rain and fog mode of the connected Livox LiDAR. + */ + kCommandIDHubRainFogSuppression = 7, + + /** + * ****************************************************** + * Don't add command id after kCommandIDHubCommandCount. + */ + kCommandIDHubCommandCount +} HubCommandID; + +static const uint16_t HubCommandTimeout[kCommandIDHubCommandCount] = {KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut, + KDefaultTimeOut}; + +/** + * Enum that represents the command type. + */ +typedef enum { + /** command type, which requires response from the receiver. */ + kCommandTypeCmd = 0, + /** acknowledge type, which is the response of command type. */ + kCommandTypeAck = 1, + /** message type, which is sent at a specified frequency. */ + kCommandTypeMsg = 2 +} CommandType; +} // namespace livox +#endif // LIVOX_SDK_COMMAND_IMPL_H diff --git a/sdk_core/src/command_handler/hub_command_handler.cpp b/sdk_core/src/command_handler/hub_command_handler.cpp new file mode 100644 index 0000000..107b40d --- /dev/null +++ b/sdk_core/src/command_handler/hub_command_handler.cpp @@ -0,0 +1,65 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "hub_command_handler.h" +#include "base/network_util.h" + +namespace livox { + +void HubCommandHandlerImpl::Uninit() { + if (channel_) { + channel_.reset(NULL); + } + is_valid_ = false; +} + +bool HubCommandHandlerImpl::AddDevice(const DeviceInfo &info) { + if (is_valid_) { + return false; + } + is_valid_ = true; + hub_info_ = info; + + channel_.reset(new CommandChannel(info.cmd_port, info.handle, info.ip, this, mem_pool_)); + channel_->Bind(loop_); + return true; +} + +bool HubCommandHandlerImpl::SendCommand(uint8_t, const Command &command) { + if (channel_ == NULL) { + return false; + } + channel_->SendAsync(command); + return true; +} + +bool HubCommandHandlerImpl::RemoveDevice(uint8_t) { + is_valid_ = false; + if (channel_) { + channel_.reset(NULL); + } + return false; +} + +} // namespace livox diff --git a/sdk_core/src/command_handler/hub_command_handler.h b/sdk_core/src/command_handler/hub_command_handler.h new file mode 100644 index 0000000..eef5382 --- /dev/null +++ b/sdk_core/src/command_handler/hub_command_handler.h @@ -0,0 +1,49 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_HUB_COMMAND_HANDLER_H_ +#define LIVOX_HUB_COMMAND_HANDLER_H_ + +#include +#include "command_handler.h" + +namespace livox { +class HubCommandHandlerImpl : public CommandHandlerImpl { + public: + HubCommandHandlerImpl(CommandHandler *handler, apr_pool_t *pool, IOLoop *loop) + : CommandHandlerImpl(handler), mem_pool_(pool), loop_(loop), is_valid_(false) {} + void Uninit(); + bool AddDevice(const DeviceInfo &info); + bool RemoveDevice(uint8_t handle); + bool SendCommand(uint8_t handle, const Command &command); + + private: + apr_pool_t *mem_pool_; + IOLoop *loop_; + bool is_valid_; + DeviceInfo hub_info_; + boost::scoped_ptr channel_; +}; +} // namespace livox +#endif // LIVOX_HUB_COMMAND_HANDLER_H_ diff --git a/sdk_core/src/command_handler/lidar_command_handler.cpp b/sdk_core/src/command_handler/lidar_command_handler.cpp new file mode 100644 index 0000000..e30b343 --- /dev/null +++ b/sdk_core/src/command_handler/lidar_command_handler.cpp @@ -0,0 +1,74 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lidar_command_handler.h" + +using std::list; + +namespace livox { + +void LidarCommandHandlerImpl::Uninit() { + devices_.clear(); +} + +bool LidarCommandHandlerImpl::AddDevice(const DeviceInfo &info) { + boost::shared_ptr channel = + boost::make_shared(info.cmd_port, info.handle, info.ip, this, mem_pool_); + channel->Bind(loop_); + + DeviceItem item = {channel, info}; + devices_.push_back(item); + return true; +} + +bool LidarCommandHandlerImpl::SendCommand(uint8_t handle, const Command &command) { + CommandChannel *channel = NULL; + for (list::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) { + if (ite->info.handle == handle) { + channel = ite->channel.get(); + break; + } + } + + if (channel) { + channel->SendAsync(command); + return true; + } + return false; +} + +bool LidarCommandHandlerImpl::RemoveDevice(uint8_t handle) { + bool found = false; + for (list::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) { + if (ite->info.handle == handle) { + found = true; + devices_.erase(ite); + break; + } + } + + return found; +} + +} // namespace livox diff --git a/sdk_core/src/command_handler/lidar_command_handler.h b/sdk_core/src/command_handler/lidar_command_handler.h new file mode 100644 index 0000000..6ae761d --- /dev/null +++ b/sdk_core/src/command_handler/lidar_command_handler.h @@ -0,0 +1,55 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_LIDAR_COMMAND_HANDLER_H_ +#define LIVOX_LIDAR_COMMAND_HANDLER_H_ +#include +#include "command_handler.h" + +namespace livox { + +class LidarCommandHandlerImpl : public CommandHandlerImpl { + public: + LidarCommandHandlerImpl(CommandHandler *handler, apr_pool_t *pool, IOLoop *loop) + : CommandHandlerImpl(handler), mem_pool_(pool), loop_(loop) {} + + void Uninit(); + + bool AddDevice(const DeviceInfo &info); + bool RemoveDevice(uint8_t handle); + bool SendCommand(uint8_t handle, const Command &command); + + private: + typedef struct { + boost::shared_ptr channel; + DeviceInfo info; + } DeviceItem; + apr_pool_t *mem_pool_; + std::list devices_; + IOLoop *loop_; +}; + +} // namespace livox + +#endif // LIVOX_LIDAR_COMMAND_HANDLER_H_ diff --git a/sdk_core/src/data_handler/data_handler.cpp b/sdk_core/src/data_handler/data_handler.cpp new file mode 100644 index 0000000..1da5163 --- /dev/null +++ b/sdk_core/src/data_handler/data_handler.cpp @@ -0,0 +1,124 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "data_handler.h" +#include +#include "hub_data_handler.h" +#include "lidar_data_handler.h" + +namespace livox { + +static const size_t kSphericalCoordinatePointSize = 9; +static const size_t kCartesianCoordinatePointSize = 13; +static const size_t kPrefixDataSize = 18; + +DataHandler &data_handler() { + static DataHandler handler; + return handler; +} + +#pragma pack(1) + +#pragma pack() + +bool DataHandler::AddDataListener(uint8_t handle, const DataCallback &cb) { + if (handle >= callbacks_.size()) { + return false; + } + callbacks_[handle] = cb; + return true; +} + +bool DataHandler::AddDevice(const DeviceInfo &info) { + if (impl_ == NULL) { + DeviceMode mode = static_cast(device_manager().device_mode()); + if (mode == kDeviceModeHub) { + impl_.reset(new HubDataHandlerImpl(this, mem_pool_)); + } else if (mode == kDeviceModeLidar) { + impl_.reset(new LidarDataHandlerImpl(this, mem_pool_)); + } + + if (impl_ == NULL || !impl_->Init()) { + impl_.reset(NULL); + return false; + } + } + return impl_->AddDevice(info); +} + +bool DataHandler::Init() { + apr_status_t rv = apr_pool_create(&mem_pool_, NULL); + if (rv != APR_SUCCESS) { + return false; + } + return true; +} + +void DataHandler::Uninit() { + if (impl_) { + impl_.reset(NULL); + } + if (mem_pool_) { + apr_pool_destroy(mem_pool_); + mem_pool_ = NULL; + } +} + +void DataHandler::OnDataCallback(uint8_t handle, void *data, uint16_t size) { + LivoxEthPacket *lidar_data = (LivoxEthPacket *)data; + if (lidar_data == NULL) { + return; + } + if (handle >= callbacks_.size()) { + return; + } + DataCallback cb = callbacks_[handle]; + if (lidar_data->data_type == 0) { + size = (size - kPrefixDataSize) / kCartesianCoordinatePointSize; + } else if (lidar_data->data_type == 1) { + size = (size - kPrefixDataSize) / kSphericalCoordinatePointSize; + } else { + return; + } + if (cb) { + // LOG_INFO << " device_sn: " << device_sn << std::endl; + // LOG_INFO << " version: " << (uint32_t) lidar_data_cb->version << std::endl; + // LOG_INFO << " slot: " << (uint32_t) lidar_data_cb->slot << std::endl; + // LOG_INFO << " resverd : " << (uint32_t) lidar_data_cb->reserved << std::endl; + // LOG_INFO << " errorCode: " << (uint32_t) lidar_data_cb->err_code << std::endl; + // LOG_INFO << " lidarID: " << (uint16_t) lidar_data_cb->id << std::endl; + // LOG_INFO << " timeStampType: " << (uint32_t) (lidar_data_cb->timestamp_type) << std::endl; + // LOG_INFO << " timeStamp: " << (uint32_t) *(lidar_data_cb->time_stamp) << std::endl; + // LOG_INFO << " dataType: " << (uint16_t) lidar_data->data_type << std::endl << std::endl; + cb(handle, lidar_data, size); + } +} + +void DataHandler::RemoveDevice(uint8_t handle) { + if (impl_) { + impl_->RemoveDevice(handle); + } +} + +} // namespace livox diff --git a/sdk_core/src/data_handler/data_handler.h b/sdk_core/src/data_handler/data_handler.h new file mode 100644 index 0000000..95dfab3 --- /dev/null +++ b/sdk_core/src/data_handler/data_handler.h @@ -0,0 +1,87 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_DATA_HANDLER_H_ +#define LIVOX_DATA_HANDLER_H_ + +#include +#include +#include +#include +#include "apr_pools.h" +#include "base/io_thread.h" +#include "device_manager.h" + +namespace livox { +class DataHandlerImpl; + +class DataHandler : public noncopyable { + public: + typedef boost::function DataCallback; + + public: + DataHandler() : mem_pool_(NULL) {} + + bool Init(); + void Uninit(); + + bool AddDevice(const DeviceInfo &info); + void RemoveDevice(uint8_t handle); + + bool AddDataListener(uint8_t handle, const DataCallback &cb); + // bool RemoveDataListener(uint8_t handle); + void OnDataCallback(uint8_t handle, void *data, uint16_t size); + + private: + apr_pool_t *mem_pool_; + boost::array callbacks_; + boost::scoped_ptr impl_; +}; + +class DataHandlerImpl : public IOLoop::IOLoopDelegate { + public: + DataHandlerImpl(DataHandler *handler) : handler_(handler) {} + virtual ~DataHandlerImpl() {} + + virtual bool Init() = 0; + virtual void Uninit(){}; + + virtual bool AddDevice(const DeviceInfo &info) = 0; + virtual void RemoveDevice(uint8_t handle) = 0; + void OnDataCallback(uint8_t handle, void *data, uint16_t size) { + if (handler_) { + handler_->OnDataCallback(handle, data, size); + } + } + + protected: + static const size_t kMaxBufferSize = 8192; + DataHandler *handler_; +}; + +DataHandler &data_handler(); + +} // namespace livox + +#endif // LIVOX_DATA_HANDLER_H_ diff --git a/sdk_core/src/data_handler/hub_data_handler.cpp b/sdk_core/src/data_handler/hub_data_handler.cpp new file mode 100644 index 0000000..e5ab106 --- /dev/null +++ b/sdk_core/src/data_handler/hub_data_handler.cpp @@ -0,0 +1,93 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "hub_data_handler.h" +#include +#include "base/network_util.h" + +namespace livox { + +bool HubDataHandlerImpl::Init() { + if (thread_->Init(false, false)) { + return thread_->Start(); + } + return false; +} + +void HubDataHandlerImpl::Uninit() { + if (thread_) { + thread_->Quit(); + thread_->Join(); + thread_->Uninit(); + thread_.reset(NULL); + } + + if (sock_) { + apr_socket_close(sock_); + sock_ = NULL; + } + is_valid_ = false; + mem_pool_ = NULL; +} + +bool HubDataHandlerImpl::AddDevice(const DeviceInfo &info) { + if (is_valid_) { + return false; + } + is_valid_ = true; + hub_info_ = info; + + sock_ = util::CreateBindSocket(info.data_port, mem_pool_); + if (sock_ == NULL) { + is_valid_ = false; + return false; + } + + thread_->loop()->AddDelegate(sock_, this); + return true; +} + +void HubDataHandlerImpl::OnData(apr_socket_t *, void *) { + apr_sockaddr_t addr; + if (buf_.size() < kMaxBufferSize) { + buf_.resize(kMaxBufferSize); + } + apr_size_t size = kMaxBufferSize; + apr_socket_recvfrom(&addr, sock_, 0, &buf_[0], &size); + + if (handler_) { + handler_->OnDataCallback(hub_info_.handle, &buf_[0], size); + } +} + +void HubDataHandlerImpl::RemoveDevice(uint8_t handle) { + if (handle == hub_info_.handle) { + thread_->loop()->RemoveDelegate(sock_, this); + apr_socket_close(sock_); + sock_ = NULL; + is_valid_ = false; + } +} + +} // namespace livox diff --git a/sdk_core/src/data_handler/hub_data_handler.h b/sdk_core/src/data_handler/hub_data_handler.h new file mode 100644 index 0000000..a3bb2bd --- /dev/null +++ b/sdk_core/src/data_handler/hub_data_handler.h @@ -0,0 +1,57 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_HUB_DATA_HANDLER_H_ +#define LIVOX_HUB_DATA_HANDLER_H_ + +#include +#include "base/io_thread.h" +#include "data_handler.h" + +namespace livox { +class HubDataHandlerImpl : public DataHandlerImpl { + public: + HubDataHandlerImpl(DataHandler *handler, apr_pool_t *mem_pool) + : DataHandlerImpl(handler), mem_pool_(mem_pool), thread_(new IOThread()), sock_(NULL), is_valid_(false) {} + + ~HubDataHandlerImpl() { Uninit(); } + bool Init(); + void Uninit(); + + bool AddDevice(const DeviceInfo &info); + void RemoveDevice(uint8_t t); + void OnData(apr_socket_t *sock, void *client_data); + + private: + apr_pool_t *mem_pool_; + boost::scoped_ptr thread_; + apr_socket_t *sock_; + DeviceInfo hub_info_; + bool is_valid_; + std::vector buf_; +}; + +} // namespace livox + +#endif // LIVOX_HUB_DATA_HANDLER_H_ diff --git a/sdk_core/src/data_handler/lidar_data_handler.cpp b/sdk_core/src/data_handler/lidar_data_handler.cpp new file mode 100644 index 0000000..4031b6a --- /dev/null +++ b/sdk_core/src/data_handler/lidar_data_handler.cpp @@ -0,0 +1,123 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "lidar_data_handler.h" +#include +#include +#include "base/network_util.h" + +using boost::lock_guard; +using boost::mutex; +using boost::shared_ptr; +using std::list; + +namespace livox { + +bool LidarDataHandlerImpl::Init() { + return true; +} + +void LidarDataHandlerImpl::Uninit() { + for (list::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) { + DeviceItem &item = *ite; + if (item.thread) { + if (item.thread->loop()) { + item.thread->loop()->RemoveDelegate(item.sock, this); + } + item.thread->Quit(); + item.thread->Join(); + item.thread->Uninit(); + } + + if (item.sock) { + apr_socket_close(item.sock); + } + } + + devices_.clear(); +} + +bool LidarDataHandlerImpl::AddDevice(const DeviceInfo &info) { + apr_socket_t *sock = util::CreateBindSocket(info.data_port, mem_pool_); + if (sock == NULL) { + return false; + } + + shared_ptr thread = boost::make_shared(); + thread->Init(false, false); + thread->loop()->AddDelegate(sock, this, reinterpret_cast(info.handle)); + { + lock_guard lock(mutex_); + DeviceItem item = {sock, thread, info.handle}; + devices_.push_back(item); + } + return thread->Start(); +} + +void LidarDataHandlerImpl::RemoveDevice(uint8_t handle) { + DeviceItem item; + bool found = false; + { + lock_guard lock(mutex_); + for (list::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) { + if (ite->handle == handle) { + item = *ite; + found = true; + devices_.erase(ite); + break; + } + } + } + + if (found) { + item.thread->loop()->RemoveDelegate(item.sock, this); + item.thread->Quit(); + item.thread->Join(); + item.thread->Uninit(); + if (item.sock) { + apr_socket_close(item.sock); + } + } +} + +void LidarDataHandlerImpl::OnData(apr_socket_t *sock, void *client_data) { + uint8_t handle = static_cast(reinterpret_cast(client_data)); + if (handle > data_buffers_.size()) { + return; + } + boost::scoped_array &buf = data_buffers_[handle]; + if (buf.get() == NULL) { + buf.reset(new char[kMaxBufferSize]); + } + + apr_sockaddr_t addr; + apr_size_t size = kMaxBufferSize; + if (APR_SUCCESS == apr_socket_recvfrom(&addr, sock, 0, buf.get(), &size)) { + if (handler_) { + handler_->OnDataCallback(handle, buf.get(), size); + } + } +} + +} // namespace livox diff --git a/sdk_core/src/data_handler/lidar_data_handler.h b/sdk_core/src/data_handler/lidar_data_handler.h new file mode 100644 index 0000000..e50b157 --- /dev/null +++ b/sdk_core/src/data_handler/lidar_data_handler.h @@ -0,0 +1,61 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_LIDAR_DATA_HANDLER_H_ +#define LIVOX_LIDAR_DATA_HANDLER_H_ + +#include +#include +#include +#include "data_handler.h" +#include "device_manager.h" + +namespace livox { + +class LidarDataHandlerImpl : public DataHandlerImpl { + public: + LidarDataHandlerImpl(DataHandler *handler, apr_pool_t *mem_pool) : DataHandlerImpl(handler), mem_pool_(mem_pool) {} + ~LidarDataHandlerImpl() { Uninit(); } + bool Init(); + void Uninit(); + bool AddDevice(const DeviceInfo &info); + void RemoveDevice(uint8_t handle); + void OnData(apr_socket_t *sock, void *client_data); + + private: + typedef struct { + apr_socket_t *sock; + boost::shared_ptr thread; + uint16_t handle; + } DeviceItem; + std::list devices_; + + boost::array, kMaxConnectedDeviceNum> data_buffers_; + apr_pool_t *mem_pool_; + boost::mutex mutex_; +}; + +} // namespace livox + +#endif // LIVOX_LIDAR_DATA_HANDLER_H_ diff --git a/sdk_core/src/device_discovery.cpp b/sdk_core/src/device_discovery.cpp new file mode 100644 index 0000000..fed9dd4 --- /dev/null +++ b/sdk_core/src/device_discovery.cpp @@ -0,0 +1,247 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "device_discovery.h" +#include +#include +#include +#include +#include +#include "apr_network_io.h" +#include "apr_pools.h" +#include "arpa/inet.h" +#include "base/logging.h" +#include "base/network_util.h" +#include "command_handler/command_impl.h" +#include "device_manager.h" +#include "livox_def.h" + +using boost::tuple; +using std::string; +using std::vector; + +namespace livox { + +uint16_t DeviceDiscovery::port_count = 0; + +bool DeviceDiscovery::Init() { + apr_status_t rv = apr_pool_create(&mem_pool_, NULL); + if (rv != APR_SUCCESS) { + LOG_ERROR << PrintAPRStatus(rv) << std::endl; + return false; + } + if (comm_port_ == NULL) { + comm_port_.reset(new CommPort()); + } + return true; +} + +bool DeviceDiscovery::Start(IOLoop *loop) { + if (loop == NULL) { + return false; + } + loop_ = loop; + sock_ = util::CreateBindSocket(kListenPort, mem_pool_); + if (sock_ == NULL) { + LOG_ERROR << "DeviceDiscovery Create Socket Failed" << std::endl; + return false; + } + loop_->AddDelegate(sock_, this); + return true; +} + +void DeviceDiscovery::OnData(apr_socket_t *sock, void *) { + apr_sockaddr_t addr; + uint32_t buf_size = 0; + uint8_t *cache_buf = comm_port_->FetchCacheFreeSpace(&buf_size); + apr_size_t size = buf_size; + apr_status_t rv = apr_socket_recvfrom(&addr, sock, 0, reinterpret_cast(cache_buf), &size); + + comm_port_->UpdateCacheWrIdx(size); + if (rv != APR_SUCCESS) { + LOG_ERROR << " Receive Failed " << PrintAPRStatus(rv) << std::endl; + return; + } + CommPacket packet; + memset(&packet, 0, sizeof(packet)); + + while ((kParseSuccess == comm_port_->ParseCommStream(&packet))) { + if (packet.cmd_set == kCommandSetGeneral && packet.cmd_code == kCommandIDGeneralBroadcast) { + OnBroadcast(packet, &addr); + } else if (packet.cmd_set == kCommandSetGeneral && packet.cmd_code == kCommandIDGeneralHandshake) { + if (connecting_devices_.find(sock) == connecting_devices_.end()) { + continue; + } + DeviceInfo info = boost::get<2>(connecting_devices_[sock]); + loop_->RemoveDelegate(sock, this); + apr_socket_close(sock); + apr_pool_destroy(boost::get<0>(connecting_devices_[sock])); + connecting_devices_.erase(sock); + + if (packet.data == NULL) { + continue; + } + if (*(uint8_t *)packet.data == 0) { + LOG_INFO << "New Device " << std::endl; + LOG_INFO << "Handle: " << static_cast(info.handle) << std::endl; + LOG_INFO << "Broadcast Code: " << info.broadcast_code << std::endl; + LOG_INFO << "Type: " << info.type << std::endl; + LOG_INFO << "IP: " << info.ip << std::endl; + LOG_INFO << "Command Port: " << info.cmd_port << std::endl; + LOG_INFO << "Data Port: " << info.data_port << std::endl; + LOG_INFO << std::endl; + DeviceFound(info); + } + } + } +} + +void DeviceDiscovery::OnTimer(apr_time_t now) { + ConnectingDeviceMap::iterator ite = connecting_devices_.begin(); + while (ite != connecting_devices_.end()) { + tuple &device_tuple = ite->second; + if (now - boost::get<1>(device_tuple) > apr_time_from_msec(500)) { + loop_->RemoveDelegate(ite->first, this); + apr_socket_close(ite->first); + apr_pool_destroy(boost::get<0>(device_tuple)); + connecting_devices_.erase(ite++); + } else { + ++ite; + } + } +} + +void DeviceDiscovery::Uninit() { + if (sock_) { + apr_socket_close(sock_); + sock_ = NULL; + } + + if (comm_port_) { + comm_port_.reset(NULL); + } + + if (mem_pool_) { + apr_pool_destroy(mem_pool_); + mem_pool_ = NULL; + } +} + +void DeviceDiscovery::OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr) { + if (packet.data == NULL) { + return; + } + + BroadcastDeviceInfo *device_info = (BroadcastDeviceInfo *)packet.data; + string broadcast_code = device_info->broadcast_code; + //LOG_INFO << " Boradcast broadcast code: " << broadcast_code << std::endl; + 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; + + char ip[16]; + memset(&ip, 0, sizeof(ip)); + apr_status_t rv = apr_sockaddr_ip_getbuf(ip, sizeof(ip), addr); + if (rv != APR_SUCCESS) { + LOG_ERROR << PrintAPRStatus(rv) << std::endl; + return; + } + strncpy(lidar_info.ip, ip, sizeof(lidar_info.ip)); + apr_pool_t *pool = NULL; + rv = apr_pool_create(&pool, mem_pool_); + if (rv != APR_SUCCESS) { + LOG_ERROR << PrintAPRStatus(rv) << std::endl; + return; + } + + apr_socket_t *cmd_sock = util::CreateBindSocket(lidar_info.cmd_port, pool); + if (cmd_sock == NULL) { + apr_pool_destroy(pool); + pool = NULL; + return; + } + + loop_->AddDelegate(cmd_sock, this); + OnTimer(apr_time_now()); + boost::get<0>(connecting_devices_[cmd_sock]) = pool; + boost::get<2>(connecting_devices_[cmd_sock]) = lidar_info; + + bool result = false; + do { + HandshakeRequest handshake_req; + uint32_t local_ip = 0; + if (util::FindLocalIP(addr->sa.sin, local_ip) == false) { + result = false; + break; + } + CommPacket packet; + memset(&packet, 0, sizeof(packet)); + handshake_req.ip_addr = local_ip; + handshake_req.cmd_port = lidar_info.cmd_port; + handshake_req.data_port = lidar_info.data_port; + packet.packet_type = kCommandTypeAck; + packet.seq_num = CommandChannel::GenerateSeq(); + packet.cmd_set = kCommandSetGeneral; + packet.cmd_code = kCommandIDGeneralHandshake; + packet.data_len = sizeof(handshake_req); + packet.data = (uint8_t *)&handshake_req; + + vector buf(kMaxCommandBufferSize + 1); + apr_size_t o_len = kMaxCommandBufferSize; + comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&o_len, packet); + rv = apr_socket_sendto(cmd_sock, addr, 0, reinterpret_cast(buf.data()), &o_len); + if (rv != APR_SUCCESS) { + result = false; + break; + } + boost::get<1>(connecting_devices_[cmd_sock]) = apr_time_now(); + result = true; + } while (0); + + if (result == false) { + loop_->RemoveDelegate(cmd_sock, this); + apr_socket_close(cmd_sock); + apr_pool_destroy(pool); + connecting_devices_.erase(cmd_sock); + } +} + +DeviceDiscovery &device_discovery() { + static DeviceDiscovery discovery; + return discovery; +} + +} // namespace livox diff --git a/sdk_core/src/device_discovery.h b/sdk_core/src/device_discovery.h new file mode 100644 index 0000000..22c3a12 --- /dev/null +++ b/sdk_core/src/device_discovery.h @@ -0,0 +1,89 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_DEVICE_DISCOVERY_ +#define LIVOX_DEVICE_DISCOVERY_ + +#include +#include +#include "apr_general.h" +#include "apr_network_io.h" +#include "apr_pools.h" +#include "base/io_thread.h" +#include "base/noncopyable.h" +#include "comm/comm_port.h" +#include "command_handler/command_channel.h" + +namespace livox { + +/** + * DeviceDiscovery listens broadcast message from devices, and proceed handshake + * if device is in the listening list. + */ +class DeviceDiscovery : public noncopyable, IOLoop::IOLoopDelegate { + public: + DeviceDiscovery() : sock_(NULL), mem_pool_(NULL), loop_(NULL), comm_port_(NULL) {} + bool Init(); + void Uninit(); + + /** + * start the listening of broadcast UDP message. + * @param loop IOLoop on where DeviceDiscovery runs. + * @return true if successfully. + */ + bool Start(IOLoop *loop); + + /** + * IOLoop callback delegate. + * @param client_data client data passed in IOLoop::AddDelegate + */ + void OnData(apr_socket_t *, void *client_data); + void OnTimer(apr_time_t now); + + private: + void OnBroadcast(const CommPacket &packet, apr_sockaddr_t *addr); + + private: + /** broadcast listening port number. */ + static const apr_port_t kListenPort = 55000; + /** command port number start offset. */ + static const apr_port_t kCmdPortOffset = 500; + /** data port number start offset. */ + static const apr_port_t kDataPortOffset = 1000; + + static uint16_t port_count; + apr_socket_t *sock_; + apr_pool_t *mem_pool_; + IOLoop *loop_; + boost::scoped_ptr comm_port_; + boost::mutex mutex_; + typedef std::map > ConnectingDeviceMap; + ConnectingDeviceMap connecting_devices_; +}; + +DeviceDiscovery &device_discovery(); + +} // namespace livox + +#endif // LIVOX_DEVICE_DISCOVERY_ diff --git a/sdk_core/src/device_manager.cpp b/sdk_core/src/device_manager.cpp new file mode 100644 index 0000000..cc4cc4f --- /dev/null +++ b/sdk_core/src/device_manager.cpp @@ -0,0 +1,260 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "device_manager.h" +#include +#include +#include +#include +#include "base/logging.h" +#include "command_handler/command_handler.h" +#include "command_handler/command_impl.h" +#include "data_handler/data_handler.h" + +using boost::lock_guard; +using boost::mutex; +using std::string; +using std::vector; + +namespace livox { + +inline bool IsHub(uint8_t mode) { + return mode == kDeviceTypeHub; +} + +inline bool IsLidar(uint8_t mode) { + return mode == kDeviceTypeLidarTele || mode == kDeviceTypeLidarMid40 || mode == kDeviceTypeLidarHorizon; +} + +bool DeviceManager::Init() { + apr_status_t rv = apr_pool_create(&mem_pool_, NULL); + if (rv != APR_SUCCESS) { + return false; + } + + return true; +} + +void DeviceManager::Uninit() { + broadcast_cb_ = NULL; + connected_cb_ = NULL; + device_mode_ = kDeviceModeNone; + + for (DeviceContainer::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) { + ite->clear(); + } + + if (mem_pool_) { + apr_pool_destroy(mem_pool_); + mem_pool_ = NULL; + } +} + +bool DeviceManager::AddDevice(const DeviceInfo &device) { + if (device_mode_ == kDeviceModeNone) { + if (IsHub(device.type)) { + device_mode_ = kDeviceModeHub; + } else if (IsLidar(device.type)) { + device_mode_ = kDeviceModeLidar; + } + } + + { + lock_guard lock(mutex_); + if (device.handle < devices_.size()) { + DetailDeviceInfo &info = devices_[device.handle]; + info.connected = true; + info.info = device; + } + } + return true; +} + +void DeviceManager::RemoveDevice(uint8_t handle) { + lock_guard lock(mutex_); + if (handle < devices_.size()) { + devices_[handle].connected = false; + } +} + +void DeviceManager::BroadcastDevices(const BroadcastDeviceInfo *info) { + if (broadcast_cb_) { + broadcast_cb_(info); + } +} + +void DeviceManager::UpdateDevices(const DeviceInfo &device, DeviceEvent type) { + if (device_mode_ == kDeviceModeLidar && connected_cb_) { + connected_cb_(&device, type); + } + + if ((device_mode_ == kDeviceModeHub) && (type == kEventConnect)) { + LOG_DEBUG << "Send Query lidars command" << std::endl; + command_handler().SendCommand(kHubDefaultHandle, + kCommandSetHub, + kCommandIDHubQueryLidarInformation, + NULL, + 0, + MakeCommandCallback( + this, &DeviceManager::HubLidarInfomationCallback)); + } +} + +void DeviceManager::HubLidarInfomationCallback(uint8_t status, uint8_t, HubQueryLidarInformationResponse *response) { + if (status == kStatusSuccess) { + { + lock_guard lock(mutex_); + for (int i = 0; i < response->count; i++) { + std::size_t index = (response->device_info_list[i].slot - 1) * 3 + response->device_info_list[i].id - 1; + if (index < devices_.size()) { + DetailDeviceInfo &info = devices_[index]; + info.connected = true; + strncpy( + info.info.broadcast_code, response->device_info_list[i].broadcast_code, sizeof(info.info.broadcast_code)); + info.info.handle = index; + } + } + } + if (connected_cb_) { + connected_cb_(&(devices_[kHubDefaultHandle].info), kEventConnect); + } + } else { + LOG_ERROR << "Failed to query lidars information connected to hub." << std::endl; + } +} + +void DeviceManager::GetConnectedDevices(vector &devices) { + lock_guard lock(mutex_); + for (DeviceContainer::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) { + if (ite->connected == true) { + devices.push_back(ite->info); + } + } +} + +bool DeviceManager::AddListeningDevice(const string &broadcast_code, DeviceMode mode, uint8_t &handle) { + lock_guard lock(mutex_); + if (mode == kDeviceModeHub) { + handle = kHubDefaultHandle; + devices_[kHubDefaultHandle].connected = false; + strncpy(devices_[kHubDefaultHandle].info.broadcast_code, + broadcast_code.c_str(), + sizeof(devices_[kHubDefaultHandle].info.broadcast_code)); + devices_[kHubDefaultHandle].info.handle = kHubDefaultHandle; + return true; + } + + for (DeviceContainer::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) { + if (strlen(ite->info.broadcast_code) == 0) { + handle = ite - devices_.begin(); + ite->connected = false; + strncpy(ite->info.broadcast_code, broadcast_code.c_str(), sizeof(ite->info.broadcast_code)); + ite->info.handle = handle; + return true; + } + } + return false; +} + +bool DeviceManager::FindDevice(uint8_t handle, DeviceInfo &info) { + lock_guard lock(mutex_); + if (handle >= devices_.size()) { + return false; + } + + info = devices_[handle].info; + return true; +} + +bool DeviceManager::FindDevice(const string &broadcast_code, DeviceInfo &info) { + lock_guard lock(mutex_); + for (DeviceContainer::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) { + if (ite->info.broadcast_code == broadcast_code) { + info = ite->info; + return true; + } + } + return false; +} + +bool DeviceManager::IsDeviceConnected(uint8_t handle) { + lock_guard lock(mutex_); + if (handle >= devices_.size()) { + return false; + } + return devices_[handle].connected; +} + +void DeviceManager::UpdateDeviceState(uint8_t handle, const HeartbeatResponse &response) { + if (handle >= devices_.size()) { + return; + } + + // LOG_INFO << " Update State " << (uint16_t) response.state << " connect " << devices_[handle].connected << + // std::endl; + bool update = false; + DeviceInfo &info = devices_[handle].info; + if (info.state != response.state) { + info.state = static_cast(response.state); + update = true; + } + if (info.feature != response.feature) { + info.feature = static_cast(response.feature); + update = true; + } + if (info.status.progress != response.error_union.progress) { + info.status.progress = response.error_union.progress; + update = true; + } + if (devices_[handle].connected && update == true) { + if (connected_cb_) { + connected_cb_(&info, kEventStateChange); + } + } +} + +DeviceManager &device_manager() { + static DeviceManager lidar_manager; + return lidar_manager; +} + +void DeviceFound(const DeviceInfo &lidar_data) { + device_manager().AddDevice(lidar_data); + command_handler().AddDevice(lidar_data); + data_handler().AddDevice(lidar_data); + device_manager().UpdateDevices(lidar_data, kEventConnect); +} + +void DeviceRemove(uint8_t handle) { + DeviceInfo info; + bool found = device_manager().FindDevice(handle, info); + device_manager().RemoveDevice(handle); + command_handler().RemoveDevice(handle); + data_handler().RemoveDevice(handle); + if (found) { + device_manager().UpdateDevices(info, kEventDisconnect); + } +} + +} // namespace livox diff --git a/sdk_core/src/device_manager.h b/sdk_core/src/device_manager.h new file mode 100644 index 0000000..05ed7a0 --- /dev/null +++ b/sdk_core/src/device_manager.h @@ -0,0 +1,156 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#ifndef LIVOX_DEVICE_MANAGER_H_ +#define LIVOX_DEVICE_MANAGER_H_ + +#include +#include +#include +#include +#include "device_discovery.h" +#include "livox_sdk.h" + +namespace livox { + +/** Maximum number of connected devices supported. */ +static const uint8_t kMaxConnectedDeviceNum = 32; +/** Default handle value of hub. */ +static const uint8_t kHubDefaultHandle = kMaxConnectedDeviceNum - 1; + +/** + * Device connected mode. + */ +typedef enum { + kDeviceModeNone = 0, /**< no devices connected. */ + kDeviceModeHub = 1, /**< connected with hub. */ + kDeviceModeLidar = 2 /**< connected with lidar. */ +} DeviceMode; + +/** + * DeviceManager manage all the devices connected, including hub and lidars. + */ +class DeviceManager : public noncopyable { + public: + DeviceManager() : mem_pool_(NULL), device_mode_(kDeviceModeNone), connected_cb_(NULL), broadcast_cb_(NULL) {} + + bool Init(); + void Uninit(); + + /** + * When a new device is successfully connected, AddDevice is called to add new device to DeviceManager. + * @param device connected device information. + * @return true if successfully added. + */ + bool AddDevice(const DeviceInfo &device); + + /** + * When device is disconnected (not response to the heartbeat command), RemoveDevice is called. + * @param handle connected device handle. + */ + void RemoveDevice(uint8_t handle); + + /** + * Find device via handle. + */ + bool FindDevice(uint8_t handle, DeviceInfo &info); + + /** + * Find device via broadcast code. + */ + bool FindDevice(const std::string &broadcast_code, DeviceInfo &info); + + /** + * Check whether a device is connected. + * @param handle device handle. + * @return true if device is connected. + */ + bool IsDeviceConnected(uint8_t handle); + + void SetDeviceConnectedCallback(const boost::function &cb) { + connected_cb_ = cb; + } + void SetDeviceBroadcastCallback(const boost::function &cb) { + broadcast_cb_ = cb; + } + void HubLidarInfomationCallback(uint8_t status, uint8_t handle, HubQueryLidarInformationResponse *response); + DeviceMode device_mode() { return device_mode_; } + void BroadcastDevices(const BroadcastDeviceInfo *info); + void UpdateDevices(const DeviceInfo &device, DeviceEvent type); + void UpdateDeviceState(uint8_t handle, const HeartbeatResponse &response); + void GetConnectedDevices(std::vector &devices); + bool AddListeningDevice(const std::string &broadcast_code, DeviceMode mode, uint8_t &handle); + + private: + typedef struct _DetailDeviceInfo { + bool connected; + DeviceInfo info; + _DetailDeviceInfo() { connected = false; } + _DetailDeviceInfo(bool _connected, + const char *broadcast_code, + uint8_t handle, + uint8_t port, + uint8_t id, + uint32_t type, + uint16_t data_port, + uint16_t cmd_port, + const char *ip) { + connected = _connected; + strncpy(info.broadcast_code, broadcast_code, sizeof(info.broadcast_code)); + info.handle = handle; + info.slot = port; + info.id = id; + info.type = type; + info.data_port = data_port; + info.cmd_port = cmd_port; + strncpy(info.ip, ip, sizeof(info.ip)); + } + void clear() { + connected = false; + memset(info.broadcast_code, 0, sizeof(info.broadcast_code)); + info.handle = 0; + info.slot = 0; + info.id = 0; + info.type = 0; + info.data_port = 0; + info.cmd_port = 0; + memset(info.ip, 0, sizeof(info.ip)); + }; + } DetailDeviceInfo; + typedef boost::array DeviceContainer; + DeviceContainer devices_; + apr_pool_t *mem_pool_; + DeviceMode device_mode_; + boost::function connected_cb_; + boost::function broadcast_cb_; + boost::mutex mutex_; +}; + +DeviceManager &device_manager(); +void DeviceFound(const DeviceInfo &data); +void DeviceRemove(uint8_t handle); + +} // namespace livox + +#endif // LIVOX_DEVICE_MANAGER_H_ diff --git a/sdk_core/src/livox_sdk.cpp b/sdk_core/src/livox_sdk.cpp new file mode 100644 index 0000000..d8653cf --- /dev/null +++ b/sdk_core/src/livox_sdk.cpp @@ -0,0 +1,98 @@ +// +// The MIT License (MIT) +// +// Copyright (c) 2019 Livox. All rights reserved. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include "livox_sdk.h" +#include "apr_general.h" +#include "command_handler/command_handler.h" +#include "data_handler/data_handler.h" +#include "device_manager.h" +using namespace livox; +IOThread *g_thread = NULL; + +bool Init() { + bool result = false; + do { + if (apr_initialize() != APR_SUCCESS) { + result = false; + break; + } + + g_thread = new IOThread(); + g_thread->Init(); + + if (device_discovery().Init() == false) { + result = false; + break; + } + + if (device_manager().Init() == false) { + result = false; + break; + } + + if (command_handler().Init(g_thread->loop()) == false) { + result = false; + break; + } + if (data_handler().Init() == false) { + result = false; + break; + } + + result = true; + } while (0); + + if (result == false) { + apr_terminate(); + } + + return result; +} + +void Uninit() { + if (g_thread) { + g_thread->Quit(); + g_thread->Join(); + } + + device_discovery().Uninit(); + command_handler().Uninit(); + data_handler().Uninit(); + device_manager().Uninit(); + + if (g_thread) { + g_thread->Uninit(); + delete g_thread; + g_thread = NULL; + } + + apr_terminate(); +} + +bool Start() { + if (g_thread && device_discovery().Start(g_thread->loop()) && g_thread->Start()) { + return true; + } + return false; +} diff --git a/sdk_core/src/third_party/FastCRC/FastCRC_tables.hpp b/sdk_core/src/third_party/FastCRC/FastCRC_tables.hpp new file mode 100644 index 0000000..4726e26 --- /dev/null +++ b/sdk_core/src/third_party/FastCRC/FastCRC_tables.hpp @@ -0,0 +1,241 @@ +/* FastCRC library code is placed under the MIT license + * Copyright (c) 2014,2015 Frank Bosing + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + + /* + Tables generated with universal_crc by Danjel McGougan + */ + +// +// modify from FastCRC library @ 2018/11/20 +// + + +#ifndef FASTCRC_FASTCRC_TABLES_H_ +#define FASTCRC_FASTCRC_TABLES_H_ + +#include + +// crc16 table +const uint16_t crc_table_mcrf4xx[1024] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78, + 0x0000, 0x19d8, 0x33b0, 0x2a68, 0x6760, 0x7eb8, 0x54d0, 0x4d08, + 0xcec0, 0xd718, 0xfd70, 0xe4a8, 0xa9a0, 0xb078, 0x9a10, 0x83c8, + 0x9591, 0x8c49, 0xa621, 0xbff9, 0xf2f1, 0xeb29, 0xc141, 0xd899, + 0x5b51, 0x4289, 0x68e1, 0x7139, 0x3c31, 0x25e9, 0x0f81, 0x1659, + 0x2333, 0x3aeb, 0x1083, 0x095b, 0x4453, 0x5d8b, 0x77e3, 0x6e3b, + 0xedf3, 0xf42b, 0xde43, 0xc79b, 0x8a93, 0x934b, 0xb923, 0xa0fb, + 0xb6a2, 0xaf7a, 0x8512, 0x9cca, 0xd1c2, 0xc81a, 0xe272, 0xfbaa, + 0x7862, 0x61ba, 0x4bd2, 0x520a, 0x1f02, 0x06da, 0x2cb2, 0x356a, + 0x4666, 0x5fbe, 0x75d6, 0x6c0e, 0x2106, 0x38de, 0x12b6, 0x0b6e, + 0x88a6, 0x917e, 0xbb16, 0xa2ce, 0xefc6, 0xf61e, 0xdc76, 0xc5ae, + 0xd3f7, 0xca2f, 0xe047, 0xf99f, 0xb497, 0xad4f, 0x8727, 0x9eff, + 0x1d37, 0x04ef, 0x2e87, 0x375f, 0x7a57, 0x638f, 0x49e7, 0x503f, + 0x6555, 0x7c8d, 0x56e5, 0x4f3d, 0x0235, 0x1bed, 0x3185, 0x285d, + 0xab95, 0xb24d, 0x9825, 0x81fd, 0xccf5, 0xd52d, 0xff45, 0xe69d, + 0xf0c4, 0xe91c, 0xc374, 0xdaac, 0x97a4, 0x8e7c, 0xa414, 0xbdcc, + 0x3e04, 0x27dc, 0x0db4, 0x146c, 0x5964, 0x40bc, 0x6ad4, 0x730c, + 0x8ccc, 0x9514, 0xbf7c, 0xa6a4, 0xebac, 0xf274, 0xd81c, 0xc1c4, + 0x420c, 0x5bd4, 0x71bc, 0x6864, 0x256c, 0x3cb4, 0x16dc, 0x0f04, + 0x195d, 0x0085, 0x2aed, 0x3335, 0x7e3d, 0x67e5, 0x4d8d, 0x5455, + 0xd79d, 0xce45, 0xe42d, 0xfdf5, 0xb0fd, 0xa925, 0x834d, 0x9a95, + 0xafff, 0xb627, 0x9c4f, 0x8597, 0xc89f, 0xd147, 0xfb2f, 0xe2f7, + 0x613f, 0x78e7, 0x528f, 0x4b57, 0x065f, 0x1f87, 0x35ef, 0x2c37, + 0x3a6e, 0x23b6, 0x09de, 0x1006, 0x5d0e, 0x44d6, 0x6ebe, 0x7766, + 0xf4ae, 0xed76, 0xc71e, 0xdec6, 0x93ce, 0x8a16, 0xa07e, 0xb9a6, + 0xcaaa, 0xd372, 0xf91a, 0xe0c2, 0xadca, 0xb412, 0x9e7a, 0x87a2, + 0x046a, 0x1db2, 0x37da, 0x2e02, 0x630a, 0x7ad2, 0x50ba, 0x4962, + 0x5f3b, 0x46e3, 0x6c8b, 0x7553, 0x385b, 0x2183, 0x0beb, 0x1233, + 0x91fb, 0x8823, 0xa24b, 0xbb93, 0xf69b, 0xef43, 0xc52b, 0xdcf3, + 0xe999, 0xf041, 0xda29, 0xc3f1, 0x8ef9, 0x9721, 0xbd49, 0xa491, + 0x2759, 0x3e81, 0x14e9, 0x0d31, 0x4039, 0x59e1, 0x7389, 0x6a51, + 0x7c08, 0x65d0, 0x4fb8, 0x5660, 0x1b68, 0x02b0, 0x28d8, 0x3100, + 0xb2c8, 0xab10, 0x8178, 0x98a0, 0xd5a8, 0xcc70, 0xe618, 0xffc0, + 0x0000, 0x5adc, 0xb5b8, 0xef64, 0x6361, 0x39bd, 0xd6d9, 0x8c05, + 0xc6c2, 0x9c1e, 0x737a, 0x29a6, 0xa5a3, 0xff7f, 0x101b, 0x4ac7, + 0x8595, 0xdf49, 0x302d, 0x6af1, 0xe6f4, 0xbc28, 0x534c, 0x0990, + 0x4357, 0x198b, 0xf6ef, 0xac33, 0x2036, 0x7aea, 0x958e, 0xcf52, + 0x033b, 0x59e7, 0xb683, 0xec5f, 0x605a, 0x3a86, 0xd5e2, 0x8f3e, + 0xc5f9, 0x9f25, 0x7041, 0x2a9d, 0xa698, 0xfc44, 0x1320, 0x49fc, + 0x86ae, 0xdc72, 0x3316, 0x69ca, 0xe5cf, 0xbf13, 0x5077, 0x0aab, + 0x406c, 0x1ab0, 0xf5d4, 0xaf08, 0x230d, 0x79d1, 0x96b5, 0xcc69, + 0x0676, 0x5caa, 0xb3ce, 0xe912, 0x6517, 0x3fcb, 0xd0af, 0x8a73, + 0xc0b4, 0x9a68, 0x750c, 0x2fd0, 0xa3d5, 0xf909, 0x166d, 0x4cb1, + 0x83e3, 0xd93f, 0x365b, 0x6c87, 0xe082, 0xba5e, 0x553a, 0x0fe6, + 0x4521, 0x1ffd, 0xf099, 0xaa45, 0x2640, 0x7c9c, 0x93f8, 0xc924, + 0x054d, 0x5f91, 0xb0f5, 0xea29, 0x662c, 0x3cf0, 0xd394, 0x8948, + 0xc38f, 0x9953, 0x7637, 0x2ceb, 0xa0ee, 0xfa32, 0x1556, 0x4f8a, + 0x80d8, 0xda04, 0x3560, 0x6fbc, 0xe3b9, 0xb965, 0x5601, 0x0cdd, + 0x461a, 0x1cc6, 0xf3a2, 0xa97e, 0x257b, 0x7fa7, 0x90c3, 0xca1f, + 0x0cec, 0x5630, 0xb954, 0xe388, 0x6f8d, 0x3551, 0xda35, 0x80e9, + 0xca2e, 0x90f2, 0x7f96, 0x254a, 0xa94f, 0xf393, 0x1cf7, 0x462b, + 0x8979, 0xd3a5, 0x3cc1, 0x661d, 0xea18, 0xb0c4, 0x5fa0, 0x057c, + 0x4fbb, 0x1567, 0xfa03, 0xa0df, 0x2cda, 0x7606, 0x9962, 0xc3be, + 0x0fd7, 0x550b, 0xba6f, 0xe0b3, 0x6cb6, 0x366a, 0xd90e, 0x83d2, + 0xc915, 0x93c9, 0x7cad, 0x2671, 0xaa74, 0xf0a8, 0x1fcc, 0x4510, + 0x8a42, 0xd09e, 0x3ffa, 0x6526, 0xe923, 0xb3ff, 0x5c9b, 0x0647, + 0x4c80, 0x165c, 0xf938, 0xa3e4, 0x2fe1, 0x753d, 0x9a59, 0xc085, + 0x0a9a, 0x5046, 0xbf22, 0xe5fe, 0x69fb, 0x3327, 0xdc43, 0x869f, + 0xcc58, 0x9684, 0x79e0, 0x233c, 0xaf39, 0xf5e5, 0x1a81, 0x405d, + 0x8f0f, 0xd5d3, 0x3ab7, 0x606b, 0xec6e, 0xb6b2, 0x59d6, 0x030a, + 0x49cd, 0x1311, 0xfc75, 0xa6a9, 0x2aac, 0x7070, 0x9f14, 0xc5c8, + 0x09a1, 0x537d, 0xbc19, 0xe6c5, 0x6ac0, 0x301c, 0xdf78, 0x85a4, + 0xcf63, 0x95bf, 0x7adb, 0x2007, 0xac02, 0xf6de, 0x19ba, 0x4366, + 0x8c34, 0xd6e8, 0x398c, 0x6350, 0xef55, 0xb589, 0x5aed, 0x0031, + 0x4af6, 0x102a, 0xff4e, 0xa592, 0x2997, 0x734b, 0x9c2f, 0xc6f3, + 0x0000, 0x1cbb, 0x3976, 0x25cd, 0x72ec, 0x6e57, 0x4b9a, 0x5721, + 0xe5d8, 0xf963, 0xdcae, 0xc015, 0x9734, 0x8b8f, 0xae42, 0xb2f9, + 0xc3a1, 0xdf1a, 0xfad7, 0xe66c, 0xb14d, 0xadf6, 0x883b, 0x9480, + 0x2679, 0x3ac2, 0x1f0f, 0x03b4, 0x5495, 0x482e, 0x6de3, 0x7158, + 0x8f53, 0x93e8, 0xb625, 0xaa9e, 0xfdbf, 0xe104, 0xc4c9, 0xd872, + 0x6a8b, 0x7630, 0x53fd, 0x4f46, 0x1867, 0x04dc, 0x2111, 0x3daa, + 0x4cf2, 0x5049, 0x7584, 0x693f, 0x3e1e, 0x22a5, 0x0768, 0x1bd3, + 0xa92a, 0xb591, 0x905c, 0x8ce7, 0xdbc6, 0xc77d, 0xe2b0, 0xfe0b, + 0x16b7, 0x0a0c, 0x2fc1, 0x337a, 0x645b, 0x78e0, 0x5d2d, 0x4196, + 0xf36f, 0xefd4, 0xca19, 0xd6a2, 0x8183, 0x9d38, 0xb8f5, 0xa44e, + 0xd516, 0xc9ad, 0xec60, 0xf0db, 0xa7fa, 0xbb41, 0x9e8c, 0x8237, + 0x30ce, 0x2c75, 0x09b8, 0x1503, 0x4222, 0x5e99, 0x7b54, 0x67ef, + 0x99e4, 0x855f, 0xa092, 0xbc29, 0xeb08, 0xf7b3, 0xd27e, 0xcec5, + 0x7c3c, 0x6087, 0x454a, 0x59f1, 0x0ed0, 0x126b, 0x37a6, 0x2b1d, + 0x5a45, 0x46fe, 0x6333, 0x7f88, 0x28a9, 0x3412, 0x11df, 0x0d64, + 0xbf9d, 0xa326, 0x86eb, 0x9a50, 0xcd71, 0xd1ca, 0xf407, 0xe8bc, + 0x2d6e, 0x31d5, 0x1418, 0x08a3, 0x5f82, 0x4339, 0x66f4, 0x7a4f, + 0xc8b6, 0xd40d, 0xf1c0, 0xed7b, 0xba5a, 0xa6e1, 0x832c, 0x9f97, + 0xeecf, 0xf274, 0xd7b9, 0xcb02, 0x9c23, 0x8098, 0xa555, 0xb9ee, + 0x0b17, 0x17ac, 0x3261, 0x2eda, 0x79fb, 0x6540, 0x408d, 0x5c36, + 0xa23d, 0xbe86, 0x9b4b, 0x87f0, 0xd0d1, 0xcc6a, 0xe9a7, 0xf51c, + 0x47e5, 0x5b5e, 0x7e93, 0x6228, 0x3509, 0x29b2, 0x0c7f, 0x10c4, + 0x619c, 0x7d27, 0x58ea, 0x4451, 0x1370, 0x0fcb, 0x2a06, 0x36bd, + 0x8444, 0x98ff, 0xbd32, 0xa189, 0xf6a8, 0xea13, 0xcfde, 0xd365, + 0x3bd9, 0x2762, 0x02af, 0x1e14, 0x4935, 0x558e, 0x7043, 0x6cf8, + 0xde01, 0xc2ba, 0xe777, 0xfbcc, 0xaced, 0xb056, 0x959b, 0x8920, + 0xf878, 0xe4c3, 0xc10e, 0xddb5, 0x8a94, 0x962f, 0xb3e2, 0xaf59, + 0x1da0, 0x011b, 0x24d6, 0x386d, 0x6f4c, 0x73f7, 0x563a, 0x4a81, + 0xb48a, 0xa831, 0x8dfc, 0x9147, 0xc666, 0xdadd, 0xff10, 0xe3ab, + 0x5152, 0x4de9, 0x6824, 0x749f, 0x23be, 0x3f05, 0x1ac8, 0x0673, + 0x772b, 0x6b90, 0x4e5d, 0x52e6, 0x05c7, 0x197c, 0x3cb1, 0x200a, + 0x92f3, 0x8e48, 0xab85, 0xb73e, 0xe01f, 0xfca4, 0xd969, 0xc5d2 +}; + +// crc32 table +const uint32_t crc_table_crc32[256] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, + 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, + 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, + 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, + 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, + 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, + 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, + 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, + 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, + 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, + 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, + 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, + 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, + 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, + 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, + 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, + 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, + 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, + 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, + 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, + 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, + 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, + 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, + 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, + 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, + 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, + 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, + 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, + 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, + 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, + 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, + 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, + 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d +}; + + +#endif + diff --git a/sdk_core/src/third_party/FastCRC/FastCRCsw.cpp b/sdk_core/src/third_party/FastCRC/FastCRCsw.cpp new file mode 100644 index 0000000..02fdd9d --- /dev/null +++ b/sdk_core/src/third_party/FastCRC/FastCRCsw.cpp @@ -0,0 +1,154 @@ +/* FastCRC library code is placed under the MIT license + * Copyright (c) 2014,2015,2016 Frank Bosing + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// +// Thanks to: +// - Catalogue of parametrised CRC algorithms, CRC RevEng +// http://reveng.sourceforge.net/crc-catalogue/ +// +// - Danjel McGougan (CRC-Table-Generator) +// + +// +// modify from FastCRC library @ 2018/11/20 +// + +#include "../include/third_party/FastCRC/FastCRC.h" +#include "FastCRC_tables.hpp" + + +// ================= 16-BIT CRC =================== +/** Constructor + */ +FastCRC16::FastCRC16(uint16_t seed) { + seed_ = seed; +} + +#define crc_n4(crc, data, table) crc ^= data; \ + crc = (table[(crc & 0xff) + 0x300]) ^ \ + (table[((crc >> 8) & 0xff) + 0x200]) ^ \ + (table[((data >> 16) & 0xff) + 0x100]) ^ \ + (table[data >> 24]); + +/** MCRF4XX + * equivalent to _crc_ccitt_update() in crc16.h from avr_libc + * @param data Pointer to Data + * @param datalen Length of Data + * @return CRC value + */ + +uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) { + + uint16_t crc = seed_; + + while (((uintptr_t)data & 3) && len) { + crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++]; + len--; + } + + while (len >= 16) { + len -= 16; + crc_n4(crc, ((uint32_t *)data)[0], crc_table_mcrf4xx); + crc_n4(crc, ((uint32_t *)data)[1], crc_table_mcrf4xx); + crc_n4(crc, ((uint32_t *)data)[2], crc_table_mcrf4xx); + crc_n4(crc, ((uint32_t *)data)[3], crc_table_mcrf4xx); + data += 16; + } + + while (len--) { + crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++]; + } + + //seed = crc; + return crc; +} + + +// ================= 32-BIT CRC =================== +/** Constructor + */ +FastCRC32::FastCRC32(uint32_t seed) { + seed_ = seed; +} + +#define crc_n4d(crc, data, table) crc ^= data; \ + crc = (table[(crc & 0xff) + 0x300]) ^ \ + (table[((crc >> 8) & 0xff) + 0x200]) ^ \ + (table[((crc >> 16) & 0xff) + 0x100]) ^ \ + (table[(crc >> 24) & 0xff]); + +#define crcsm_n4d(crc, data, table) crc ^= data; \ + crc = (crc >> 8) ^ (table[crc & 0xff]); \ + crc = (crc >> 8) ^ (table[crc & 0xff]); \ + crc = (crc >> 8) ^ (table[crc & 0xff]); \ + crc = (crc >> 8) ^ (table[crc & 0xff]); + +/** CRC32 + * Alias CRC-32/ADCCP, PKZIP, Ethernet, 802.3 + * @param data Pointer to Data + * @param datalen Length of Data + * @return CRC value + */ +#if CRC_BIGTABLES +#define CRC_TABLE_CRC32 crc_table_crc32_big +#else +#define CRC_TABLE_CRC32 crc_table_crc32 +#endif + +uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) { + + uint32_t crc = seed_^0xffffffff; + + while (((uintptr_t)data & 3) && len) { + crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++]; + len--; + } + + while (len >= 16) { + len -= 16; + #if CRC_BIGTABLES + crc_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32); + crc_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32); + crc_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32); + crc_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32); + #else + crcsm_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32); + crcsm_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32); + crcsm_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32); + crcsm_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32); + #endif + data += 16; + } + + while (len--) { + crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++]; + } + + //seed = crc; + crc ^= 0xffffffff; + + return crc; +} + + diff --git a/sdk_core/src/third_party/FastCRC/LICENSE.md b/sdk_core/src/third_party/FastCRC/LICENSE.md new file mode 100644 index 0000000..b7c7a98 --- /dev/null +++ b/sdk_core/src/third_party/FastCRC/LICENSE.md @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2016 Frank + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/sdk_core/src/third_party/FastCRC/README.md b/sdk_core/src/third_party/FastCRC/README.md new file mode 100644 index 0000000..97ae86c --- /dev/null +++ b/sdk_core/src/third_party/FastCRC/README.md @@ -0,0 +1,56 @@ +FastCRC +======= + +Fast CRC Arduino library +Up to 30 times faster than crc16.h (_avr_libc) + + - uses the on-chip hardware for Teensy 3.0 / 3.1 / 3.2 / 3.5 / 3.6 + - uses fast table-algorithms for other chips + +List of supported CRC calculations: +- +7 BIT: + +CRC7 + (poly=0x09 init=0x00 refin=false refout=false xorout=0x00 check=0x75) + MultiMediaCard interface + + +8 BIT: + +SMBUS + (poly=0x07 init=0x00 refin=false refout=false xorout=0x00 check=0xf4) + +MAXIM + (poly=0x31 init=0x00 refin=true refout=true xorout=0x00 check=0xa1) + + +16 BIT: + +KERMIT (Alias CRC-16/CCITT, CRC-16/CCITT-TRUE, CRC-CCITT) + (poly=0x1021 init=0x0000 refin=true refout=true xorout=0x0000 check=0x2189 + Attention: sometimes you'll find byteswapped presentation of result in other implementations) + +CCITT-FALSE + (poly=0x1021 init=0xffff refin=false refout=false xorout=0x0000 check=0x29b1) + +MCRF4XX + (poly=0x1021 init=0xffff refin=true refout=true xorout=0x0000 check=0x6f91) + +MODBUS + (poly=0x8005 init=0xffff refin=true refout=true xorout=0x0000 check=0x4b37) + +XMODEM (Alias ZMODEM, CRC-16/ACORN) + (poly=0x1021 init=0x0000 refin=false refout=false xorout=0x0000 check=0x31c3) + +X25 (Alias CRC-16/IBM-SDLC, CRC-16/ISO-HDLC, CRC-B) + (poly=0x1021 init=0xffff refin=true refout=true xorout=0xffff check=0x906e) + + +32 BIT: + +CRC32, CRC-32/ADCCP, PKZIP, ETHERNET, 802.3 + (poly=0x04c11db7 init=0xffffffff refin=true refout=true xorout=0xffffffff check=0xcbf43926) + +CKSUM, CRC-32/POSIX + (poly=0x04c11db7 init=0x00000000 refin=false refout=false xorout=0xffffffff check=0x765e7680)