diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 97f3944d..20c2b511 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -50,7 +50,10 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, logSubscriptionSet(this), debugCameraSet(this), matchedCentroids(this), + blobSet(this), blobsSet(this), + centroidSet(this), + centroidsSet(this), strHelper(strHelper), paramJsonFile(jsonFileStr), powerSwitch(powerSwitch) { @@ -474,6 +477,12 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi prepareTimeRequest(); return returnvalue::OK; } + case (startracker::REQ_CENTROID): { + return returnvalue::OK; + } + case (startracker::REQ_CENTROIDS): { + return returnvalue::OK; + } case (startracker::BOOT): { prepareBootCommand(); return returnvalue::OK; @@ -745,10 +754,14 @@ void StarTrackerHandler::fillCommandAndReplyMap() { startracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(startracker::REQ_MATCHED_CENTROIDS, 3, &matchedCentroids, startracker::MAX_FRAME_SIZE * 2 + 2); - this->insertInCommandAndReplyMap(startracker::REQ_BLOB, 3, &blobsSet, + this->insertInCommandAndReplyMap(startracker::REQ_BLOB, 3, &blobSet, startracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(startracker::REQ_BLOBS, 3, &blobsSet, startracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(startracker::REQ_CENTROID, 3, ¢roidSet, + startracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(startracker::REQ_CENTROIDS, 3, ¢roidsSet, + startracker::MAX_FRAME_SIZE * 2 + 2); } ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) { @@ -1012,6 +1025,14 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, result = handleBlobsTm(packet); break; } + case (startracker::REQ_CENTROID): { + result = handleCentroidTm(packet); + break; + } + case (startracker::REQ_CENTROIDS): { + result = handleCentroidsTm(packet); + break; + } case (startracker::REQ_TEMPERATURE): { result = handleTm(packet, temperatureSet, startracker::TemperatureSet::SIZE, "REQ_TEMP"); break; @@ -1398,6 +1419,17 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(startracker::PoolIds::BLOBS_X_COORDS, new PoolEntry(8)); localDataPoolMap.emplace(startracker::PoolIds::BLOBS_Y_COORDS, new PoolEntry(8)); + localDataPoolMap.emplace(startracker::PoolIds::CENTROID_TICKS, new PoolEntry()); + localDataPoolMap.emplace(startracker::PoolIds::CENTROID_TIME, new PoolEntry()); + localDataPoolMap.emplace(startracker::PoolIds::CENTROID_COUNT, new PoolEntry()); + + localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_TICKS, new PoolEntry()); + localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_TIME, new PoolEntry()); + localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_COUNT, new PoolEntry()); + localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_X_COORDS, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_Y_COORDS, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_MAGNITUDES, new PoolEntry(16)); + poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( @@ -1412,6 +1444,16 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l subdp::RegularHkPeriodicParams(histogramSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(lisaSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(matchedCentroids.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(blobSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(blobsSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(centroidSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0)); return returnvalue::OK; } @@ -1853,6 +1895,22 @@ ReturnValue_t StarTrackerHandler::prepareRequestBlobsTm() { return returnvalue::OK; } +ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() { + uint32_t length = 0; + arc_tm_pack_centroid_req(commandBuffer, &length); + rawPacket = commandBuffer; + rawPacketLen = length; + return returnvalue::OK; +} + +ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() { + uint32_t length = 0; + arc_tm_pack_centroids_req(commandBuffer, &length); + rawPacket = commandBuffer; + rawPacketLen = length; + return returnvalue::OK; +} + ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData, size_t commandDataLen, ArcsecJsonParamBase& paramSet, @@ -2229,24 +2287,22 @@ ReturnValue_t StarTrackerHandler::handleBlobTm(const uint8_t* rawFrame) { if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&blobsSet.ticksBlobTm, &rawFrame, &remainingLen, + result = SerializeAdapter::deSerialize(&blobSet.ticks, &rawFrame, &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&blobsSet.timeUsBlobTm, &rawFrame, &remainingLen, + result = SerializeAdapter::deSerialize(&blobSet.timeUs, &rawFrame, &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&blobsSet.blobCount, &rawFrame, &remainingLen, + result = SerializeAdapter::deSerialize(&blobSet.blobCount, &rawFrame, &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - blobsSet.ticksBlobTm.setValid(true); - blobsSet.timeUsBlobTm.setValid(true); - blobsSet.blobCount.setValid(true); + blobSet.setValidity(true, true); return returnvalue::OK; } @@ -2261,12 +2317,12 @@ ReturnValue_t StarTrackerHandler::handleBlobsTm(const uint8_t* rawFrame) { if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&blobsSet.ticksBlobsTm, &rawFrame, &remainingLen, + result = SerializeAdapter::deSerialize(&blobsSet.ticks, &rawFrame, &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; } - result = SerializeAdapter::deSerialize(&blobsSet.timeUsBlobsTm, &rawFrame, &remainingLen, + result = SerializeAdapter::deSerialize(&blobsSet.timeUs, &rawFrame, &remainingLen, SerializeIF::Endianness::LITTLE); if (result != returnvalue::OK) { return result; @@ -2298,13 +2354,86 @@ ReturnValue_t StarTrackerHandler::handleBlobsTm(const uint8_t* rawFrame) { return result; } } - blobsSet.ticksBlobsTm.setValid(true); - blobsSet.timeUsBlobsTm.setValid(true); - blobsSet.blobsCount.setValid(true); - blobsSet.blobsCountUsed.setValid(true); - blobsSet.nr4LinesSkipped.setValid(true); - blobsSet.xCoords.setValid(true); - blobsSet.yCoords.setValid(true); + blobsSet.setValidity(true, true); + return returnvalue::OK; +} + +ReturnValue_t StarTrackerHandler::handleCentroidTm(const uint8_t* rawFrame) { + ReturnValue_t result = statusFieldCheck(rawFrame); + if (result != returnvalue::OK) { + return result; + } + size_t remainingLen = fullPacketLen; + PoolReadGuard pg(¢roidsSet); + result = pg.getReadResult(); + if (result != returnvalue::OK) { + return result; + } + + result = SerializeAdapter::deSerialize(¢roidSet.ticks, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(¢roidSet.timeUs, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(¢roidSet.centroidCount, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + centroidSet.setValidity(true, true); + return returnvalue::OK; +} + +ReturnValue_t StarTrackerHandler::handleCentroidsTm(const uint8_t* rawFrame) { + ReturnValue_t result = statusFieldCheck(rawFrame); + if (result != returnvalue::OK) { + return result; + } + size_t remainingLen = fullPacketLen; + PoolReadGuard pg(¢roidsSet); + result = pg.getReadResult(); + if (result != returnvalue::OK) { + return result; + } + + result = SerializeAdapter::deSerialize(¢roidsSet.ticksCentroidsTm, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(¢roidsSet.timeUsCentroidsTm, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(¢roidsSet.centroidsCount, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + for (unsigned idx = 0; idx < 16; idx++) { + result = SerializeAdapter::deSerialize(¢roidsSet.centroidsXCoords[idx], &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(¢roidsSet.centroidsYCoords[idx], &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(¢roidsSet.centroidsMagnitudes[idx], &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + } + centroidsSet.setValidity(true, true); return returnvalue::OK; } @@ -2446,6 +2575,8 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { case startracker::REQ_MATCHED_CENTROIDS: case startracker::REQ_BLOB: case startracker::REQ_BLOBS: + case startracker::REQ_CENTROID: + case startracker::REQ_CENTROIDS: if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) { return STARTRACKER_NOT_RUNNING_FIRMWARE; } diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index db8335b0..e7fdc307 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -206,7 +206,10 @@ class StarTrackerHandler : public DeviceHandlerBase { startracker::LogSubscriptionSet logSubscriptionSet; startracker::DebugCameraSet debugCameraSet; startracker::MatchedCentroidsSet matchedCentroids; + startracker::BlobSet blobSet; startracker::BlobsSet blobsSet; + startracker::CentroidSet centroidSet; + startracker::CentroidsSet centroidsSet; // Pointer to object responsible for uploading and downloading images to/from the star tracker StrComHandler* strHelper = nullptr; @@ -437,6 +440,8 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t prepareRequestMatchedCentroids(); ReturnValue_t prepareRequestBlobTm(); ReturnValue_t prepareRequestBlobsTm(); + ReturnValue_t prepareRequestCentroidTm(); + ReturnValue_t prepareRequestCentroidsTm(); ReturnValue_t prepareRequestTrackingParams(); ReturnValue_t prepareRequestValidationParams(); ReturnValue_t prepareRequestAlgoParams(); @@ -500,6 +505,8 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame); ReturnValue_t handleBlobTm(const uint8_t* rawFrame); ReturnValue_t handleBlobsTm(const uint8_t* rawFrame); + ReturnValue_t handleCentroidTm(const uint8_t* rawFrame); + ReturnValue_t handleCentroidsTm(const uint8_t* rawFrame); /** * @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 5d9c483f..8bd63512 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -294,6 +294,17 @@ enum PoolIds : lp_id_t { BLOBS_NR_4LINES_SKIPPED, BLOBS_X_COORDS, BLOBS_Y_COORDS, + + CENTROID_TICKS, + CENTROID_TIME, + CENTROID_COUNT, + + CENTROIDS_TICKS, + CENTROIDS_TIME, + CENTROIDS_COUNT, + CENTROIDS_X_COORDS, + CENTROIDS_Y_COORDS, + CENTROIDS_MAGNITUDES, }; static const DeviceCommandId_t PING_REQUEST = 0; @@ -352,6 +363,8 @@ static constexpr DeviceCommandId_t AUTO_THRESHOLD = 88; static constexpr DeviceCommandId_t REQ_MATCHED_CENTROIDS = 89; static constexpr DeviceCommandId_t REQ_BLOB = 90; static constexpr DeviceCommandId_t REQ_BLOBS = 91; +static constexpr DeviceCommandId_t REQ_CENTROID = 92; +static constexpr DeviceCommandId_t REQ_CENTROIDS = 93; static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; @@ -377,7 +390,10 @@ static const uint32_t SUBSCRIPTION_SET_ID = REQ_SUBSCRIPTION; static const uint32_t LOG_SUBSCRIPTION_SET_ID = REQ_LOG_SUBSCRIPTION; static const uint32_t DEBUG_CAMERA_SET_ID = REQ_DEBUG_CAMERA; static const uint32_t MATCHED_CENTROIDS_SET_ID = REQ_MATCHED_CENTROIDS; +static const uint32_t BLOB_SET_ID = REQ_BLOB; 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; /** Max size of unencoded frame */ static const size_t MAX_FRAME_SIZE = 1200; @@ -1436,21 +1452,23 @@ class MatchedCentroidsSet : public StaticLocalDataSet<20> { private: }; -class BlobsSet : public StaticLocalDataSet<20> { +class BlobSet : public StaticLocalDataSet<5> { public: - BlobsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOBS_SET_ID) {} - + BlobSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOB_SET_ID) {} // The blob count received from the Blob Telemetry Set (ID 25) // Ticks timestamp - lp_var_t ticksBlobTm = lp_var_t(sid.objectId, PoolIds::BLOB_TICKS, this); + lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::BLOB_TICKS, this); // Unix time stamp - lp_var_t timeUsBlobTm = lp_var_t(sid.objectId, PoolIds::BLOB_TIME, this); + lp_var_t timeUs = lp_var_t(sid.objectId, PoolIds::BLOB_TIME, this); lp_var_t blobCount = lp_var_t(sid.objectId, PoolIds::BLOB_COUNT, this); - +}; +class BlobsSet : public StaticLocalDataSet<10> { + public: + BlobsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOBS_SET_ID) {} // Ticks timestamp - lp_var_t ticksBlobsTm = lp_var_t(sid.objectId, PoolIds::BLOBS_TICKS, this); + lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::BLOBS_TICKS, this); // Unix time stamp - lp_var_t timeUsBlobsTm = lp_var_t(sid.objectId, PoolIds::BLOBS_TIME, this); + lp_var_t timeUs = lp_var_t(sid.objectId, PoolIds::BLOBS_TIME, this); lp_var_t blobsCount = lp_var_t(sid.objectId, PoolIds::BLOBS_COUNT, this); lp_var_t blobsCountUsed = lp_var_t(sid.objectId, PoolIds::BLOBS_COUNT_USED, this); @@ -1462,6 +1480,40 @@ class BlobsSet : public StaticLocalDataSet<20> { lp_vec_t(sid.objectId, PoolIds::BLOBS_Y_COORDS, this); }; +class CentroidSet : public StaticLocalDataSet<5> { + public: + CentroidSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CENTROID_SET_ID) {} + + // Data received from the Centroid Telemetry Set (ID 26) + // Ticks timestamp + lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::CENTROID_TICKS, this); + // Unix time stamp + lp_var_t timeUs = lp_var_t(sid.objectId, PoolIds::CENTROID_TIME, this); + // The centroid count received from the Centroid Telemetry Set (ID 26) + lp_var_t centroidCount = + lp_var_t(sid.objectId, PoolIds::CENTROID_COUNT, this); +}; + +class CentroidsSet : public StaticLocalDataSet<10> { + public: + CentroidsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CENTROIDS_SET_ID) {} + + // Data received from the Centroids Telemetry Set (ID 37) + lp_var_t ticksCentroidsTm = + lp_var_t(sid.objectId, PoolIds::CENTROIDS_TICKS, this); + // Unix time stamp + lp_var_t timeUsCentroidsTm = + lp_var_t(sid.objectId, PoolIds::CENTROIDS_TIME, this); + lp_var_t centroidsCount = + lp_var_t(sid.objectId, PoolIds::CENTROIDS_COUNT, this); + lp_vec_t centroidsXCoords = + lp_vec_t(sid.objectId, PoolIds::CENTROIDS_X_COORDS, this); + lp_vec_t centroidsYCoords = + lp_vec_t(sid.objectId, PoolIds::CENTROIDS_Y_COORDS, this); + lp_vec_t centroidsMagnitudes = + lp_vec_t(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this); +}; + /** * @brief Will store the requested algo parameters */