diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 147c04d8..7999519c 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -1162,7 +1162,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, break; } case (startracker::REQ_SOLUTION): { - result = handleTm(packet, solutionSet, "REQ_SOLUTION"); + result = handleSolution(packet); break; } case (startracker::REQ_CONTRAST): { @@ -2438,6 +2438,36 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat return result; } +ReturnValue_t StarTrackerHandler::handleSolution(const uint8_t* rawFrame) { + ReturnValue_t result = statusFieldCheck(rawFrame); + if (result != returnvalue::OK) { + return result; + } + PoolReadGuard pg(&solutionSet); + if (pg.getReadResult() != returnvalue::OK) { + return result; + } + const uint8_t* reply = rawFrame + TICKS_OFFSET; + solutionSet.setValidityBufferGeneration(false); + size_t sizeLeft = fullPacketLen; + result = solutionSet.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for solution set: 0x" + << std::hex << std::setw(4) << result << std::dec << std::endl; + } + solutionSet.setValidityBufferGeneration(true); + solutionSet.setValidity(true, true); + solutionSet.caliQw.setValid(solutionSet.isTrustWorthy.value); + solutionSet.caliQx.setValid(solutionSet.isTrustWorthy.value); + solutionSet.caliQy.setValid(solutionSet.isTrustWorthy.value); + solutionSet.caliQz.setValid(solutionSet.isTrustWorthy.value); + solutionSet.trackQw.setValid(solutionSet.isTrustWorthy.value); + solutionSet.trackQx.setValid(solutionSet.isTrustWorthy.value); + solutionSet.trackQy.setValid(solutionSet.isTrustWorthy.value); + solutionSet.trackQz.setValid(solutionSet.isTrustWorthy.value); + return result; +} + ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) { ReturnValue_t result = statusFieldCheck(rawFrame); if (result != returnvalue::OK) { diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index 6bc1ff09..91651e04 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -527,6 +527,7 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, const char* context); + ReturnValue_t handleSolution(const uint8_t* rawFrame); ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame); ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame); ReturnValue_t handleBlobTm(const uint8_t* rawFrame);