diff --git a/mission/acs/str/ArcsecDatalinkLayer.cpp b/mission/acs/str/ArcsecDatalinkLayer.cpp index d67f6e75..c1082fc7 100644 --- a/mission/acs/str/ArcsecDatalinkLayer.cpp +++ b/mission/acs/str/ArcsecDatalinkLayer.cpp @@ -1,10 +1,11 @@ #include "ArcsecDatalinkLayer.h" extern "C" { +#include #include } -ArcsecDatalinkLayer::ArcsecDatalinkLayer() : decodeRingBuf(BUFFER_LENGTHS, true) { slipInit(); } +ArcsecDatalinkLayer::ArcsecDatalinkLayer() : decodeRingBuf(BUFFER_LENGTHS, true) {} ArcsecDatalinkLayer::~ArcsecDatalinkLayer() {} @@ -15,35 +16,50 @@ ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedF return DEC_IN_PROGRESS; } decodeRingBuf.readData(rxAnalysisBuffer, currentLen); + + bool startFound = false; + size_t startIdx = 0; for (size_t idx = 0; idx < currentLen; idx++) { - enum arc_dec_result decResult = - arc_transport_decode_body(rxAnalysisBuffer[idx], &slipInfo, decodedRxFrame, &rxFrameSize); - switch (decResult) { - case ARC_DEC_INPROGRESS: { - break; - } - case ARC_DEC_ERROR: { - decodeRingBuf.deleteData(idx); - return returnvalue::FAILED; - } - case ARC_DEC_ASYNC: - case ARC_DEC_SYNC: { - // Reset length of SLIP struct for next frame - slipInfo.length = 0; + if (rxAnalysisBuffer[idx] != SLIP_START_AND_END) { + continue; + } + if (not startFound) { + startFound = true; + startIdx = idx; + continue; + } + // Now we can try decoding the whole frame. + size_t encodedDataSize = 0; + slip_error_t slipError = + slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx, + idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID); + decodeRingBuf.deleteData(idx + 1); + switch (slipError) { + case (SLIP_OK): { if (decodedFrame != nullptr) { *decodedFrame = decodedRxFrame; } frameLen = rxFrameSize; - decodeRingBuf.deleteData(idx); return returnvalue::OK; } - default: - sif::debug << "ArcsecDatalinkLayer::decodeFrame: Unknown result code" << std::endl; - break; + case (SLIP_BAD_CRC): { + return CRC_FAILURE; + } + case (SLIP_OVERFLOW): { + return SLIP_OVERFLOW_RETVAL; + } + // Should not happen, we searched for start and end marker.. + case (SLIP_NO_END): { return returnvalue::FAILED; + } + case (SLIP_ID_MISMATCH): { + return SLIP_ID_MISSMATCH_RETVAL; + } + default: { + return returnvalue::FAILED; + } } } - decodeRingBuf.deleteData(currentLen); return DEC_IN_PROGRESS; } @@ -56,18 +72,11 @@ ReturnValue_t ArcsecDatalinkLayer::feedData(const uint8_t* rawData, size_t rawDa return decodeRingBuf.writeData(rawData, rawDataLen); } -void ArcsecDatalinkLayer::reset() { - slipInit(); - decodeRingBuf.clear(); -} - -void ArcsecDatalinkLayer::slipInit() { - slip_decode_init(rxBufferArc, sizeof(rxBufferArc), &slipInfo); -} +void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); } void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame, size_t& size) { - arc_transport_encode_body(data, length, txEncoded, &size); + slip_encode_frame(data, length, txEncoded, &size, ARC_DEF_SAGITTA_SLIP_ID); if (txFrame != nullptr) { *txFrame = txEncoded; } diff --git a/mission/acs/str/ArcsecDatalinkLayer.h b/mission/acs/str/ArcsecDatalinkLayer.h index 1453e517..06d1485d 100644 --- a/mission/acs/str/ArcsecDatalinkLayer.h +++ b/mission/acs/str/ArcsecDatalinkLayer.h @@ -25,6 +25,8 @@ class ArcsecDatalinkLayer { static const ReturnValue_t REPLY_TOO_SHORT = MAKE_RETURN_CODE(0xA1); //! [EXPORT] : [COMMENT] Detected CRC failure in received frame static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA2); + static const ReturnValue_t SLIP_OVERFLOW_RETVAL = MAKE_RETURN_CODE(0xA3); + static const ReturnValue_t SLIP_ID_MISSMATCH_RETVAL = MAKE_RETURN_CODE(0xA4); static const uint8_t STATUS_OK = 0; @@ -77,7 +79,7 @@ class ArcsecDatalinkLayer { // Decoded frame will be copied to this buffer uint8_t decodedRxFrame[startracker::MAX_FRAME_SIZE]; // Size of decoded frame - uint32_t rxFrameSize = 0; + size_t rxFrameSize = 0; // Buffer where encoded frames will be stored. First byte of encoded frame represents type of // reply @@ -85,7 +87,7 @@ class ArcsecDatalinkLayer { // Size of encoded frame uint32_t txFrameSize = 0; - slip_decode_state slipInfo; + // slip_decode_state slipInfo; void slipInit(); }; diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index cb0ca2c8..d40e5fab 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -5,7 +5,11 @@ #include extern "C" { -#include +#include +#include +#include +#include +#include } #include @@ -880,7 +884,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, switch (id) { case (startracker::REQ_TIME): { - result = handleTm(packet, timeSet, startracker::TimeSet::SIZE); + result = handleTm(packet, timeSet, startracker::TimeSet::SIZE, "REQ_TIME"); break; } case (startracker::PING_REQUEST): { @@ -895,7 +899,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, break; } case (startracker::REQ_VERSION): { - result = handleTm(packet, versionSet, startracker::VersionSet::SIZE); + result = handleTm(packet, versionSet, startracker::VersionSet::SIZE, "REQ_VERSION"); if (result != returnvalue::OK) { return result; } @@ -906,23 +910,23 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, break; } case (startracker::REQ_INTERFACE): { - result = handleTm(packet, interfaceSet, startracker::InterfaceSet::SIZE); + result = handleTm(packet, interfaceSet, startracker::InterfaceSet::SIZE, "REQ_INTERFACE"); break; } case (startracker::REQ_POWER): { - result = handleTm(packet, powerSet, startracker::PowerSet::SIZE); + result = handleTm(packet, powerSet, startracker::PowerSet::SIZE, "REQ_POWER"); break; } case (startracker::REQ_SOLUTION): { - result = handleTm(packet, solutionSet, startracker::SolutionSet::SIZE); + result = handleTm(packet, solutionSet, startracker::SolutionSet::SIZE, "REQ_SOLUTION"); break; } case (startracker::REQ_TEMPERATURE): { - result = handleTm(packet, temperatureSet, startracker::TemperatureSet::SIZE); + result = handleTm(packet, temperatureSet, startracker::TemperatureSet::SIZE, "REQ_TEMP"); break; } case (startracker::REQ_HISTOGRAM): { - result = handleTm(packet, histogramSet, startracker::HistogramSet::SIZE); + result = handleTm(packet, histogramSet, startracker::HistogramSet::SIZE, "REQ_HISTO"); break; } case (startracker::SUBSCRIPTION): @@ -1972,7 +1976,7 @@ ReturnValue_t StarTrackerHandler::checkProgram() { } ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, - size_t size) { + size_t size, const char* context) { ReturnValue_t result = returnvalue::OK; uint8_t status = startracker::getStatusField(rawFrame); if (status != startracker::STATUS_OK) { @@ -1988,7 +1992,8 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat dataset.setValidityBufferGeneration(false); result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { - sif::warning << "StarTrackerHandler::handleTm: Deserialization failed" << std::endl; + sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for " << context + << std::endl; } dataset.setValidityBufferGeneration(true); dataset.setValidity(true, true); diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index ea46cb43..a74bff44 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -481,7 +481,8 @@ class StarTrackerHandler : public DeviceHandlerBase { * * @return returnvalue::OK if successful, otherwise error return value */ - ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size); + ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size, + const char* context); /** * @brief Checks if star tracker is in valid mode for executing the received command. diff --git a/mission/acs/str/strHelpers.h b/mission/acs/str/strHelpers.h index 16b82661..69729df5 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -641,7 +641,7 @@ class TimeSet : public StaticLocalDataSet { */ class SolutionSet : public StaticLocalDataSet { public: - static const size_t SIZE = 78; + static const size_t SIZE = 79; SolutionSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, SOLUTION_SET_ID) {} diff --git a/thirdparty/sagittactl b/thirdparty/sagittactl index 29e87667..2c066bfb 160000 --- a/thirdparty/sagittactl +++ b/thirdparty/sagittactl @@ -1 +1 @@ -Subproject commit 29e876671a72fcdb0b393e2f692303725f00724f +Subproject commit 2c066bfbe9ef8d6b00e142b8f48d45988a469618