diff --git a/fsfw b/fsfw index 7f89022f..f4c4f994 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 7f89022f5ba90d48abd8ba5508e3d6840ce47d74 +Subproject commit f4c4f9946c7fb9fd050178baa4865da654366b70 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8894c59a..c7309097 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,3 +1,4 @@ +#include #include "AcsController.h" AcsController::AcsController(object_id_t objectId) @@ -28,10 +29,20 @@ void AcsController::performControlOperation() { break; } + if (mgmData.read() == RETURN_OK) { + copyMgmData(); + mgmData.commit(); + } } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); return HasReturnvaluesIF::RETURN_OK; } @@ -43,4 +54,35 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, } void AcsController::copyMgmData() { + { + PoolReadGuard pg(&mgm0Lis3Set); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float)); + } + } + { + PoolReadGuard pg(&mgm1Rm3100Set); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); + } + } + { + PoolReadGuard pg(&mgm2Lis3Set); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float)); + } + } + { + PoolReadGuard pg(&mgm3Rm3100Set); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); + } + } + { + PoolReadGuard pg(&imtqMgmSet); + if(pg.getReadResult() == RETURN_OK) { + std::memcpy(mgmData.imtqCal.value, imtqMgmSet.mgmXyz.value, 3 * sizeof(int32_t)); + mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value; + } + } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index f73cb9a6..35503c48 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -36,9 +36,15 @@ class AcsController : public ExtendedControllerBase { MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm2Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); + RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); IMTQ::CalibratedMtmMeasurementSet imtqMgmSet = IMTQ::CalibratedMtmMeasurementSet(objects::IMTQ_HANDLER); + PoolEntry mgm0PoolVec = PoolEntry(3); + PoolEntry mgm1PoolVec = PoolEntry(3); + PoolEntry mgm2PoolVec = PoolEntry(3); + PoolEntry mgm3PoolVec = PoolEntry(3); + PoolEntry imtqMgmPoolVec = PoolEntry(3); + PoolEntry imtqCalActStatus = PoolEntry(); void copyMgmData(); // Initial delay to make sure all pool variables have been initialized their owners