From b8a07a3299ee141a2d1118e37482b2e9481ebbea Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Feb 2024 13:05:13 +0100 Subject: [PATCH] continue with new set --- mission/acs/str/StarTrackerHandler.cpp | 24 ++++++++++++++++++++---- mission/acs/str/StarTrackerHandler.h | 1 + mission/acs/str/strHelpers.h | 26 ++++++++++++++++++++++++-- 3 files changed, 45 insertions(+), 6 deletions(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 25d67b72..98066ee4 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -63,6 +63,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, centroidSet(this), centroidsSet(this), contrastSet(this), + blobStatsSet(this), strHelper(strHelper), powerSwitch(powerSwitch), sdCardIF(sdCardIF), @@ -597,6 +598,11 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi prepareRequestContrastTm(); return returnvalue::OK; } + case (startracker::REQ_BLOB_STATS): { + // TODO: Implement function. + // prepareRequestCentroidsTm(); + return returnvalue::OK; + } case (startracker::BOOT): { prepareBootCommand(static_cast(firmwareTargetRaw)); return returnvalue::OK; @@ -638,10 +644,6 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi prepareHistogramRequest(); return returnvalue::OK; } - case (startracker::REQ_BLOB_STATS): { - prepareHistogramRequest(); - return returnvalue::OK; - } case (startracker::LIMITS): { result = prepareParamCommand(commandData, commandDataLen, jcfgs.limits, reinitNextSetParam); return result; @@ -889,6 +891,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() { startracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet, startracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(startracker::REQ_BLOB_STATS, 3, &blobStatsSet, + startracker::MAX_FRAME_SIZE * 2 + 2); } ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) { @@ -1086,6 +1090,12 @@ void StarTrackerHandler::resetSecondaryTmSet() { histogramSet.setValidity(false, true); } } + { + PoolReadGuard pg(&blobStatsSet); + if (pg.getReadResult() == returnvalue::OK) { + histogramSet.setValidity(false, true); + } + } } void StarTrackerHandler::bootBootloader() { @@ -1234,6 +1244,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, result = handleTm(packet, histogramSet, "REQ_HISTO"); break; } + case (startracker::REQ_BLOB_STATS): { + result = handleTm(packet, blobStatsSet, "REQ_BLOB_STATS"); + break; + } case (startracker::SUBSCRIPTION): case (startracker::LOGLEVEL): case (startracker::LOGSUBSCRIPTION): @@ -1663,6 +1677,8 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(blobStatsSet.getSid(), false, 10.0)); return returnvalue::OK; } diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index f6611ed6..ab5e4540 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -216,6 +216,7 @@ class StarTrackerHandler : public DeviceHandlerBase { startracker::CentroidSet centroidSet; startracker::CentroidsSet centroidsSet; startracker::ContrastSet contrastSet; + startracker::BlobStatsSet blobStatsSet; // Pointer to object responsible for uploading and downloading images to/from the star tracker StrComHandler* strHelper = nullptr; diff --git a/mission/acs/str/strHelpers.h b/mission/acs/str/strHelpers.h index 009b8a47..4ec6af5a 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -328,6 +328,11 @@ enum PoolIds : lp_id_t { CONTRAST_B, CONTRAST_C, CONTRAST_D, + + BLOB_STATS_NOISE, + BLOB_STATS_THOLD, + BLOB_STATS_LVALID, + BLOB_STATS_OFLOW, }; static const DeviceCommandId_t PING_REQUEST = 0; @@ -394,8 +399,9 @@ static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 95; static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 96; static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97; static constexpr DeviceCommandId_t RELOAD_JSON_CFG_FILE = 100; -static const DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101; -static const DeviceCommandId_t REQ_BLOB_STATS= 102; +static constexpr DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101; +static constexpr DeviceCommandId_t REQ_BLOB_STATS = 102; + static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; @@ -427,6 +433,7 @@ static const uint32_t BLOBS_SET_ID = REQ_BLOBS; static const uint32_t CENTROID_SET_ID = REQ_CENTROID; static const uint32_t CENTROIDS_SET_ID = REQ_CENTROIDS; static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST; +static const uint32_t BLOB_STATS_SET_ID = REQ_BLOB_STATS; /** Max size of unencoded frame */ static const size_t MAX_FRAME_SIZE = 1200; @@ -1559,6 +1566,21 @@ class CentroidsSet : public StaticLocalDataSet<10> { lp_vec_t(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this); }; +class BlobStatsSet : public StaticLocalDataSet<6> { + public: + BlobStatsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOB_STATS_SET_ID) {} + + // Data received from the Centroids Telemetry Set (ID 49) + lp_vec_t noise = + lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_NOISE, this); + lp_vec_t thold = + lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_THOLD, this); + lp_vec_t lvalid = + lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_LVALID, this); + lp_vec_t oflow = + lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_OFLOW, this); +}; + class ContrastSet : public StaticLocalDataSet<8> { public: ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {}