diff --git a/linux/devices/startracker/ArcsecDatalinkLayer.cpp b/linux/devices/startracker/ArcsecDatalinkLayer.cpp index c7f1144e..6eb3fdc3 100644 --- a/linux/devices/startracker/ArcsecDatalinkLayer.cpp +++ b/linux/devices/startracker/ArcsecDatalinkLayer.cpp @@ -1,9 +1,47 @@ #include "ArcsecDatalinkLayer.h" -ArcsecDatalinkLayer::ArcsecDatalinkLayer() { slipInit(); } +ArcsecDatalinkLayer::ArcsecDatalinkLayer() : decodeRingBuf(4096, true) { slipInit(); } ArcsecDatalinkLayer::~ArcsecDatalinkLayer() {} +ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedFrame, + size_t& frameLen) { + size_t currentLen = decodeRingBuf.getAvailableReadData(); + decodeRingBuf.readData(decodedRxFrame, currentLen); + for (size_t idx = 0; idx < currentLen; idx++) { + enum arc_dec_result decResult = + arc_transport_decode_body(decodedRxFrame[idx], &slipInfo, decodedRxFrame, &rxFrameSize); + switch (decResult) { + case ARC_DEC_INPROGRESS: { + break; + } + case ARC_DEC_ERROR_FRAME_SHORT: { + decodeRingBuf.deleteData(idx); + return REPLY_TOO_SHORT; + } + case ARC_DEC_ERROR_CHECKSUM: + decodeRingBuf.deleteData(idx); + return CRC_FAILURE; + case ARC_DEC_ASYNC: + case ARC_DEC_SYNC: { + // Reset length of SLIP struct for next frame + slipInfo.length = 0; + if (decodedFrame != nullptr) { + *decodedFrame = decodedRxFrame; + } + frameLen = rxFrameSize; + decodeRingBuf.deleteData(idx); + return returnvalue::OK; + } + default: + sif::debug << "ArcsecDatalinkLayer::decodeFrame: Unknown result code" << std::endl; + break; + return returnvalue::FAILED; + } + } + return DEC_IN_PROGRESS; +} + void ArcsecDatalinkLayer::slipInit() { slipInfo.buffer = rxBuffer; slipInfo.maxlength = startracker::MAX_FRAME_SIZE; @@ -17,7 +55,7 @@ ReturnValue_t ArcsecDatalinkLayer::decodeFrame(const uint8_t* rawData, size_t ra size_t bytePos = 0; for (bytePos = 0; bytePos < rawDataSize; bytePos++) { enum arc_dec_result decResult = - arc_transport_decode_body(*(rawData + bytePos), &slipInfo, decodedFrame, &decFrameSize); + arc_transport_decode_body(*(rawData + bytePos), &slipInfo, decodedRxFrame, &rxFrameSize); *bytesLeft = rawDataSize - bytePos - 1; switch (decResult) { case ARC_DEC_INPROGRESS: { @@ -45,18 +83,18 @@ ReturnValue_t ArcsecDatalinkLayer::decodeFrame(const uint8_t* rawData, size_t ra return returnvalue::FAILED; } -uint8_t ArcsecDatalinkLayer::getReplyFrameType() { return decodedFrame[0]; } +uint8_t ArcsecDatalinkLayer::getReplyFrameType() { return decodedRxFrame[0]; } -const uint8_t* ArcsecDatalinkLayer::getReply() { return &decodedFrame[1]; } +const uint8_t* ArcsecDatalinkLayer::getReply() { return &decodedRxFrame[1]; } -void ArcsecDatalinkLayer::encodeFrame(const uinah uint32_t length) { - arc_transport_encode_body(data, length, encBuffer, &encFrameSize); +void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, uint8_t** txFrame, + size_t& size) { + arc_transport_encode_body(data, length, txEncoded, &size); + if (txFrame != nullptr) { + *txFrame = txEncoded; + } } -uint8_t* ArcsecDatalinkLayer::getEncodedFrame() { return encBuffer; } +uint8_t ArcsecDatalinkLayer::getStatusField() { return *(decodedRxFrame + STATUS_OFFSET); } -uint32_t ArcsecDatalinkLayer::getEncodedLength() { return encFrameSize; } - -uint8_t ArcsecDatalinkLayer::getStatusField() { return *(decodedFrame + STATUS_OFFSET); } - -uint8_t ArcsecDatalinkLayer::getId() { return *(decodedFrame + ID_OFFSET); } +uint8_t ArcsecDatalinkLayer::getId() { return *(decodedRxFrame + ID_OFFSET); } diff --git a/linux/devices/startracker/ArcsecDatalinkLayer.h b/linux/devices/startracker/ArcsecDatalinkLayer.h index 5681d3ca..204b9f4a 100644 --- a/linux/devices/startracker/ArcsecDatalinkLayer.h +++ b/linux/devices/startracker/ArcsecDatalinkLayer.h @@ -1,6 +1,8 @@ #ifndef BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_ #define BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_ +#include + #include "eive/resultClassIds.h" #include "fsfw/returnvalues/returnvalue.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" @@ -28,9 +30,19 @@ class ArcsecDatalinkLayer { ArcsecDatalinkLayer(); virtual ~ArcsecDatalinkLayer(); + /** + * Feed received data to the internal ring buffer. + * @param rawData + * @param rawDataLen + * @return + */ + ReturnValue_t feedData(const uint8_t* rawData, size_t rawDataLen); + + ReturnValue_t checkRingBufForFrame(const uint8_t** decodedFrame, size_t& frameLen); + /** * @brief Applies decoding to data referenced by rawData pointer - * + * TODO: To be deleted soon, replaced by proper buffering. * @param rawData Pointer to raw data received from star tracker * @param rawDataSize Size of raw data stream * @param remainingBytes Number of bytes left @@ -43,7 +55,7 @@ class ArcsecDatalinkLayer { * @param data Pointer to data to encode * @param length Length of buffer to encode */ - void encodeFrame(const uint8_t* data, uint32_t length); + void encodeFrame(const uint8_t* data, size_t length, uint8_t** txFrame, size_t& frameLen); /** * @brief Returns the frame type field of a decoded frame. @@ -55,16 +67,6 @@ class ArcsecDatalinkLayer { */ const uint8_t* getReply(); - /** - * @brief Returns size of encoded frame - */ - uint32_t getEncodedLength(); - - /** - * @brief Returns pointer to encoded frame - */ - uint8_t* getEncodedFrame(); - /** * @brief Returns status of reply */ @@ -81,15 +83,19 @@ class ArcsecDatalinkLayer { // Used by arcsec slip decoding function process received data uint8_t rxBuffer[startracker::MAX_FRAME_SIZE]; + SimpleRingBuffer decodeRingBuf; + uint8_t rxAnalysisBuffer[4096]; + // Decoded frame will be copied to this buffer - uint8_t decodedFrame[startracker::MAX_FRAME_SIZE]; + uint8_t decodedRxFrame[startracker::MAX_FRAME_SIZE]; + // Size of decoded frame + uint32_t rxFrameSize = 0; + // Buffer where encoded frames will be stored. First byte of encoded frame represents type of // reply - uint8_t encBuffer[startracker::MAX_FRAME_SIZE * 2 + 2]; - // Size of decoded frame - uint32_t decFrameSize = 0; + uint8_t txEncoded[startracker::MAX_FRAME_SIZE * 2 + 2]; // Size of encoded frame - uint32_t encFrameSize = 0; + uint32_t txFrameSize = 0; slip_decode_state slipInfo; diff --git a/linux/devices/startracker/StrHelper.cpp b/linux/devices/startracker/StrHelper.cpp index 8dd3641d..651285cc 100644 --- a/linux/devices/startracker/StrHelper.cpp +++ b/linux/devices/startracker/StrHelper.cpp @@ -491,9 +491,11 @@ ReturnValue_t StrHelper::sendAndRead(size_t size, uint32_t parameter, uint32_t d uint8_t* receivedData = nullptr; size_t bytesLeft = 0; uint32_t missedReplies = 0; - datalinkLayer.encodeFrame(commandBuffer, size); - result = uartComIF->sendMessage(comCookie, datalinkLayer.getEncodedFrame(), - datalinkLayer.getEncodedLength()); + + uint8_t* sendData; + size_t txFrameLen = 0; + datalinkLayer.encodeFrame(commandBuffer, size, &sendData, txFrameLen); + result = uartComIF->sendMessage(comCookie, sendData, txFrameLen); if (result != returnvalue::OK) { sif::warning << "StrHelper::sendAndRead: Failed to send packet" << std::endl; triggerEvent(STR_HELPER_SENDING_PACKET_FAILED, result, parameter);