From bea918e8616487c8f9e7545d878b83cf8b667f72 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 1 Aug 2023 16:37:42 +0200 Subject: [PATCH 1/6] added fused rotation calculation to detumble and write detumble strat to dataset --- mission/controller/AcsController.cpp | 32 +++++++++++++++++----------- mission/controller/AcsController.h | 2 +- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b79fb714..a7011502 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -273,6 +273,8 @@ void AcsController::performDetumble() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, + &gyrDataProcessed, &fusedRotRateData); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && @@ -345,7 +347,7 @@ void AcsController::performDetumble() { startTransition(mode, acs::SafeSubmode::DEFAULT); } - disableCtrlValData(); + updateCtrlValData(safeCtrlStrat); updateActuatorCmdData(cmdDipoleMtqs); commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); @@ -616,6 +618,23 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque, } } +void AcsController::updateCtrlValData(uint8_t safeModeStrat) { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double)); + ctrlValData.tgtQuat.setValid(false); + std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double)); + ctrlValData.errQuat.setValid(false); + ctrlValData.errAng.value = 0; + ctrlValData.errAng.setValid(false); + std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double)); + ctrlValData.tgtRotRate.setValid(false); + ctrlValData.safeStrat.value = safeModeStrat; + ctrlValData.safeStrat.setValid(true); + ctrlValData.setValidity(true, false); + } +} + void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { @@ -646,17 +665,6 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu } } -void AcsController::disableCtrlValData() { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double)); - std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double)); - ctrlValData.errAng.value = 0; - std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double)); - ctrlValData.setValidity(false, true); - } -} - ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { // MGM Raw diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c78c7e15..a7dfbf6a 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -117,10 +117,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void updateActuatorCmdData(const int16_t* mtqTargetDipole); void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed, const int16_t* mtqTargetDipole); + void updateCtrlValData(uint8_t safeModeStrat); void updateCtrlValData(double errAng, uint8_t safeModeStrat); void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng, const double* tgtRotRate); - void disableCtrlValData(); /* ACS Sensor Values */ ACS::SensorValues sensorValues; -- 2.43.0 From 77ffc6223607e6430b564db59b528a42c40c24e6 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 1 Aug 2023 16:41:37 +0200 Subject: [PATCH 2/6] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 61ee5871..21d1e4da 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,8 @@ will consitute of a breaking change warranting a new major release: - Small SCEX fix: The temperatur check option was not passed on for commands with a user data size larger than 1. +- ACS Controller strategy is now actually written to the dataset +- During detumble the fused rotation rate is now calculated # [v6.2.0] 2023-07-26 -- 2.43.0 From 6bdafe62ad339a2d9be83b2baa53f168c90f742f Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 2 Aug 2023 10:22:15 +0200 Subject: [PATCH 3/6] thank god we never needed this before --- mission/controller/AcsController.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index a7011502..08e8760d 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -323,18 +323,18 @@ void AcsController::performDetumble() { if (acsParameters.safeModeControllerParameters.useMekf) { if (mekfData.satRotRateMekf.isValid() and VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleStart) { + acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } } else if (acsParameters.safeModeControllerParameters.useGyr) { if (gyrDataProcessed.gyrVecTot.isValid() and VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleStart) { + acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } } else if (fusedRotRateData.rotRateTotal.isValid() and VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleStart) { + acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } else if (detumbleCounter > 0) { detumbleCounter -= 1; -- 2.43.0 From c08024bd0b1a45f5fdbf541e7a1f74cf2c89e5bd Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 2 Aug 2023 10:23:02 +0200 Subject: [PATCH 4/6] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 21d1e4da..8b17753a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,7 @@ will consitute of a breaking change warranting a new major release: on for commands with a user data size larger than 1. - ACS Controller strategy is now actually written to the dataset - During detumble the fused rotation rate is now calculated +- Detumbling is now exited when its exit value is surpased and not its entry value # [v6.2.0] 2023-07-26 -- 2.43.0 From 004c60030b0d1c5c5707dac63aeb5bc0a03a91a2 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 2 Aug 2023 10:24:24 +0200 Subject: [PATCH 5/6] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8b17753a..dc278939 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,7 +20,7 @@ will consitute of a breaking change warranting a new major release: - Small SCEX fix: The temperatur check option was not passed on for commands with a user data size larger than 1. -- ACS Controller strategy is now actually written to the dataset +- ACS Controller strategy is now actually written to the dataset for detumbling - During detumble the fused rotation rate is now calculated - Detumbling is now exited when its exit value is surpased and not its entry value -- 2.43.0 From 5f82b05d3ecbf637575324d18b31570b71244b9d Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 2 Aug 2023 10:27:14 +0200 Subject: [PATCH 6/6] how many changelog commits will it be --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc278939..81b791c2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,7 +22,7 @@ will consitute of a breaking change warranting a new major release: on for commands with a user data size larger than 1. - ACS Controller strategy is now actually written to the dataset for detumbling - During detumble the fused rotation rate is now calculated -- Detumbling is now exited when its exit value is surpased and not its entry value +- Detumbling is now exited when its exit value is undercut and not its entry value # [v6.2.0] 2023-07-26 -- 2.43.0