diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8c92d968..2b0689c7 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -5,11 +5,11 @@ AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId, objects::NO_OBJECT), - /*sensorProcessing(&acsParameters), - navigation(&acsParameters), - actuatorCmd(&acsParameters), - detumble(&acsParameters), - detumbleCounter{0},*/ + sensorProcessing(&acsParameters), + navigation(&acsParameters), + actuatorCmd(&acsParameters), + detumble(&acsParameters), + detumbleCounter{0}, mgmData(this), susData(this) {} @@ -64,14 +64,16 @@ void AcsController::performControlOperation() { copySusData(); } } + sif::debug << susData.sus0.value[0] << "," << susData.sus0.value[1] << "," + << susData.sus0.value[2] << "," << susData.sus0.value[3] << "," + << susData.sus0.value[4] << "," << susData.sus0.value[5] << std::endl; + sif::debug << susData.sus0.isValid() << std::endl; } -void AcsController::performSafe(){ - -} +void AcsController::performSafe() {} void AcsController::performDetumble() { - /*ACS::SensorValues sensorValues; + ACS::SensorValues sensorValues; ACS::OutputValues outputValues; // sensorValues.read(); @@ -104,12 +106,10 @@ void AcsController::performDetumble() { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { submode = SUBMODE_SAFE; detumbleCounter = 0; - }*/ + } } -void AcsController::performPointingCtrl(){ - -} +void AcsController::performPointingCtrl() {} ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { @@ -120,6 +120,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD 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); + poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); // SUS localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec); @@ -133,8 +134,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec); - - poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); + poolManager.subscribeForRegularPeriodicPacket({susData.getSid(), false, 5.0}); return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 054f2b8f..fa4ba88e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -33,13 +33,13 @@ class AcsController : public ExtendedControllerBase { void performPointingCtrl(); private: - /*AcsParameters acsParameters; + AcsParameters acsParameters; SensorProcessing sensorProcessing; Navigation navigation; ActuatorCmd actuatorCmd; Detumble detumble; - uint8_t detumbleCounter;*/ + uint8_t detumbleCounter; enum class InternalState { STARTUP, INITIAL_DELAY, READY };