From dcd2dd87ce8034a46e4150a89dc69f6ba1a22d8d Mon Sep 17 00:00:00 2001 From: Edvin Date: Thu, 10 Apr 2025 11:35:25 +0200 Subject: Implemented code for Slamtec rplidar C1 --- .../sdk/src/dataunpacker/dataunnpacker_commondef.h | 60 ++ .../sdk/src/dataunpacker/dataunnpacker_internal.h | 74 ++ LIDAR/sdk/src/dataunpacker/dataunpacker.cpp | 259 +++++ LIDAR/sdk/src/dataunpacker/dataunpacker.h | 93 ++ LIDAR/sdk/src/dataunpacker/dataupacker_namespace.h | 5 + .../src/dataunpacker/unpacker/handler_capsules.cpp | 1054 ++++++++++++++++++++ .../src/dataunpacker/unpacker/handler_capsules.h | 149 +++ .../src/dataunpacker/unpacker/handler_hqnode.cpp | 192 ++++ .../sdk/src/dataunpacker/unpacker/handler_hqnode.h | 63 ++ .../dataunpacker/unpacker/handler_normalnode.cpp | 159 +++ .../src/dataunpacker/unpacker/handler_normalnode.h | 63 ++ 11 files changed, 2171 insertions(+) create mode 100644 LIDAR/sdk/src/dataunpacker/dataunnpacker_commondef.h create mode 100644 LIDAR/sdk/src/dataunpacker/dataunnpacker_internal.h create mode 100644 LIDAR/sdk/src/dataunpacker/dataunpacker.cpp create mode 100644 LIDAR/sdk/src/dataunpacker/dataunpacker.h create mode 100644 LIDAR/sdk/src/dataunpacker/dataupacker_namespace.h create mode 100644 LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.cpp create mode 100644 LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.h create mode 100644 LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp create mode 100644 LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.h create mode 100644 LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp create mode 100644 LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.h (limited to 'LIDAR/sdk/src/dataunpacker') diff --git a/LIDAR/sdk/src/dataunpacker/dataunnpacker_commondef.h b/LIDAR/sdk/src/dataunpacker/dataunnpacker_commondef.h new file mode 100644 index 0000000..70b9c99 --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/dataunnpacker_commondef.h @@ -0,0 +1,60 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * External Reference and dependencies + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "hal/waiter.h" +#include "hal/byteorder.h" +#include "sl_lidar_driver.h" +#include "sl_crc.h" +#include +#include + +#define CONF_NO_BOOST_CRC_SUPPORT + +#include "dataupacker_namespace.h" + + diff --git a/LIDAR/sdk/src/dataunpacker/dataunnpacker_internal.h b/LIDAR/sdk/src/dataunpacker/dataunnpacker_internal.h new file mode 100644 index 0000000..6b1b6dd --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/dataunnpacker_internal.h @@ -0,0 +1,74 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Internal Definition + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +BEGIN_DATAUNPACKER_NS() + + +class LIDARSampleDataUnpackerInner: public LIDARSampleDataUnpacker +{ +public: + LIDARSampleDataUnpackerInner(LIDARSampleDataListener& l): LIDARSampleDataUnpacker(l){} + + virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0; + virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size) = 0; + virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size) = 0; + virtual void publishNewScanReset() = 0; + + + virtual _u64 getCurrentTimestamp_uS() = 0; + +}; + +class IDataUnpackerHandler +{ +public: + IDataUnpackerHandler() {} + virtual ~IDataUnpackerHandler() {} + + + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) = 0; + + virtual _u8 getSampleAnswerType() const = 0; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size) = 0; + virtual void reset() = 0; + +}; + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/dataunpacker.cpp b/LIDAR/sdk/src/dataunpacker/dataunpacker.cpp new file mode 100644 index 0000000..74680c0 --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/dataunpacker.cpp @@ -0,0 +1,259 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "dataunnpacker_commondef.h" +#include "dataunpacker.h" +#include "dataunnpacker_internal.h" + + +#include + + +#define REGISTER_HANDLER(_c_) { \ + auto newBorn = new unpacker::_c_(); \ + if (!newBorn) return false; \ + handlerList.push_back(newBorn); \ + } + +// How to include new handlers? +// 1. add extra include line below if a new handle is to be included +// 2. update the code in function _registerDataUnpackerHandlers +#include "unpacker/handler_capsules.h" +#include "unpacker/handler_hqnode.h" +#include "unpacker/handler_normalnode.h" + + +#define DEF_REGISTER_HANDLER_LIST + + +BEGIN_DATAUNPACKER_NS() + + +static bool _registerDataUnpackerHandlers(std::vector & handlerList) +{ + REGISTER_HANDLER(UnpackerHandler_NormalNode); + REGISTER_HANDLER(UnpackerHandler_HQNode); + REGISTER_HANDLER(UnpackerHandler_CapsuleNode); + REGISTER_HANDLER(UnpackerHandler_UltraCapsuleNode); + REGISTER_HANDLER(UnpackerHandler_DenseCapsuleNode); + REGISTER_HANDLER(UnpackerHandler_UltraDenseCapsuleNode); + return true; +} + + +class LIDARSampleDataUnpackerImpl : public LIDARSampleDataUnpackerInner +{ +public: + + void registerHandler(_u8 ansType, IDataUnpackerHandler* handler) + { + _handlerMap[ansType] = handler; + } + + + void unregisterAllHandlers() + { + for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr) + { + delete itr->second; + } + _handlerMap.clear(); + } + + LIDARSampleDataUnpackerImpl(LIDARSampleDataListener& l) + : LIDARSampleDataUnpackerInner(l) + , _enabled(false) + , _lastActiveAnsType(0) + , _lastActiveHandler(nullptr) + { + + } + + virtual ~LIDARSampleDataUnpackerImpl() + { + unregisterAllHandlers(); + } + + + virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size) + { + + // notify the handlers ... + for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr) + { + itr->second->onUnpackerContextSet(type, data, size); + } + } + + virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) { + if (!_enabled) return false; + + + if (_lastActiveAnsType != ansType) { + onDeselectHandler(); + + auto itr = _handlerMap.find(ansType); + if (itr != _handlerMap.end()) { + onSelectHandler(ansType, itr->second); + } + else { + onSelectHandler(ansType, nullptr); + } + + } + + if (_lastActiveHandler) { + _lastActiveHandler->onData(this, reinterpret_cast(buffer), size); + return true; + } + else { + return false; + } + } + + virtual void reset() + { + clearCache(); + _lastActiveHandler = nullptr; + _lastActiveAnsType = 0; + + } + + virtual void enable() + { + _enabled = true; + reset(); + } + + virtual void disable() + { + _enabled = false; + reset(); + + } + + virtual void clearCache() + { + if (_lastActiveHandler) { + _lastActiveHandler->reset(); + } + } + + virtual _u64 getCurrentTimestamp_uS() { + return getus(); + } + + virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) + { + _listener.onHQNodeDecoded(timestamp_uS, node); + } + + + virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size) + { + _listener.onDecodingError(errorType, ansType, payload, size); + + } + + virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size) + { + _listener.onCustomSampleDataDecoded(ansType, customCode, payload, size); + } + + + virtual void publishNewScanReset() + { + _listener.onHQNodeScanResetReq(); + } +protected: + + void onSelectHandler(_u8 ansType, IDataUnpackerHandler* handler) + { + _lastActiveHandler = handler; + _lastActiveAnsType = ansType; + } + + void onDeselectHandler() + { + reset(); + } + + +protected: + bool _enabled; + std::map<_u8, IDataUnpackerHandler*> _handlerMap; + + _u8 _lastActiveAnsType; + IDataUnpackerHandler* _lastActiveHandler; +}; + +LIDARSampleDataUnpacker* LIDARSampleDataUnpacker::CreateInstance(LIDARSampleDataListener& listener) +{ + LIDARSampleDataUnpackerImpl* impl = new LIDARSampleDataUnpackerImpl(listener); + + std::vector list; + if (!_registerDataUnpackerHandlers(list)) { + delete impl; + for (auto itr = list.begin(); itr != list.end(); ++itr) { + delete* itr; + } + impl = nullptr; + } + + for (auto itr = list.begin(); itr != list.end(); ++itr) { + impl->registerHandler((*itr)->getSampleAnswerType(), (*itr)); + } + return impl; +} + +void LIDARSampleDataUnpacker::ReleaseInstance(LIDARSampleDataUnpacker* unpacker) { + delete unpacker; +} + +LIDARSampleDataUnpacker::~LIDARSampleDataUnpacker() { + +} + +LIDARSampleDataUnpacker::LIDARSampleDataUnpacker(LIDARSampleDataListener& l) + : _listener(l) +{ + +} + + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/dataunpacker.h b/LIDAR/sdk/src/dataunpacker/dataunpacker.h new file mode 100644 index 0000000..2cdf6ca --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/dataunpacker.h @@ -0,0 +1,93 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + + +#pragma once + +#include "dataupacker_namespace.h" + +BEGIN_DATAUNPACKER_NS() + + +class LIDARSampleDataListener +{ + + +public: + virtual void onHQNodeScanResetReq() = 0; + virtual void onHQNodeDecoded(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0; + virtual void onCustomSampleDataDecoded(_u8 ansType, _u32 customCode, const void* data, size_t size) {} + + virtual void onDecodingError(int errMsg, _u8 ansType, const void* payload, size_t size) {} +}; + +class LIDARSampleDataUnpacker +{ +public: + enum { + ERR_EVENT_ON_EXP_ENCODER_RESET = 0x8001, + ERR_EVENT_ON_EXP_CHECKSUM_ERR = 0x8002, + }; + + enum UnpackerContextType { + UNPACKER_CONTEXT_TYPE_LIDAR_UNKNOWN = 0, + UNPACKER_CONTEXT_TYPE_LIDAR_TIMING = 1, + UNPACKER_CONTEXT_TYPE_TRIANGULATION_OPTICAL_FACTOR = 2, + + }; + + virtual ~LIDARSampleDataUnpacker(); + static LIDARSampleDataUnpacker* CreateInstance(LIDARSampleDataListener& listener); + static void ReleaseInstance(LIDARSampleDataUnpacker*); + + virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size) = 0; + + virtual void enable() = 0; + virtual void disable() = 0; + + virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) = 0; + virtual void reset() = 0; + virtual void clearCache() = 0; + +protected: + LIDARSampleDataUnpacker(LIDARSampleDataListener&); + LIDARSampleDataListener& _listener; + +}; + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/dataupacker_namespace.h b/LIDAR/sdk/src/dataunpacker/dataupacker_namespace.h new file mode 100644 index 0000000..9e27192 --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/dataupacker_namespace.h @@ -0,0 +1,5 @@ +#pragma once + + +#define BEGIN_DATAUNPACKER_NS() namespace sl{ namespace internal{ +#define END_DATAUNPACKER_NS() }} \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.cpp b/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.cpp new file mode 100644 index 0000000..177c214 --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.cpp @@ -0,0 +1,1054 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Capsule Style Sample Node Handlers + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "../dataunnpacker_commondef.h" +#include "../dataunpacker.h" +#include "../dataunnpacker_internal.h" + + + +#include "handler_capsules.h" + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker{ + + +// UnpackerHandler_CapsuleNode +/////////////////////////////////////////////////////////////////////////////////// + +static _u64 _getSampleDelayOffsetInExpressMode(const SlamtecLidarTimingDesc& timing, int sampleIdx) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_capsule_measurement_nodes_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + const _u64 groupingDelay = (31 - sampleIdx) * timing.sample_duration_uS; + + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay; +} + + +UnpackerHandler_CapsuleNode::UnpackerHandler_CapsuleNode() + : _cached_scan_node_buf_pos(0) + , _is_previous_capsuledataRdy(false) + , _cached_last_data_timestamp_us(0) +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_CapsuleNode::~UnpackerHandler_CapsuleNode() +{ + +} + +void UnpackerHandler_CapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + + +_u8 UnpackerHandler_CapsuleNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; +} + +void UnpackerHandler_CapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + + case sizeof(rplidar_response_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_capsule_measurement_nodes_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); + + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4)); + for (size_t cpos = offsetof(rplidar_response_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= _cached_scan_node_buf[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + + // perform data endianess convertion if necessary +#ifdef _CPU_ENDIAN_BIG + node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6); + for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) { + node->cabins[cpos].distance_angle_1 = le16_to_cpu(node->cabins[cpos].distance_angle_1); + node->cabins[cpos].distance_angle_2 = le16_to_cpu(node->cabins[cpos].distance_angle_2); + } +#endif + if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + if (_is_previous_capsuledataRdy) { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET + , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED, node, sizeof(*node)); + } + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + engine->publishNewScanReset(); + + + } + _onScanNodeCapsuleData(*node, engine); + } + else { + _is_previous_capsuledataRdy = false; + + + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED, node, sizeof(*node)); + + } + continue; + } + break; + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } + +} + +void UnpackerHandler_CapsuleNode::reset() +{ + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + _cached_last_data_timestamp_us = 0; +} + +void UnpackerHandler_CapsuleNode::_onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine) +{ + _u64 currentTS = engine->getCurrentTimestamp_uS(); + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3); + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (int pos = 0; pos < (int)_countof(_cached_previous_capsuledata.cabins); ++pos) + { + int dist_q2[2]; + int angle_q6[2]; + int syncBit[2]; + + dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC); + dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC); + + int angle_offset1_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3) << 4)); + int angle_offset2_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3) << 4)); + + angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10); + syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + currentAngle_raw_q16 += angleInc_q16; + + + angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10); + syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + currentAngle_raw_q16 += angleInc_q16; + + for (int cpos = 0; cpos < 2; ++cpos) { + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + rplidar_response_measurement_node_hq_t hqNode; + + + hqNode.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + hqNode.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + + hqNode.angle_z_q14 = (angle_q6[cpos] << 8) / 90; + hqNode.dist_mm_q2 = dist_q2[cpos]; + + engine->publishHQNode(_cached_last_data_timestamp_us - _getSampleDelayOffsetInExpressMode(_cachedTimingDesc, pos * 2 + cpos), &hqNode); + } + + } + } + + _cached_previous_capsuledata = capsule; + _is_previous_capsuledataRdy = true; + _cached_last_data_timestamp_us = currentTS; + +} + + +// UnpackerHandler_UltraCapsuleNode +/////////////////////////////////////////////////////////////////////////////////// + +static _u64 _getSampleDelayOffsetInUltraBoostMode(const SlamtecLidarTimingDesc& timing, int sampleIdx) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 256000; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + const _u64 groupingDelay = ((32 * 3 - 1) - sampleIdx) * timing.sample_duration_uS; + + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay; +} + + +UnpackerHandler_UltraCapsuleNode::UnpackerHandler_UltraCapsuleNode() + : _cached_scan_node_buf_pos(0) + , _is_previous_capsuledataRdy(false) + , _cached_last_data_timestamp_us(0) +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_UltraCapsuleNode::~UnpackerHandler_UltraCapsuleNode() +{ + +} + +void UnpackerHandler_UltraCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + + +_u8 UnpackerHandler_UltraCapsuleNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA; +} + +void UnpackerHandler_UltraCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + + case sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_ultra_capsule_measurement_nodes_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); + + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4)); + for (size_t cpos = offsetof(rplidar_response_ultra_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= _cached_scan_node_buf[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + + // perform data endianess convertion if necessary +#ifdef _CPU_ENDIAN_BIG + node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6); + for (size_t cpos = 0; cpos < _countof(node->ultra_cabins); ++cpos) { + node->ultra_cabins[cpos].combined_x3 = le32_to_cpu(node->ultra_cabins[cpos].combined_x3); + } +#endif + if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + if (_is_previous_capsuledataRdy) { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET + , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA, node, sizeof(*node)); + + } + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + + engine->publishNewScanReset(); + + } + _onScanNodeUltraCapsuleData(*node, engine); + } + else { + _is_previous_capsuledataRdy = false; + + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA, node, sizeof(*node)); + + } + continue; + } + break; + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } + +} + +void UnpackerHandler_UltraCapsuleNode::reset() +{ + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; +} + +static _u32 _varbitscale_decode(_u32 scaled, _u32& scaleLevel) +{ + static const _u32 VBS_SCALED_BASE[] = { + RPLIDAR_VARBITSCALE_X16_DEST_VAL, + RPLIDAR_VARBITSCALE_X8_DEST_VAL, + RPLIDAR_VARBITSCALE_X4_DEST_VAL, + RPLIDAR_VARBITSCALE_X2_DEST_VAL, + 0, + }; + + static const _u32 VBS_SCALED_LVL[] = { + 4, + 3, + 2, + 1, + 0, + }; + + static const _u32 VBS_TARGET_BASE[] = { + (0x1 << RPLIDAR_VARBITSCALE_X16_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X8_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X4_SRC_BIT), + (0x1 << RPLIDAR_VARBITSCALE_X2_SRC_BIT), + 0, + }; + + for (size_t i = 0; i < _countof(VBS_SCALED_BASE); ++i) + { + int remain = ((int)scaled - (int)VBS_SCALED_BASE[i]); + if (remain >= 0) { + scaleLevel = VBS_SCALED_LVL[i]; + return VBS_TARGET_BASE[i] + (remain << scaleLevel); + } + } + + return 0; +} + +void UnpackerHandler_UltraCapsuleNode::_onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine) +{ + _u64 currentTS = engine->getCurrentTimestamp_uS(); + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_ultracapsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3) / 3; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (int pos = 0; pos < (int)_countof(_cached_previous_ultracapsuledata.ultra_cabins); ++pos) + { + int dist_q2[3]; + int angle_q6[3]; + int syncBit[3]; + + + _u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3; + + // unpack ... + int dist_major = (combined_x3 & 0xFFF); + + // signed partical integer, using the magic shift here + // DO NOT TOUCH + + int dist_predict1 = (((int)(combined_x3 << 10)) >> 22); + int dist_predict2 = (((int)combined_x3) >> 22); + + int dist_major2; + + _u32 scalelvl1=0, scalelvl2 = 0; + + // prefetch next ... + if (pos == _countof(_cached_previous_ultracapsuledata.ultra_cabins) - 1) + { + dist_major2 = (capsule.ultra_cabins[0].combined_x3 & 0xFFF); + } + else { + dist_major2 = (_cached_previous_ultracapsuledata.ultra_cabins[pos + 1].combined_x3 & 0xFFF); + } + + // decode with the var bit scale ... + dist_major = _varbitscale_decode(dist_major, scalelvl1); + dist_major2 = _varbitscale_decode(dist_major2, scalelvl2); + + + int dist_base1 = dist_major; + int dist_base2 = dist_major2; + + if ((!dist_major) && dist_major2) { + dist_base1 = dist_major2; + scalelvl1 = scalelvl2; + } + + + dist_q2[0] = (dist_major << 2); + if (((_u32)dist_predict1 == 0xFFFFFE00) || ((_u32)dist_predict1 == 0x1FF)) { + dist_q2[1] = 0; + } + else { + dist_predict1 = (int)(dist_predict1 << scalelvl1); + dist_q2[1] = (dist_predict1 + dist_base1) << 2; + + } + + if (((_u32)dist_predict2 == 0xFFFFFE00) || ((_u32)dist_predict2 == 0x1FF)) { + dist_q2[2] = 0; + } + else { + dist_predict2 = (int)(dist_predict2 << scalelvl2); + dist_q2[2] = (dist_predict2 + dist_base2) << 2; + } + + for (int cpos = 0; cpos < 3; ++cpos) + { + + syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + + + rplidar_response_measurement_node_hq_t hqNode; + + + int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0); + + if (dist_q2[cpos] >= (50 * 4)) + { + const int k1 = 98361; + const int k2 = int(k1 / dist_q2[cpos]); + + offsetAngleMean_q16 = (int)(8 * 3.1415926535 * (1 << 16) / 180) - (k2 << 6) - (k2 * k2 * k2) / 98304; + } + + angle_q6[cpos] = ((currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10); + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + + hqNode.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + hqNode.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + + hqNode.angle_z_q14 = (angle_q6[cpos] << 8) / 90; + hqNode.dist_mm_q2 = dist_q2[cpos]; + + engine->publishHQNode(_cached_last_data_timestamp_us - _getSampleDelayOffsetInUltraBoostMode(_cachedTimingDesc, pos * 3 + cpos), &hqNode); + } + + } + } + + _cached_previous_ultracapsuledata = capsule; + _is_previous_capsuledataRdy = true; + _cached_last_data_timestamp_us = currentTS; + +} + + +// UnpackerHandler_DenseCapsuleNode +/////////////////////////////////////////////////////////////////////////////////// + +static _u64 _getSampleDelayOffsetInDenseMode(const SlamtecLidarTimingDesc& timing, int sampleIdx) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 256000; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_dense_capsule_measurement_nodes_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + const _u64 groupingDelay = (39 - sampleIdx) * timing.sample_duration_uS; + + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay; +} + +UnpackerHandler_DenseCapsuleNode::UnpackerHandler_DenseCapsuleNode() + : _cached_scan_node_buf_pos(0) + , _is_previous_capsuledataRdy(false) + , _cached_last_data_timestamp_us(0) + +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_dense_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_DenseCapsuleNode::~UnpackerHandler_DenseCapsuleNode() +{ + +} + +void UnpackerHandler_DenseCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + + +_u8 UnpackerHandler_DenseCapsuleNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED; +} + + +void UnpackerHandler_DenseCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + + case sizeof(rplidar_response_dense_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_dense_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_dense_capsule_measurement_nodes_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); + + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4)); + for (size_t cpos = offsetof(rplidar_response_dense_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(rplidar_response_dense_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= _cached_scan_node_buf[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + + // perform data endianess convertion if necessary +#ifdef _CPU_ENDIAN_BIG + node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6); + for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) { + node->cabins[cpos].distance_angle_1 = le16_to_cpu(node->cabins[cpos].distance_angle_1); + node->cabins[cpos].distance_angle_2 = le16_to_cpu(node->cabins[cpos].distance_angle_2); + } +#endif + if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + if (_is_previous_capsuledataRdy) { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET + , RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED, node, sizeof(*node)); + } + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + engine->publishNewScanReset(); + + + } + _onScanNodeDenseCapsuleData(*node, engine); + } + else { + _is_previous_capsuledataRdy = false; + + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED, node, sizeof(*node)); + + } + continue; + } + break; + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } +} + +void UnpackerHandler_DenseCapsuleNode::reset() +{ + _cached_scan_node_buf_pos = 0; + _cached_last_data_timestamp_us = 0; +} + +void UnpackerHandler_DenseCapsuleNode::_onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t& dense_capsule, LIDARSampleDataUnpackerInner* engine) +{ + static int lastNodeSyncBit = 0; + _u64 currentTs = engine->getCurrentTimestamp_uS(); + + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((dense_capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + int maxDiffAngleThreshold_q8 = (360/* 360 degree */ * 100 /*100Hz*/ * _countof(dense_capsule.cabins) /*40 points per capsule*/ / (1000000 / _cachedTimingDesc.sample_duration_uS)) << 8; + if (diffAngle_q8 > maxDiffAngleThreshold_q8) {//discard + _cached_previous_dense_capsuledata = dense_capsule; + return; + } + + int angleInc_q16 = (diffAngle_q8 << 8) / 40; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (int pos = 0; pos < (int)_countof(_cached_previous_dense_capsuledata.cabins); ++pos) + { + int dist_q2; + int angle_q6; + int syncBit; + const int dist = static_cast(_cached_previous_dense_capsuledata.cabins[pos].distance); + dist_q2 = dist << 2; + angle_q6 = (currentAngle_raw_q16 >> 10); + syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < (angleInc_q16 << 1)) ? 1 : 0; + syncBit = (syncBit ^ lastNodeSyncBit) & syncBit;//Ensure that syncBit is exactly detected + + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6 < 0) angle_q6 += (360 << 6); + if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6); + + rplidar_response_measurement_node_hq_t hqNode; + + + hqNode.flag = (syncBit | ((!syncBit) << 1)); + hqNode.quality = dist_q2 ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + hqNode.angle_z_q14 = (angle_q6 << 8) / 90; + hqNode.dist_mm_q2 = dist_q2; + engine->publishHQNode(currentTs - _getSampleDelayOffsetInDenseMode(_cachedTimingDesc, pos), &hqNode); + + lastNodeSyncBit = syncBit; + + } + } + + _cached_previous_dense_capsuledata = dense_capsule; + _is_previous_capsuledataRdy = true; + +} + +// UnpackerHandler_UltraDenseCapsuleNode +/////////////////////////////////////////////////////////////////////////////////// + +static _u64 _getSampleDelayOffsetInUltraDenseMode(const SlamtecLidarTimingDesc& timing, int sampleIdx) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate ? timing.native_baudrate : 1000000; + + _u64 tranmissionDelay = 1000000ULL * sizeof(sl_lidar_response_ultra_dense_capsule_measurement_nodes_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + const _u64 groupingDelay = ((32 * 2 - 1) - sampleIdx) * timing.sample_duration_uS; + + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS + groupingDelay; +} + + +UnpackerHandler_UltraDenseCapsuleNode::UnpackerHandler_UltraDenseCapsuleNode() + : _cached_scan_node_buf_pos(0) + , _is_previous_capsuledataRdy(false) + , _cached_last_data_timestamp_us(0) + , _last_node_sync_bit(0) + , _last_dist_q2(0) + +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_UltraDenseCapsuleNode::~UnpackerHandler_UltraDenseCapsuleNode() +{ + +} + + +void UnpackerHandler_UltraDenseCapsuleNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + + +_u8 UnpackerHandler_UltraDenseCapsuleNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED; +} + +void UnpackerHandler_UltraDenseCapsuleNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit 1 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + _u8 tmp = (current_data >> 4); + if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + + case sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_ultra_dense_capsule_measurement_nodes_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); + + // calc the checksum ... + _u8 checksum = 0; + _u8 recvChecksum = ((node->s_checksum_1 & 0xF) | (node->s_checksum_2 << 4)); + for (size_t cpos = offsetof(rplidar_response_ultra_dense_capsule_measurement_nodes_t, time_stamp); + cpos < sizeof(rplidar_response_ultra_dense_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= _cached_scan_node_buf[cpos]; + } + + if (recvChecksum == checksum) + { + // only consider vaild if the checksum matches... + + // perform data endianess convertion if necessary +#ifdef _CPU_ENDIAN_BIG + node->start_angle_sync_q6 = le16_to_cpu(node->start_angle_sync_q6); + for (size_t cpos = 0; cpos < _countof(node->cabins); ++cpos) { + node->cabins[cpos].qualityl_distance_scale[0] = le16_to_cpu(node->cabins[cpos].qualityl_distance_scale[0]); + node->cabins[cpos].qualityl_distance_scale[1] = le16_to_cpu(node->cabins[cpos].qualityl_distance_scale[1]); + } +#endif + if (node->start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) + { + if (_is_previous_capsuledataRdy) { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_ENCODER_RESET + , RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED, node, sizeof(*node)); + + } + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + engine->publishNewScanReset(); + + } + _onScanNodeUltraDenseCapsuleData(*node, engine); + } + else { + _is_previous_capsuledataRdy = false; + + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED, node, sizeof(*node)); + + } + continue; + } + break; + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } + +} + +void UnpackerHandler_UltraDenseCapsuleNode::reset() +{ + _cached_scan_node_buf_pos = 0; + _cached_last_data_timestamp_us = 0; + _last_node_sync_bit = 0; + _last_dist_q2 = 0; +} + +void UnpackerHandler_UltraDenseCapsuleNode::_onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t& capsule, LIDARSampleDataUnpackerInner* engine) +{ + _u64 currentTimestamp = engine->getCurrentTimestamp_uS(); + + const rplidar_response_ultra_dense_capsule_measurement_nodes_t* ultra_dense_capsule = reinterpret_cast(&capsule); + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((ultra_dense_capsule->start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_ultra_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int maxDiffAngleThreshold_q8 = (360/* 360 degree */ * 100 /*100Hz*/ * _countof(ultra_dense_capsule->cabins) /*64 points per capsule*/ / (1000000 / _cachedTimingDesc.sample_duration_uS)) << 8; + if (diffAngle_q8 > maxDiffAngleThreshold_q8) {//discard + _cached_previous_ultra_dense_capsuledata = *ultra_dense_capsule; + return; + } +#define DISTANCE_THRESHOLD_TO_SCALE_1 2046 // (2^10 - 1)*2 mm +#define DISTANCE_THRESHOLD_TO_SCALE_2 8187 // (2^11 - 1)*3 + 2046 mm +#define DISTANCE_THRESHOLD_TO_SCALE_3 24567 // (2^12 - 1)*4 + 8187 mm + int angleInc_q16 = (diffAngle_q8 << 8) / 64; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (int pos = 0; pos < (int)_countof(_cached_previous_ultra_dense_capsuledata.cabins) * 2; ++pos) + { + int angle_q6; + int syncBit; + size_t cabin_idx = pos >> 1; + _u32 quality_dist_scale; + if (!(pos & 0x1)) { + quality_dist_scale = _cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityl_distance_scale[0] | ((_cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityh_array & 0x0F) << 16); + } + else { + quality_dist_scale = _cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityl_distance_scale[1] | ((_cached_previous_ultra_dense_capsuledata.cabins[cabin_idx].qualityh_array >> 4) << 16); + } + + _u8 scale = quality_dist_scale & 0x3; + _u8 quality = 0; + int dist_q2 = 0; + + switch (scale) { + case 0: + quality = quality_dist_scale >> 12; + dist_q2 = (quality_dist_scale & 0xFFC) * 2; + if (_last_dist_q2) { + if (abs(dist_q2 - _last_dist_q2) <= 8/*2mm *2*/) { + dist_q2 = (dist_q2 + _last_dist_q2) >> 1; + } + } + break; + case 1: + quality = (quality_dist_scale >> 13) << 1; + dist_q2 = (quality_dist_scale & 0x1FFC) * 3 + (DISTANCE_THRESHOLD_TO_SCALE_1 << 2); + break; + case 2: + quality = (quality_dist_scale >> 14) << 2; + dist_q2 = (quality_dist_scale & 0x3FFC) * 4 + (DISTANCE_THRESHOLD_TO_SCALE_2 << 2); + break; + case 3: + quality = (quality_dist_scale >> 15) << 3; + dist_q2 = (quality_dist_scale & 0x7FFC) * 5 + (DISTANCE_THRESHOLD_TO_SCALE_3 << 2); + break; + } + _last_dist_q2 = dist_q2; + angle_q6 = (currentAngle_raw_q16 >> 10); + syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < (angleInc_q16 << 1)) ? 1 : 0; + syncBit = (syncBit ^ _last_node_sync_bit) & syncBit;//Ensure that syncBit is exactly detected + + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6 < 0) angle_q6 += (360 << 6); + if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6); + + + rplidar_response_measurement_node_hq_t hqNode; + + + + hqNode.flag = (syncBit | ((!syncBit) << 1)); + hqNode.quality = quality; + hqNode.angle_z_q14 = (angle_q6 << 8) / 90; + hqNode.dist_mm_q2 = dist_q2; + engine->publishHQNode(currentTimestamp - _getSampleDelayOffsetInUltraDenseMode(_cachedTimingDesc, pos), &hqNode); + + _last_node_sync_bit = syncBit; + + } + } + + _cached_previous_ultra_dense_capsuledata = *ultra_dense_capsule; + _is_previous_capsuledataRdy = true; + +} + + + +} + + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.h b/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.h new file mode 100644 index 0000000..008f544 --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_capsules.h @@ -0,0 +1,149 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Capsule Style Sample Node Handlers + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker { + +class UnpackerHandler_CapsuleNode : public IDataUnpackerHandler { +public: + UnpackerHandler_CapsuleNode(); + virtual ~UnpackerHandler_CapsuleNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + + void _onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t &, LIDARSampleDataUnpackerInner* engine); + + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + bool _is_previous_capsuledataRdy; + + rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + _u64 _cached_last_data_timestamp_us; + + SlamtecLidarTimingDesc _cachedTimingDesc; +}; + +class UnpackerHandler_UltraCapsuleNode : public IDataUnpackerHandler { +public: + UnpackerHandler_UltraCapsuleNode(); + virtual ~UnpackerHandler_UltraCapsuleNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + void _onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine); + + + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + bool _is_previous_capsuledataRdy; + + rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; + _u64 _cached_last_data_timestamp_us; + + SlamtecLidarTimingDesc _cachedTimingDesc; + +}; + + + +class UnpackerHandler_DenseCapsuleNode : public IDataUnpackerHandler { +public: + UnpackerHandler_DenseCapsuleNode(); + virtual ~UnpackerHandler_DenseCapsuleNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + void _onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine); + + + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + bool _is_previous_capsuledataRdy; + + rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; + _u64 _cached_last_data_timestamp_us; + + SlamtecLidarTimingDesc _cachedTimingDesc; + +}; + + +class UnpackerHandler_UltraDenseCapsuleNode : public IDataUnpackerHandler { +public: + UnpackerHandler_UltraDenseCapsuleNode(); + virtual ~UnpackerHandler_UltraDenseCapsuleNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + void _onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine); + + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + bool _is_previous_capsuledataRdy; + + rplidar_response_ultra_dense_capsule_measurement_nodes_t _cached_previous_ultra_dense_capsuledata; + _u64 _cached_last_data_timestamp_us; + + + + int _last_node_sync_bit; + int _last_dist_q2; + + SlamtecLidarTimingDesc _cachedTimingDesc; +}; + + +} + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp b/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp new file mode 100644 index 0000000..8ed450e --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.cpp @@ -0,0 +1,192 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * HQNode Sample Node Handler + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "../dataunnpacker_commondef.h" +#include "../dataunpacker.h" +#include "../dataunnpacker_internal.h" + +#ifdef CONF_NO_BOOST_CRC_SUPPORT +#include "sl_crc.h" +#endif + +#include "handler_hqnode.h" + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker{ + + +static _u64 _getSampleDelayOffsetInHQMode(const SlamtecLidarTimingDesc& timing) +{ + // FIXME: to eval + // + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:1000000; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_hq_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS; +} + +UnpackerHandler_HQNode::UnpackerHandler_HQNode() + : _cached_scan_node_buf_pos(0) +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +} + +UnpackerHandler_HQNode::~UnpackerHandler_HQNode() +{ + +} + +_u8 UnpackerHandler_HQNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT_HQ; +} + +void UnpackerHandler_HQNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + + for (size_t pos = 0; pos < cnt; ++pos) + { + _u8 current_data = data[pos]; + + switch (_cached_scan_node_buf_pos) + { + case 0: // expect the sync byte + { + if (current_data == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC) { + // pass + } + else { + continue; + } + } + break; + + case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4: // get bytes to calculate crc ready + { + + } + break; + + case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + rplidar_response_hq_capsule_measurement_nodes_t* nodesData = reinterpret_cast(&_cached_scan_node_buf[0]); + +#ifdef CONF_NO_BOOST_CRC_SUPPORT + _u32 crcCalc = crc32::getResult(&_cached_scan_node_buf[0], sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 4); + + +#else + // calculate crc with boost crc method + boost::crc_optimal<32, 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true> mycrc; + std::vector<_u8> crcInputData; + crcInputData.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4); + memcpy(&crcInputData[0], nodesData, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4); + //supplement crcInputData to mutiples of 4 + int leftBytes = 4 - (crcInputData.size() & 3); + for (int i = 0; i < leftBytes; i++) + crcInputData.push_back(0); + mycrc.process_bytes(&crcInputData[0], crcInputData.size()); + _u32 crcCalc = mycrc.checksum(); + +#endif + + _u32 recvCRC = nodesData->crc32; +#ifdef _CPU_ENDIAN_BIG + recvCRC = le32_to_cpu(recvCRC); + nodesData->time_stamp = le64_to_cpu(nodesData->time_stamp); +#endif + if (recvCRC == crcCalc) + { + for (size_t pos = 0; pos < _countof(nodesData->node_hq); ++pos) + { + rplidar_response_measurement_node_hq_t hqNode = nodesData->node_hq[pos]; +#ifdef _CPU_ENDIAN_BIG + hqNode.angle_z_q14 = le16_to_cpu(hqNode.angle_z_q14); + hqNode.dist_mm_q2 = le32_to_cpu(hqNode.dist_mm_q2); +#endif + engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInHQMode(_cachedTimingDesc), &hqNode); + } + } + else //crc check not passed + { + engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR + , RPLIDAR_ANS_TYPE_MEASUREMENT_HQ, nodesData, sizeof(*nodesData)); + } + continue; + } + break; + + + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } + +} + + +void UnpackerHandler_HQNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + +void UnpackerHandler_HQNode::reset() +{ + _cached_scan_node_buf_pos = 0; +} +} + + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.h b/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.h new file mode 100644 index 0000000..178a8c6 --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_hqnode.h @@ -0,0 +1,63 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * HQNode Sample Node Handler + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker { + + class UnpackerHandler_HQNode : public IDataUnpackerHandler { + public: + UnpackerHandler_HQNode(); + virtual ~UnpackerHandler_HQNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); + + protected: + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + SlamtecLidarTimingDesc _cachedTimingDesc; + }; + +} + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp b/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp new file mode 100644 index 0000000..b799b77 --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.cpp @@ -0,0 +1,159 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Normal Sample Node Handler + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "../dataunnpacker_commondef.h" +#include "../dataunpacker.h" +#include "../dataunnpacker_internal.h" + + +#include "handler_normalnode.h" + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker{ + + +static _u64 _getSampleDelayOffsetInLegacyMode(const SlamtecLidarTimingDesc& timing) +{ + // guess channel baudrate by LIDAR model .... + const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200; + + _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_t) * 10 / channelBaudRate; + + if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET) + { + tranmissionDelay = 100; //dummy value + } + + // center of the sample duration + const _u64 sampleDelay = (timing.sample_duration_uS >> 1); + const _u64 sampleFilterDelay = timing.sample_duration_uS; + + return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS; +} + +UnpackerHandler_NormalNode::UnpackerHandler_NormalNode() + : _cached_scan_node_buf_pos(0) +{ + _cached_scan_node_buf.resize(sizeof(rplidar_response_measurement_node_t)); + memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc)); +;} + +UnpackerHandler_NormalNode::~UnpackerHandler_NormalNode() +{ + +} + +_u8 UnpackerHandler_NormalNode::getSampleAnswerType() const +{ + return RPLIDAR_ANS_TYPE_MEASUREMENT; +} + +void UnpackerHandler_NormalNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt) +{ + for (size_t pos = 0; pos < cnt; ++pos) { + _u8 current_data = data[pos]; + switch (_cached_scan_node_buf_pos) { + case 0: // expect the sync bit and its reverse in this byte + { + _u8 tmp = (current_data >> 1); + if ((tmp ^ current_data) & 0x1) { + // pass + } + else { + continue; + } + + } + break; + case 1: // expect the highest bit to be 1 + { + if (current_data & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) { + // pass + } + else { + _cached_scan_node_buf_pos = 0; + continue; + } + } + break; + case sizeof(rplidar_response_measurement_node_t) - 1: // new data ready + { + _cached_scan_node_buf[sizeof(rplidar_response_measurement_node_t) - 1] = current_data; + _cached_scan_node_buf_pos = 0; + + rplidar_response_measurement_node_t* node = reinterpret_cast(&_cached_scan_node_buf[0]); +#ifdef _CPU_ENDIAN_BIG + node->angle_q6_checkbit = le16_to_cpu(node->angle_q6_checkbit); + node->distance_q2 = le16_to_cpu(node->distance_q2); +#endif + //cast node to rplidar_response_measurement_node_hq_t + rplidar_response_measurement_node_hq_t hqNode; + hqNode.angle_z_q14 = (((node->angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle + hqNode.dist_mm_q2 = node->distance_q2; + hqNode.flag = (node->sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field + hqNode.quality = (node->sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255 + + + engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInLegacyMode(_cachedTimingDesc), &hqNode); + continue; + + } + break; + } + _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data; + } +} + + +void UnpackerHandler_NormalNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) +{ + if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) { + assert(size == sizeof(_cachedTimingDesc)); + _cachedTimingDesc = *reinterpret_cast(data); + } +} + +void UnpackerHandler_NormalNode::reset() +{ + _cached_scan_node_buf_pos = 0; +} +} + + +END_DATAUNPACKER_NS() \ No newline at end of file diff --git a/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.h b/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.h new file mode 100644 index 0000000..9068b1b --- /dev/null +++ b/LIDAR/sdk/src/dataunpacker/unpacker/handler_normalnode.h @@ -0,0 +1,63 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + + /* + * Sample Data Unpacker System + * Normal Sample Node Handler + */ + + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +BEGIN_DATAUNPACKER_NS() + +namespace unpacker{ + +class UnpackerHandler_NormalNode : public IDataUnpackerHandler { +public: + UnpackerHandler_NormalNode(); + virtual ~UnpackerHandler_NormalNode(); + + virtual _u8 getSampleAnswerType() const; + virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size); + virtual void reset(); + virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size); +protected: + std::vector<_u8> _cached_scan_node_buf; + int _cached_scan_node_buf_pos; + + SlamtecLidarTimingDesc _cachedTimingDesc; +}; + +} + +END_DATAUNPACKER_NS() \ No newline at end of file -- cgit v1.2.3