From 6a6789ae99ecfe95be7581fdb7a368c70e1b0625 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 2 Oct 2023 16:11:17 +0200 Subject: [PATCH] STR extensions --- mission/acs/str/StarTrackerHandler.cpp | 124 +++++++++++++++++++++++-- mission/acs/str/StarTrackerHandler.h | 7 ++ mission/acs/str/strHelpers.h | 44 ++++++++- tmtc | 2 +- 4 files changed, 164 insertions(+), 13 deletions(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index a6091d7c..fb895d78 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -607,6 +607,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi result = prepareRequestLisaParams(); return result; } + case (startracker::REQ_MATCHED_CENTROIDS): { + result = prepareRequestMatchedCentroids(); + return result; + } case (startracker::REQ_MATCHING): { result = prepareRequestMatchingParams(); return result; @@ -729,6 +733,8 @@ 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, + startracker::MAX_FRAME_SIZE * 2 + 2); } ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) { @@ -860,6 +866,16 @@ void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonF JCFG_DONE = true; } +ReturnValue_t StarTrackerHandler::statusFieldCheck(const uint8_t* rawFrame) { + uint8_t status = startracker::getStatusField(rawFrame); + if (status != startracker::STATUS_OK) { + sif::warning << "StarTrackerHandler::handleTm: Reply error: " + << static_cast(status) << std::endl; + return REPLY_ERROR; + } + return returnvalue::OK; +} + void StarTrackerHandler::bootBootloader() { if (internalState == InternalState::IDLE) { internalState = InternalState::BOOT_BOOTLOADER; @@ -896,24 +912,24 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t rema } case TMTC_ACTIONREPLY: { *foundLen = remainingSize; + fullPacketLen = remainingSize; return scanForActionReply(startracker::getId(start), foundId); - break; } case TMTC_SETPARAMREPLY: { *foundLen = remainingSize; + fullPacketLen = remainingSize; return scanForSetParameterReply(startracker::getId(start), foundId); - break; } case TMTC_PARAMREPLY: { *foundLen = remainingSize; + fullPacketLen = remainingSize; return scanForGetParameterReply(startracker::getId(start), foundId); - break; } case TMTC_TELEMETRYREPLYA: case TMTC_TELEMETRYREPLY: { *foundLen = remainingSize; + fullPacketLen = remainingSize; return scanForTmReply(startracker::getId(start), foundId); - break; } default: { sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl; @@ -970,6 +986,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, result = handleTm(packet, solutionSet, startracker::SolutionSet::SIZE, "REQ_SOLUTION"); break; } + case (startracker::REQ_MATCHED_CENTROIDS): { + result = handleMatchedCentroidTm(packet); + break; + } case (startracker::REQ_TEMPERATURE): { result = handleTm(packet, temperatureSet, startracker::TemperatureSet::SIZE, "REQ_TEMP"); break; @@ -1032,6 +1052,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, handleParamRequest(packet, trackingSet, startracker::TrackingSet::SIZE); break; } + case (startracker::REQ_MATCHED_CENTROIDS): { + handleParamRequest(packet, matchedCentroids, startracker::MatchedCentroidsSet::SIZE); + break; + } case (startracker::REQ_VALIDATION): { handleParamRequest(packet, validationSet, startracker::ValidationSet::SIZE); break; @@ -1328,6 +1352,20 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(startracker::DEBUG_CAMERA_TEST, new PoolEntry({0})); localDataPoolMap.emplace(startracker::CHKSUM, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::PoolIds::NUM_MATCHED_CENTROIDS, new PoolEntry()); + localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_STAR_IDS, + new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_X_COORDS, + new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_Y_COORDS, + new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_X_ERRORS, + new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_Y_ERRORS, + new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::TICKS_MATCHED_CENTROIDS, + new PoolEntry()); + localDataPoolMap.emplace(startracker::PoolIds::TIME_MATCHED_CENTROIDS, new PoolEntry()); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0)); @@ -1760,6 +1798,14 @@ void StarTrackerHandler::prepareHistogramRequest() { rawPacketLen = length; } +ReturnValue_t StarTrackerHandler::prepareRequestMatchedCentroids() { + uint32_t length = 0; + arc_tm_pack_matchedcentroids_req(commandBuffer, &length); + rawPacket = commandBuffer; + rawPacketLen = length; + return returnvalue::OK; +} + ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData, size_t commandDataLen, ArcsecJsonParamBase& paramSet, @@ -2039,12 +2085,9 @@ ReturnValue_t StarTrackerHandler::checkProgram() { ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size, const char* context) { - ReturnValue_t result = returnvalue::OK; - uint8_t status = startracker::getStatusField(rawFrame); - if (status != startracker::STATUS_OK) { - sif::warning << "StarTrackerHandler::handleTm: Reply error: " - << static_cast(status) << std::endl; - return REPLY_ERROR; + ReturnValue_t result = statusFieldCheck(rawFrame); + if (result != returnvalue::OK) { + return result; } result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); if (result != returnvalue::OK) { @@ -2069,6 +2112,66 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat return result; } +ReturnValue_t StarTrackerHandler::handleMatchedCentroidTm(const uint8_t* rawFrame) { + ReturnValue_t result = statusFieldCheck(rawFrame); + if (result != returnvalue::OK) { + return result; + } + const uint8_t* reply = rawFrame + TICKS_OFFSET; + size_t remainingLen = fullPacketLen; + PoolReadGuard pg(&matchedCentroids); + result = pg.getReadResult(); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&matchedCentroids.ticks, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&matchedCentroids.timeUs, &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&matchedCentroids.numberOfMatchedCentroids, &rawFrame, + &remainingLen, SerializeIF::Endianness::LITTLE); + + if (result != returnvalue::OK) { + return result; + } + // 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); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&matchedCentroids.xCoords[idx], &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&matchedCentroids.yCoords[idx], &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&matchedCentroids.xErrors[idx], &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + result = SerializeAdapter::deSerialize(&matchedCentroids.yErrors[idx], &rawFrame, &remainingLen, + SerializeIF::Endianness::LITTLE); + if (result != returnvalue::OK) { + return result; + } + } + matchedCentroids.setValidity(true, true); + return returnvalue::OK; +} + ReturnValue_t StarTrackerHandler::handleActionReplySet(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size) { ReturnValue_t result = returnvalue::OK; @@ -2204,6 +2307,7 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { case startracker::REQ_SUBSCRIPTION: case startracker::REQ_LOG_SUBSCRIPTION: case startracker::REQ_DEBUG_CAMERA: + case startracker::REQ_MATCHED_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 1e790695..723a8aa8 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -204,6 +204,7 @@ class StarTrackerHandler : public DeviceHandlerBase { startracker::SubscriptionSet subscriptionSet; startracker::LogSubscriptionSet logSubscriptionSet; startracker::DebugCameraSet debugCameraSet; + startracker::MatchedCentroidsSet matchedCentroids; // Pointer to object responsible for uploading and downloading images to/from the star tracker StrComHandler* strHelper = nullptr; @@ -297,6 +298,8 @@ class StarTrackerHandler : public DeviceHandlerBase { const power::Switch_t powerSwitch = power::NO_SWITCH; + size_t fullPacketLen = 0; + /** * @brief Handles internal state */ @@ -429,6 +432,7 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t prepareRequestCentroidingParams(); ReturnValue_t prepareRequestLisaParams(); ReturnValue_t prepareRequestMatchingParams(); + ReturnValue_t prepareRequestMatchedCentroids(); ReturnValue_t prepareRequestTrackingParams(); ReturnValue_t prepareRequestValidationParams(); ReturnValue_t prepareRequestAlgoParams(); @@ -477,6 +481,7 @@ class StarTrackerHandler : public DeviceHandlerBase { */ void handleStartup(uint8_t tmType, uint8_t parameterId); + ReturnValue_t statusFieldCheck(const uint8_t* rawFrame); /** * @brief Handles telemtry replies and fills the appropriate dataset * @@ -488,6 +493,8 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size, const char* context); + ReturnValue_t handleMatchedCentroidTm(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 c6336186..b6a64f29 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -272,7 +272,16 @@ enum PoolIds : lp_id_t { LOG_SUBSCRIPTION_LEVEL2, LOG_SUBSCRIPTION_MODULE2, DEBUG_CAMERA_TIMING, - DEBUG_CAMERA_TEST + DEBUG_CAMERA_TEST, + + TICKS_MATCHED_CENTROIDS, + TIME_MATCHED_CENTROIDS, + NUM_MATCHED_CENTROIDS, + MATCHED_CENTROIDS_STAR_IDS, + MATCHED_CENTROIDS_X_COORDS, + MATCHED_CENTROIDS_Y_COORDS, + MATCHED_CENTROIDS_X_ERRORS, + MATCHED_CENTROIDS_Y_ERRORS, }; static const DeviceCommandId_t PING_REQUEST = 0; @@ -328,6 +337,7 @@ static const DeviceCommandId_t DISABLE_TIMESTAMP_GENERATION = 85; static const DeviceCommandId_t ENABLE_TIMESTAMP_GENERATION = 86; static constexpr DeviceCommandId_t SET_TIME_FROM_SYS_TIME = 87; static constexpr DeviceCommandId_t AUTO_THRESHOLD = 88; +static constexpr DeviceCommandId_t REQ_MATCHED_CENTROIDS = 89; static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; @@ -352,6 +362,7 @@ static const uint32_t ALGO_SET_ID = REQ_ALGO; 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; /** Max size of unencoded frame */ static const size_t MAX_FRAME_SIZE = 1200; @@ -666,7 +677,7 @@ class SolutionSet : public StaticLocalDataSet { // Ticks timestamp lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::TICKS_SOLUTION_SET, this); - /// Unix time stamp + // Unix time stamp lp_var_t timeUs = lp_var_t(sid.objectId, PoolIds::TIME_SOLUTION_SET, this); // Calibrated quaternion (takes into account the mounting quaternion), typically same as // track q values @@ -1381,6 +1392,35 @@ class ValidationSet : public StaticLocalDataSet { } }; +class MatchedCentroidsSet : public StaticLocalDataSet<20> { + public: + static constexpr size_t SIZE = 321; + MatchedCentroidsSet(HasLocalDataPoolIF* owner) + : StaticLocalDataSet(owner, MATCHED_CENTROIDS_SET_ID) {} + MatchedCentroidsSet(object_id_t objectId) + : StaticLocalDataSet(sid_t(objectId, MATCHED_CENTROIDS_SET_ID)) {} + // Ticks timestamp + lp_var_t ticks = + lp_var_t(sid.objectId, PoolIds::TICKS_MATCHED_CENTROIDS, this); + // Unix time stamp + lp_var_t timeUs = + lp_var_t(sid.objectId, PoolIds::TIME_MATCHED_CENTROIDS, this); + lp_var_t numberOfMatchedCentroids = + lp_var_t(sid.objectId, PoolIds::NUM_MATCHED_CENTROIDS, this); + lp_vec_t starIds = + lp_vec_t(sid.objectId, PoolIds::MATCHED_CENTROIDS_STAR_IDS); + lp_vec_t xCoords = + lp_vec_t(sid.objectId, PoolIds::MATCHED_CENTROIDS_X_COORDS); + lp_vec_t yCoords = + lp_vec_t(sid.objectId, PoolIds::MATCHED_CENTROIDS_Y_COORDS); + lp_vec_t xErrors = + lp_vec_t(sid.objectId, PoolIds::MATCHED_CENTROIDS_X_ERRORS); + lp_vec_t yErrors = + lp_vec_t(sid.objectId, PoolIds::MATCHED_CENTROIDS_Y_ERRORS); + + private: +}; + /** * @brief Will store the requested algo parameters */ diff --git a/tmtc b/tmtc index 22a42108..783bdd29 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 22a42108b431bafa707de2b3a24cad9de95b70d0 +Subproject commit 783bdd297a931a96c8b077ec2c2456a203b23ffc