diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index bf31c99a..6278739e 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -49,7 +49,8 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, subscriptionSet(this), logSubscriptionSet(this), debugCameraSet(this), - matchedCentroids(this), + autoBlobSet(this), + matchedCentroidsSet(this), blobSet(this), blobsSet(this), centroidSet(this), @@ -792,7 +793,7 @@ void StarTrackerHandler::fillCommandAndReplyMap() { startracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(startracker::REQ_DEBUG_CAMERA, 3, &debugCameraSet, startracker::MAX_FRAME_SIZE * 2 + 2); - this->insertInCommandAndReplyMap(startracker::REQ_MATCHED_CENTROIDS, 3, &matchedCentroids, + this->insertInCommandAndReplyMap(startracker::REQ_MATCHED_CENTROIDS, 3, &matchedCentroidsSet, startracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(startracker::REQ_BLOB, 3, &blobSet, startracker::MAX_FRAME_SIZE * 2 + 2); @@ -1494,7 +1495,7 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(lisaSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( - subdp::RegularHkPeriodicParams(matchedCentroids.getSid(), false, 10.0)); + subdp::RegularHkPeriodicParams(matchedCentroidsSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(blobSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( @@ -2266,28 +2267,58 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat return result; } +ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) { + ReturnValue_t result = statusFieldCheck(rawFrame); + if (result != returnvalue::OK) { + return result; + } + size_t remainingLen = fullPacketLen; + PoolReadGuard pg(&autoBlobSet); + result = pg.getReadResult(); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&autoBlobSet.ticks, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&autoBlobSet.timeUs, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&autoBlobSet.threshold, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + autoBlobSet.setValidity(true, true); + return returnvalue::OK; +} + ReturnValue_t StarTrackerHandler::handleMatchedCentroidTm(const uint8_t* rawFrame) { ReturnValue_t result = statusFieldCheck(rawFrame); if (result != returnvalue::OK) { return result; } size_t remainingLen = fullPacketLen; - PoolReadGuard pg(&matchedCentroids); + PoolReadGuard pg(&matchedCentroidsSet); result = pg.getReadResult(); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&matchedCentroids.ticks, &rawFrame, &remainingLen, + result = SerializeAdapter::deSerialize(&matchedCentroidsSet.ticks, &rawFrame, &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&matchedCentroids.timeUs, &rawFrame, &remainingLen, + result = SerializeAdapter::deSerialize(&matchedCentroidsSet.timeUs, &rawFrame, &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&matchedCentroids.numberOfMatchedCentroids, &rawFrame, + result = SerializeAdapter::deSerialize(&matchedCentroidsSet.numberOfMatchedCentroids, &rawFrame, &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { @@ -2295,33 +2326,33 @@ ReturnValue_t StarTrackerHandler::handleMatchedCentroidTm(const uint8_t* rawFram } // Yeah, we serialize it like that because I can't model anything with that local datapool crap. for (unsigned idx = 0; idx < 16; idx++) { - result = SerializeAdapter::deSerialize(&matchedCentroids.starIds[idx], &rawFrame, &remainingLen, - SerializeIF::Endianness::LITTLE); + result = SerializeAdapter::deSerialize(&matchedCentroidsSet.starIds[idx], &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&matchedCentroids.xCoords[idx], &rawFrame, &remainingLen, - SerializeIF::Endianness::LITTLE); + result = SerializeAdapter::deSerialize(&matchedCentroidsSet.xCoords[idx], &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&matchedCentroids.yCoords[idx], &rawFrame, &remainingLen, - SerializeIF::Endianness::LITTLE); + result = SerializeAdapter::deSerialize(&matchedCentroidsSet.yCoords[idx], &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&matchedCentroids.xErrors[idx], &rawFrame, &remainingLen, - SerializeIF::Endianness::LITTLE); + result = SerializeAdapter::deSerialize(&matchedCentroidsSet.xErrors[idx], &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&matchedCentroids.yErrors[idx], &rawFrame, &remainingLen, - SerializeIF::Endianness::LITTLE); + result = SerializeAdapter::deSerialize(&matchedCentroidsSet.yErrors[idx], &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } } - matchedCentroids.setValidity(true, true); + matchedCentroidsSet.setValidity(true, true); return returnvalue::OK; } diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index b239559a..90261285 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -206,7 +206,8 @@ class StarTrackerHandler : public DeviceHandlerBase { startracker::SubscriptionSet subscriptionSet; startracker::LogSubscriptionSet logSubscriptionSet; startracker::DebugCameraSet debugCameraSet; - startracker::MatchedCentroidsSet matchedCentroids; + startracker::AutoBlobSet autoBlobSet; + startracker::MatchedCentroidsSet matchedCentroidsSet; startracker::BlobSet blobSet; startracker::BlobsSet blobsSet; startracker::CentroidSet centroidSet; @@ -518,6 +519,7 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size, const char* context); + ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame); ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame); ReturnValue_t handleBlobTm(const uint8_t* rawFrame); ReturnValue_t handleBlobsTm(const uint8_t* rawFrame);