|
|
@ -747,7 +747,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 10.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 60.0});
|
|
|
|
// MGM Processed
|
|
|
|
// MGM Processed
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
|
|
|
@ -757,7 +757,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 10.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 60.0});
|
|
|
|
// SUS Raw
|
|
|
|
// SUS Raw
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
|
|
|
@ -771,7 +771,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 10.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 60.0});
|
|
|
|
// SUS Processed
|
|
|
|
// SUS Processed
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
|
|
|
@ -788,20 +788,20 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 10.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 60.0});
|
|
|
|
// GYR Raw
|
|
|
|
// GYR Raw
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw);
|
|
|
|
poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 10.0});
|
|
|
|
poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 60.0});
|
|
|
|
// GYR Processed
|
|
|
|
// GYR Processed
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot);
|
|
|
|
poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 10.0});
|
|
|
|
poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 60.0});
|
|
|
|
// GPS Processed
|
|
|
|
// GPS Processed
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
|
|
|
@ -809,38 +809,38 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 60.0});
|
|
|
|
// Attitude Estimation
|
|
|
|
// Attitude Estimation
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
|
|
|
|
poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0});
|
|
|
|
poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 30.0});
|
|
|
|
// Ctrl Values
|
|
|
|
// Ctrl Values
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 10.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 30.0});
|
|
|
|
// Actuator CMD
|
|
|
|
// Actuator CMD
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 30.0});
|
|
|
|
// Fused Rot Rate
|
|
|
|
// Fused Rot Rate
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 30.0});
|
|
|
|
// Fused Rot Rate Sources
|
|
|
|
// Fused Rot Rate Sources
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
|
|
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 60.0});
|
|
|
|
return returnvalue::OK;
|
|
|
|
return returnvalue::OK;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
@ -926,6 +926,17 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
|
|
|
|
if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::DETUMBLE) {
|
|
|
|
if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::DETUMBLE) {
|
|
|
|
detumbleState = DetumbleState::NO_DETUMBLE;
|
|
|
|
detumbleState = DetumbleState::NO_DETUMBLE;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (mode > acs::AcsMode::PTG_IDLE) {
|
|
|
|
|
|
|
|
poolManager.changeCollectionInterval(ctrlValData.getSid(), 10);
|
|
|
|
|
|
|
|
poolManager.changeCollectionInterval(actuatorCmdData.getSid(), 10);
|
|
|
|
|
|
|
|
poolManager.changeCollectionInterval(fusedRotRateData.getSid(), 10);
|
|
|
|
|
|
|
|
poolManager.changeCollectionInterval(attitudeEstimationData.getSid(), 10);
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
|
|
|
poolManager.changeCollectionInterval(ctrlValData.getSid(), 30);
|
|
|
|
|
|
|
|
poolManager.changeCollectionInterval(actuatorCmdData.getSid(), 30);
|
|
|
|
|
|
|
|
poolManager.changeCollectionInterval(fusedRotRateData.getSid(), 30);
|
|
|
|
|
|
|
|
poolManager.changeCollectionInterval(attitudeEstimationData.getSid(), 30);
|
|
|
|
|
|
|
|
}
|
|
|
|
return ExtendedControllerBase::modeChanged(mode, submode);
|
|
|
|
return ExtendedControllerBase::modeChanged(mode, submode);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|