diff options
Diffstat (limited to 'LIDAR/sdk/src/sl_async_transceiver.cpp')
| -rw-r--r-- | LIDAR/sdk/src/sl_async_transceiver.cpp | 412 |
1 files changed, 412 insertions, 0 deletions
diff --git a/LIDAR/sdk/src/sl_async_transceiver.cpp b/LIDAR/sdk/src/sl_async_transceiver.cpp new file mode 100644 index 0000000..c8033a5 --- /dev/null +++ b/LIDAR/sdk/src/sl_async_transceiver.cpp @@ -0,0 +1,412 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * 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 "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 "sl_async_transceiver.h" + + + +namespace sl { namespace internal { + + + + +ProtocolMessage::ProtocolMessage() + : len(0) + , cmd(0) + , data(NULL) + , _databufsize(0) + , _usingOutterData(false) +{ + _changeBufSize(); +} + +ProtocolMessage::ProtocolMessage(_u8 cmd, const void* buffer, size_t size) + : len(size) + , cmd(cmd) + , data(NULL) + , _databufsize(0) + , _usingOutterData(false) +{ + _changeBufSize(); + if (buffer) + { + memcpy(data, buffer, size); + } +} + +ProtocolMessage::ProtocolMessage(const ProtocolMessage& srcMsg) + : len(srcMsg.len) + , cmd(srcMsg.cmd) + , data(NULL) + , _databufsize(0) + , _usingOutterData(false) +{ + _changeBufSize( true ); + if (srcMsg.data && len) + { + memcpy(data, srcMsg.data, len); + } +} + +ProtocolMessage::~ProtocolMessage() +{ + this->cleanData(); +} + +ProtocolMessage& ProtocolMessage::operator =(const ProtocolMessage& srcMessage) +{ + this->cleanData(); + + + this->len = srcMessage.len; + this->cmd = srcMessage.cmd; + + _changeBufSize( true ); + if (srcMessage.data && len) + { + memcpy(data, srcMessage.data, len); + } + + return *this; +} + +void ProtocolMessage::setDataBuf(_u8 *buffer, size_t size) +{ + this->cleanData(); + + len = size; + data = buffer; + _databufsize = size; + _usingOutterData = true; +} + +void ProtocolMessage::fillData(const void * buffer, size_t size) +{ + len = size; + _changeBufSize(); + if (buffer) + memcpy(data, buffer, size); +} + +void ProtocolMessage::cleanData() +{ + if (data) + { + if (!_usingOutterData) + { + delete [] data; + } + data = NULL; + len = 1; + _databufsize = 0; + } +} + +void ProtocolMessage::_changeBufSize( bool force_compact) +{ + size_t actual_size = getPayloadSize(); + + size_t new_buf_size = actual_size; + + + if (!_usingOutterData) + { + // nothing to do + if ( new_buf_size == _databufsize ) return; + + if ( new_buf_size < _databufsize){ + + if ( (_databufsize >> 1) < new_buf_size) + { + // reuse the current buffer + if (!force_compact) return; + }else + { + // the current buffer size is much bigger, we need to release it to save memory + } + } + } + + // we need to change the buffer + cleanData(); + // the cleanData() will reset the length info, so we need to restore it + len = actual_size; + data = new _u8[new_buf_size]; + _databufsize = new_buf_size; +} + + +AsyncTransceiver::AsyncTransceiver(IAsyncProtocolCodec& codec) + : _bindedChannel(NULL) + , _codec(codec) + , _isWorking(false) + , _workingFlag(0) +{ + +} + +AsyncTransceiver::~AsyncTransceiver() +{ + unbindAndClose(); +} + +u_result AsyncTransceiver::openChannelAndBind(IChannel* channel) +{ + if (!channel) return RESULT_INVALID_DATA; + + unbindAndClose(); + u_result ans = RESULT_OK; + do + { + rp::hal::AutoLocker l(_opLocker); + + // try to open the channel ... + Result<nullptr_t> ans = SL_RESULT_OK; + + if (!channel->open()) { + ans= RESULT_OPERATION_FAIL; + break; + } + + + // force a flush to clear any pending data + channel->flush(); + + _dataEvt.set(false); + + _isWorking = true; + _workingFlag = 0; + _bindedChannel = channel; + + + _decoderThread = CLASS_THREAD(AsyncTransceiver, _proc_decoderThread); + _rxThread = CLASS_THREAD(AsyncTransceiver, _proc_rxThread); + + + + + } while (0); + + return ans; +} + +void AsyncTransceiver::unbindAndClose() +{ + rp::hal::AutoLocker l(_opLocker); + if (!_isWorking) return; + + assert(_bindedChannel); + + + _isWorking = false; + _dataEvt.set(); // set signal to wake up threads + + _decoderThread.join(); + _rxThread.join(); + + + _bindedChannel->close(); + + _bindedChannel = NULL; + + + for (std::list< Buffer* >::iterator itr = _rxQueue.begin(); itr != _rxQueue.end(); ++itr) + { + delete [] *itr; + } + _rxQueue.clear(); + +} + +u_result AsyncTransceiver::sendMessage(message_autoptr_t& msg) +{ + assert(msg); + + if (!_isWorking) return RESULT_OPERATION_NOT_SUPPORT; + + rp::hal::AutoLocker l(_opLocker); + + size_t requiredBufferSize = _codec.estimateLength(msg); + + if (requiredBufferSize == 0) { + // nothing to send + return RESULT_OK; + } + + u_result ans = RESULT_OK; + + _u8* txBuffer = new _u8[requiredBufferSize]; + + do { + + if (!txBuffer) { + return RESULT_INSUFFICIENT_MEMORY; + } + + _codec.onEncodeData(msg, txBuffer, &requiredBufferSize); + + int txSize = _bindedChannel->write(txBuffer, requiredBufferSize); + + if (txSize < 0) ans = RESULT_OPERATION_FAIL; + + } while (0); + + + delete[] txBuffer; + return ans; +} + +sl_result AsyncTransceiver::_proc_rxThread() +{ + assert(_bindedChannel); + + rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH); + + u_result result; + size_t hintedSize = 0; + while (_isWorking) + { + result = _bindedChannel->waitForDataExt(hintedSize, 1000); + + if (IS_FAIL(result)) + { + // timeout is allowed + if (result == RESULT_OPERATION_TIMEOUT) { + continue; + } + if (_isWorking) { + _workingFlag |= WORKING_FLAG_ERROR; + _codec.onChannelError(result); + break; + } + } + + // no data in buffer, sleep and wait for the next round + if (!hintedSize) + { + continue; + } + + + Buffer* decodeBuffer = new Buffer(); + + decodeBuffer->data = new _u8[hintedSize]; + + decodeBuffer->size = _bindedChannel->read(decodeBuffer->data, hintedSize); +#ifdef _DEBUG_DUMP_PACKET + printf("Revc: %d\n", decodeBuffer->size); +#endif + + if (!decodeBuffer->size) { + delete decodeBuffer; + + + _workingFlag |= WORKING_FLAG_ERROR; + _codec.onChannelError(RESULT_OPERATION_ABORTED); + break; + } + + assert(hintedSize >= decodeBuffer->size); + + +#ifdef _DEBUG_DUMP_PACKET + printf("=== Dump RX Packet, size = %d ===\n", decodeBuffer->size); + for (int pos = 0; pos < decodeBuffer->size; pos++) + { + printf("%02x ", decodeBuffer->data[pos]); + } + printf("\n=== END ===\n"); +#endif + + _rxLocker.lock(); + _rxQueue.push_back(decodeBuffer); + _dataEvt.set(); + _rxLocker.unlock(); + + + } + _workingFlag |= WORKING_FLAG_RX_DISABLED; + return RESULT_OK; +} + +sl_result AsyncTransceiver::_proc_decoderThread() +{ + + assert(_bindedChannel); + rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH); + _codec.onDecodeReset(); + + + while (_isWorking) + { + _rxLocker.lock(); + + if (_rxQueue.empty()) + { + _rxLocker.unlock(); + + if (_dataEvt.wait(1000)) + continue; + + _rxLocker.lock(); + } + assert(!_rxQueue.empty()); + + Buffer * bufferToDecode = _rxQueue.front(); + _rxQueue.pop_front(); + + _rxLocker.unlock(); + + //cout<<"decoding "<< bufferToDecode->size <<" bytes of data"<<endl; + _codec.onDecodeData(bufferToDecode->data, bufferToDecode->size); + + + delete bufferToDecode; + } + + return RESULT_OK; + +} + + +}}
\ No newline at end of file |