From 026776c1ec4d149e3afb5a2803119f28f522f107 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 19 Feb 2024 14:54:17 +0100 Subject: [PATCH 01/14] desaturation cares about disabled wheels now --- mission/controller/AcsController.cpp | 40 +++++++++++----------- mission/controller/acs/control/PtgCtrl.cpp | 14 +++++--- mission/controller/acs/control/PtgCtrl.h | 3 +- 3 files changed, 32 insertions(+), 25 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8d35a8fd..1b7ac2a4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -433,10 +433,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.idleModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_TARGET: @@ -456,10 +456,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.targetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_TARGET_GS: @@ -476,10 +476,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.gsTargetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_NADIR: @@ -498,10 +498,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.nadirModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_INERTIAL: @@ -520,10 +520,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.inertialModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; default: sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 0294e8b6..446fd661 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -146,7 +146,8 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable, 4); } -void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, +void PtgCtrl::ptgDesaturation(const bool allRwAvailable, + AcsParameters::PointingLawParameters *pointingLawParameters, const double *magFieldB, const bool magFieldBValid, const double *satRate, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) { @@ -170,9 +171,14 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed // relocate RW speed zero to nullspace RW speed double refSpeedRws[4] = {0, 0, 0, 0}; - VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, - pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); - VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + if (allRwAvailable) { + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, + pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + } else if (VectorOperations::maxAbsValue(speedRws, 4) < + pointingLawParameters->nullspaceSpeed) { + return; + } // convert speed from 10 RPM to 1 RPM VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); // convert to rad/s diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 66402b0d..0935328a 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -38,7 +38,8 @@ class PtgCtrl { const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *rwTrqNs); - void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + void ptgDesaturation(const bool allRwAvabilable, + AcsParameters::PointingLawParameters *pointingLawParameters, const double *magFieldB, const bool magFieldBValid, const double *satRate, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes); From dc730bb6def93e892ce8550d2be8acbd57dd2819 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Feb 2024 11:27:31 +0100 Subject: [PATCH 02/14] stupid new set --- mission/acs/str/StarTrackerHandler.cpp | 4 ++++ mission/acs/str/strHelpers.h | 1 + 2 files changed, 5 insertions(+) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index a88014ab..25d67b72 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -638,6 +638,10 @@ 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; diff --git a/mission/acs/str/strHelpers.h b/mission/acs/str/strHelpers.h index e73a58af..009b8a47 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -395,6 +395,7 @@ 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 const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; From b8a07a3299ee141a2d1118e37482b2e9481ebbea Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Feb 2024 13:05:13 +0100 Subject: [PATCH 03/14] 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) {} From d43d1c4f65c98341f314ed83d9b06692c43cf17d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Feb 2024 13:34:55 +0100 Subject: [PATCH 04/14] almost done --- mission/acs/str/StarTrackerHandler.cpp | 12 ++++++++++-- mission/acs/str/StarTrackerHandler.h | 1 + 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 98066ee4..002b5937 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -599,8 +599,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi return returnvalue::OK; } case (startracker::REQ_BLOB_STATS): { - // TODO: Implement function. - // prepareRequestCentroidsTm(); + prepareRequestBlobStatsTm(); return returnvalue::OK; } case (startracker::BOOT): { @@ -2165,6 +2164,15 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() { return returnvalue::OK; } +ReturnValue_t StarTrackerHandler::prepareRequestBlobStatsTm() { + uint32_t length = 0; + // TODO: Wait for API update and call the API here. + // 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); diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index ab5e4540..acad969f 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -471,6 +471,7 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t prepareRequestCentroidTm(); ReturnValue_t prepareRequestCentroidsTm(); ReturnValue_t prepareRequestContrastTm(); + ReturnValue_t prepareRequestBlobStatsTm(); ReturnValue_t prepareRequestTrackingParams(); ReturnValue_t prepareRequestValidationParams(); ReturnValue_t prepareRequestAlgoParams(); From 557051ba37fcbfbbd2fe67e54e84a88dfdfcb6bc Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 10:25:02 +0100 Subject: [PATCH 05/14] detumble fixes and improvements --- mission/controller/AcsController.cpp | 22 ++++----- mission/controller/AcsController.h | 2 + mission/controller/acs/Guidance.cpp | 24 ++++++---- mission/controller/acs/Guidance.h | 3 +- mission/controller/acs/control/PtgCtrl.cpp | 46 ++++++++----------- mission/controller/acs/control/PtgCtrl.h | 17 ++++--- .../AcsCtrlDefinitions.h | 7 +++ 7 files changed, 66 insertions(+), 55 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1b77701d..066d7c18 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -396,7 +396,7 @@ void AcsController::performPointingCtrl() { bool allRwAvailable = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv, &rwAvail); if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { @@ -433,8 +433,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.idleModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; @@ -456,8 +456,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.targetModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; @@ -476,8 +476,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.gsTargetModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; @@ -498,8 +498,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.nadirModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; @@ -520,8 +520,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.inertialModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e8102f35..500542fc 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -89,6 +89,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int16_t cmdDipoleMtqs[3] = {0, 0, 0}; + acsctrl::RwAvail rwAvail; + #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4f7727f2..f9182cbd 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -327,28 +327,32 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, - double *rwPseudoInv) { - bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); - bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); - bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); - bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); + double *rwPseudoInv, acsctrl::RwAvail *rwAvail) { + rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); + rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); + rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); + rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); - if (rw1valid and rw2valid and rw3valid and rw4valid) { + if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; - } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { + } else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { + } else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { + } else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { + } else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and + not rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index a914ecfe..aa771d8d 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -44,7 +44,8 @@ class Guidance { double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle); - ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); + ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv, + acsctrl::RwAvail *rwAvail); private: const AcsParameters *acsParameters; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index f9ef3019..297230cf 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -1,11 +1,5 @@ #include "PtgCtrl.h" -#include -#include -#include -#include -#include - PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::~PtgCtrl() {} @@ -146,10 +140,10 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable, 4); } -void PtgCtrl::ptgDesaturation(const bool allRwAvailable, +void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail *rwAvail, AcsParameters::PointingLawParameters *pointingLawParameters, const double *magFieldB, const bool magFieldBValid, - const double *satRate, const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) { if (not magFieldBValid or not pointingLawParameters->desatOn) { return; @@ -163,22 +157,24 @@ void PtgCtrl::ptgDesaturation(const bool allRwAvailable, double magFieldBT[3] = {0, 0, 0}; VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); - // calculate angular momentum of the satellite - double angMomentumSat[3] = {0, 0, 0}; - MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, - angMomentumSat, 3, 3, 1); - // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed // relocate RW speed zero to nullspace RW speed double refSpeedRws[4] = {0, 0, 0, 0}; - if (allRwAvailable) { - VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, - pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); - VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); - } else if (VectorOperations::maxAbsValue(speedRws, 4) < - pointingLawParameters->nullspaceSpeed) { - return; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, + pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + if (not allRwAvailable) { + if (not rwAvail->rw1avail) { + refSpeedRws[0] = 0.0; + } else if (not rwAvail->rw2avail) { + refSpeedRws[1] = 0.0; + } else if (not rwAvail->rw3avail) { + refSpeedRws[2] = 0.0; + } else if (not rwAvail->rw4avail) { + refSpeedRws[3] = 0.0; + } } + VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + // convert speed from 10 RPM to 1 RPM VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); // convert to rad/s @@ -194,16 +190,12 @@ void PtgCtrl::ptgDesaturation(const bool allRwAvailable, // calculate total angular momentum double angMomentumTotal[3] = {0, 0, 0}; - VectorOperations::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3); - - // calculating momentum error - double deltaAngMomentum[3] = {0, 0, 0}; - VectorOperations::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef, - deltaAngMomentum, 3); + VectorOperations::add(angMomentumRw, pointingLawParameters->desatMomentumRef, + angMomentumTotal, 3); // resulting magnetic dipole command double crossAngMomentumMagField[3] = {0, 0, 0}; - VectorOperations::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField); + VectorOperations::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField); double factor = pointingLawParameters->deSatGainFactor / VectorOperations::norm(magFieldBT, 3); VectorOperations::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3); diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 0935328a..daeff4a2 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,11 +1,16 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ -#include +#include +#include +#include +#include #include #include #include -#include +#include + +#include class PtgCtrl { /* @@ -38,11 +43,11 @@ class PtgCtrl { const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *rwTrqNs); - void ptgDesaturation(const bool allRwAvabilable, + void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail, AcsParameters::PointingLawParameters *pointingLawParameters, - const double *magFieldB, const bool magFieldBValid, const double *satRate, - const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, - const int32_t speedRw3, double *mgtDpDes); + const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0, + const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, + double *mgtDpDes); /* @brief: Commands the stiction torque in case wheel speed is to low * torqueCommand modified torque after anti-stiction diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c830ffac..61489db2 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -21,6 +21,13 @@ static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3); //! [EXPORT] : [COMMENT] Multiple RWs have failed. static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4); +struct RwAvail { + bool rw1avail = false; + bool rw2avail = false; + bool rw3avail = false; + bool rw4avail = false; +}; + enum SetIds : uint32_t { MGM_SENSOR_DATA, MGM_PROCESSED_DATA, From efd17a971f6923f51b327e1528f2bbe4eb8a78e3 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 10:28:08 +0100 Subject: [PATCH 06/14] to keep it in line --- mission/controller/acs/control/PtgCtrl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 297230cf..5cbaec39 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -190,8 +190,8 @@ void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail // calculate total angular momentum double angMomentumTotal[3] = {0, 0, 0}; - VectorOperations::add(angMomentumRw, pointingLawParameters->desatMomentumRef, - angMomentumTotal, 3); + VectorOperations::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef, + angMomentumTotal, 3); // resulting magnetic dipole command double crossAngMomentumMagField[3] = {0, 0, 0}; From dbb5d6d359dbf250a20b19d289b957fa53f07543 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 10:33:25 +0100 Subject: [PATCH 07/14] small improv --- mission/controller/acs/control/PtgCtrl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 5cbaec39..a334b3a7 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -62,9 +62,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters // Inverse of gainMatrix double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; - gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; - gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; + gainMatrixInverse[0][0] = 1. / gainMatrix[0][0]; + gainMatrixInverse[1][1] = 1. / gainMatrix[1][1]; + gainMatrixInverse[2][2] = 1. / gainMatrix[2][2]; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MatrixOperations::multiply( From af6acb035ca436347f2bbd32b4ed1d7337e5ac89 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 10:57:36 +0100 Subject: [PATCH 08/14] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9153d254..64740d85 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,7 @@ will consitute of a breaking change warranting a new major release: - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode and is triggered by the `AcsController` now. - Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`. +- Fixed calculation of desaturation torque for faulty RWs. ## Changed @@ -42,6 +43,7 @@ will consitute of a breaking change warranting a new major release: also limits the rotation for the reference target quaternion to prevent spikes in required rotation rates. - Updated QUEST and Sun Vector Params to new values. +- Removed the satellites's angular momentum from desaturation calculation. ## Added From e97970ef640dffe659309e46f92bd904de137149 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Feb 2024 11:51:06 +0100 Subject: [PATCH 09/14] that should be all necessary changes --- linux/acs/StrComHandler.cpp | 16 ++++++++-------- mission/acs/str/ArcsecDatalinkLayer.cpp | 4 ++-- mission/acs/str/StarTrackerHandler.cpp | 25 +++++++++++++++++-------- mission/acs/str/strHelpers.h | 1 + thirdparty/sagittactl | 2 +- 5 files changed, 29 insertions(+), 19 deletions(-) diff --git a/linux/acs/StrComHandler.cpp b/linux/acs/StrComHandler.cpp index 9c5e045f..65a1b5a4 100644 --- a/linux/acs/StrComHandler.cpp +++ b/linux/acs/StrComHandler.cpp @@ -270,7 +270,7 @@ ReturnValue_t StrComHandler::performImageDownload() { file.close(); return returnvalue::OK; } - arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size); + prv_arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size); result = sendAndRead(size, downloadReq.position); if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { @@ -349,7 +349,7 @@ ReturnValue_t StrComHandler::performImageUpload() { } file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg); file.read(reinterpret_cast(uploadReq.data), SIZE_IMAGE_PART); - arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size); + prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size); result = sendAndRead(size, uploadReq.position); if (result != returnvalue::OK) { return returnvalue::FAILED; @@ -375,7 +375,7 @@ ReturnValue_t StrComHandler::performImageUpload() { file.seekg(fullChunks * SIZE_IMAGE_PART, file.beg); file.read(reinterpret_cast(uploadReq.data), remainder); file.close(); - arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size); + prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size); result = sendAndRead(size, uploadReq.position); if (result != returnvalue::OK) { return returnvalue::FAILED; @@ -445,7 +445,7 @@ ReturnValue_t StrComHandler::performFlashWrite() { bytesWrittenInRegion = 0; } req.address = bytesWrittenInRegion; - arc_pack_write_action_req(&req, cmdBuf.data(), &size); + prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size); result = sendAndRead(size, req.address); if (result != returnvalue::OK) { return result; @@ -488,7 +488,7 @@ ReturnValue_t StrComHandler::performFlashWrite() { req.length = remainingBytes; totalBytesWritten += CHUNK_SIZE; bytesWrittenInRegion += remainingBytes; - arc_pack_write_action_req(&req, cmdBuf.data(), &size); + prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size); result = sendAndRead(size, req.address); if (result != returnvalue::OK) { return result; @@ -536,7 +536,7 @@ ReturnValue_t StrComHandler::performFlashRead() { } else { req.length = CHUNK_SIZE; } - arc_pack_read_action_req(&req, cmdBuf.data(), &size); + prv_arc_pack_read_action_req(&req, cmdBuf.data(), &size); result = sendAndRead(size, req.address); if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { @@ -752,7 +752,7 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) { for (uint32_t idx = from; idx < to; idx++) { unlockReq.region = idx; unlockReq.code = startracker::region_secrets::SECRETS[idx]; - arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size); + prv_arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size); result = sendAndRead(size, unlockReq.region); if (result != returnvalue::OK) { return result; @@ -764,7 +764,7 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) { return result; } eraseReq.region = idx; - arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size); + prv_arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size); result = sendAndRead(size, eraseReq.region); if (result != returnvalue::OK) { } diff --git a/mission/acs/str/ArcsecDatalinkLayer.cpp b/mission/acs/str/ArcsecDatalinkLayer.cpp index c1082fc7..3a645472 100644 --- a/mission/acs/str/ArcsecDatalinkLayer.cpp +++ b/mission/acs/str/ArcsecDatalinkLayer.cpp @@ -32,7 +32,7 @@ ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedF size_t encodedDataSize = 0; slip_error_t slipError = slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx, - idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID); + idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID, 0); decodeRingBuf.deleteData(idx + 1); switch (slipError) { case (SLIP_OK): { @@ -76,7 +76,7 @@ void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); } void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame, size_t& size) { - slip_encode_frame(data, length, txEncoded, &size, ARC_DEF_SAGITTA_SLIP_ID); + slip_encode_frame(data, length, txEncoded,sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID); if (txFrame != nullptr) { *txFrame = txEncoded; } diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 002b5937..eabb2c9d 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -573,7 +573,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi Clock::getClock(&tv); setTimeRequest.unixTime = (static_cast(tv.tv_sec) * 1000 * 1000) + (static_cast(tv.tv_usec)); - arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen); + prv_arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen); size_t serLen = 0; // Time in milliseconds. Manual serialization because arcsec API ignores endianness. SerializeAdapter::serialize(&setTimeRequest.unixTime, commandBuffer + 2, &serLen, @@ -1648,6 +1648,11 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry(9)); localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry(9)); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry(16)); + poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( @@ -1924,6 +1929,10 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandI *foundId = startracker::REQ_BLOB; break; } + case (startracker::ID::BLOB_STATS): { + *foundId = startracker::REQ_BLOB_STATS; + break; + } case (startracker::ID::BLOBS): { *foundId = startracker::REQ_BLOBS; break; @@ -2008,7 +2017,7 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) { uint32_t length = 0; struct BootActionRequest bootRequest = {static_cast(target)}; - arc_pack_boot_action_req(&bootRequest, commandBuffer, &length); + prv_arc_pack_boot_action_req(&bootRequest, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; } @@ -2041,7 +2050,7 @@ ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandD return result; } uint32_t rawCmdLength = 0; - arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength); + prv_arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength); rawPacket = commandBuffer; rawPacketLen = rawCmdLength; checksumCmd.rememberRegion = req.region; @@ -2060,7 +2069,7 @@ void StarTrackerHandler::prepareTimeRequest() { void StarTrackerHandler::preparePingRequest() { uint32_t length = 0; struct PingActionRequest pingRequest = {PING_ID}; - arc_pack_ping_action_req(&pingRequest, commandBuffer, &length); + prv_arc_pack_ping_action_req(&pingRequest, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; } @@ -2089,7 +2098,7 @@ void StarTrackerHandler::preparePowerRequest() { void StarTrackerHandler::prepareSwitchToBootloaderCmd() { uint32_t length = 0; struct RebootActionRequest rebootReq {}; - arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length); + prv_arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; } @@ -2098,7 +2107,7 @@ void StarTrackerHandler::prepareTakeImageCommand(const uint8_t* commandData) { uint32_t length = 0; struct CameraActionRequest camReq; camReq.actionid = *commandData; - arc_pack_camera_action_req(&camReq, commandBuffer, &length); + prv_arc_pack_camera_action_req(&camReq, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; } @@ -2166,8 +2175,7 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() { ReturnValue_t StarTrackerHandler::prepareRequestBlobStatsTm() { uint32_t length = 0; - // TODO: Wait for API update and call the API here. - // arc_tm_pack_centroid_req(commandBuffer, &length); + arc_tm_pack_blobstats_req(commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; return returnvalue::OK; @@ -2922,6 +2930,7 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { case startracker::REQ_MATCHED_CENTROIDS: case startracker::REQ_BLOB: case startracker::REQ_BLOBS: + case startracker::REQ_BLOB_STATS: case startracker::REQ_CENTROID: case startracker::REQ_CENTROIDS: case startracker::REQ_CONTRAST: { diff --git a/mission/acs/str/strHelpers.h b/mission/acs/str/strHelpers.h index 4ec6af5a..9d502d3b 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -500,6 +500,7 @@ static constexpr uint8_t CENTROID = 26; static constexpr uint8_t CENTROIDS = 37; static constexpr uint8_t AUTO_BLOB = 39; static constexpr uint8_t MATCHED_CENTROIDS = 40; +static constexpr uint8_t BLOB_STATS = 49; } // namespace ID namespace Program { diff --git a/thirdparty/sagittactl b/thirdparty/sagittactl index 20f32a2f..0c5fb464 160000 --- a/thirdparty/sagittactl +++ b/thirdparty/sagittactl @@ -1 +1 @@ -Subproject commit 20f32a2f291d00869c693066de02c0257dec7be4 +Subproject commit 0c5fb46405d122caf4d46beab7cb8ef19317a1ee From 6609ac85e377612969be5e0ef177bb1805fe958b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Feb 2024 12:07:33 +0100 Subject: [PATCH 10/14] bugfix and tmtc bump --- mission/acs/str/ArcsecDatalinkLayer.cpp | 2 +- mission/acs/str/StarTrackerHandler.cpp | 2 ++ mission/acs/str/strHelpers.h | 4 ++++ tmtc | 2 +- 4 files changed, 8 insertions(+), 2 deletions(-) diff --git a/mission/acs/str/ArcsecDatalinkLayer.cpp b/mission/acs/str/ArcsecDatalinkLayer.cpp index 3a645472..9d5d26e3 100644 --- a/mission/acs/str/ArcsecDatalinkLayer.cpp +++ b/mission/acs/str/ArcsecDatalinkLayer.cpp @@ -76,7 +76,7 @@ void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); } void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame, size_t& size) { - slip_encode_frame(data, length, txEncoded,sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID); + slip_encode_frame(data, length, txEncoded, sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID); if (txFrame != nullptr) { *txFrame = txEncoded; } diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index eabb2c9d..11c719c1 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -1648,6 +1648,8 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry(9)); localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry(9)); + localDataPoolMap.emplace(startracker::TICKS_BLOB_STATS, new PoolEntry()); + localDataPoolMap.emplace(startracker::TIME_BLOB_STATS, new PoolEntry()); localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry(16)); localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry(16)); localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry(16)); diff --git a/mission/acs/str/strHelpers.h b/mission/acs/str/strHelpers.h index 9d502d3b..ebbc80c9 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -329,6 +329,8 @@ enum PoolIds : lp_id_t { CONTRAST_C, CONTRAST_D, + TICKS_BLOB_STATS, + TIME_BLOB_STATS, BLOB_STATS_NOISE, BLOB_STATS_THOLD, BLOB_STATS_LVALID, @@ -1572,6 +1574,8 @@ class BlobStatsSet : public StaticLocalDataSet<6> { BlobStatsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOB_STATS_SET_ID) {} // Data received from the Centroids Telemetry Set (ID 49) + lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::TICKS_BLOB_STATS, this); + lp_var_t time = lp_var_t(sid.objectId, PoolIds::TIME_BLOB_STATS, this); lp_vec_t noise = lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_NOISE, this); lp_vec_t thold = diff --git a/tmtc b/tmtc index 588d7a80..68ea889d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 588d7a8079194c6c51d6ecafd4c940cb1bf0554c +Subproject commit 68ea889d0f01540372a366c6fd64eed010ef8355 From e77e403e382e348fb89c213b2bb7dbb9411fc1e1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Feb 2024 12:14:47 +0100 Subject: [PATCH 11/14] STR bugfix --- mission/acs/str/StarTrackerHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 11c719c1..d8409e21 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -1653,7 +1653,7 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry(16)); localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry(16)); localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry(16)); - localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_OFLOW, new PoolEntry(16)); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0)); From 6d5f86ff778e62966a7a813e4f79996b2f4a91a9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Feb 2024 12:27:39 +0100 Subject: [PATCH 12/14] bump tmtc again --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 68ea889d..0201eb27 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 68ea889d0f01540372a366c6fd64eed010ef8355 +Subproject commit 0201eb27c48c247a4c17e06da67f6b3907eefbc8 From d0f8b7981f26493107da7745d0840a0edbeb6d56 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Feb 2024 12:31:22 +0100 Subject: [PATCH 13/14] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9153d254..31aeec04 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -42,10 +42,12 @@ will consitute of a breaking change warranting a new major release: also limits the rotation for the reference target quaternion to prevent spikes in required rotation rates. - Updated QUEST and Sun Vector Params to new values. +- Bumped internal `sagittactl` library to v11.11. ## Added - Updated STR handler to unlock and allow using the secondary firmware slot. +- STR handling for new BlobStats TM set. # [v7.6.1] 2024-02-05 From 9cc991e600b9d2d8aa954b7d902f646fb9853625 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 12:40:17 +0100 Subject: [PATCH 14/14] bumped sagittactl and tmtc --- thirdparty/sagittactl | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/thirdparty/sagittactl b/thirdparty/sagittactl index 0c5fb464..a5586936 160000 --- a/thirdparty/sagittactl +++ b/thirdparty/sagittactl @@ -1 +1 @@ -Subproject commit 0c5fb46405d122caf4d46beab7cb8ef19317a1ee +Subproject commit a558693675dae09aff7ef74242c25f826584c46a diff --git a/tmtc b/tmtc index 0201eb27..6ade001d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 0201eb27c48c247a4c17e06da67f6b3907eefbc8 +Subproject commit 6ade001d3d3b3b9c0d2838aa94c3112aaf486a61