#include "AcsController.h" #include #include "mission/acsDefs.h" #include "mission/config/torquer.h" AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), sensorProcessing(&acsParameters), navigation(&acsParameters), actuatorCmd(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), detumble(&acsParameters), ptgCtrl(&acsParameters), detumbleCounter{0}, parameterHelper(this), mgmDataRaw(this), mgmDataProcessed(this), susDataRaw(this), susDataProcessed(this), gyrDataRaw(this), gyrDataProcessed(this), gpsDataProcessed(this), mekfData(this), ctrlValData(this), actuatorCmdData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t result = actionHelper.handleActionMessage(message); if (result == returnvalue::OK) { return result; } result = parameterHelper.handleParameterMessage(message); if (result == returnvalue::OK) { return result; } return result; } MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); } ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId, ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues, uint16_t startAtIndex) { return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues, startAtIndex); } void AcsController::performControlOperation() { #if OBSW_THREAD_TRACING == 1 trace::threadTrace(opCounter, "ACS & TCS PST"); #endif switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); internalState = InternalState::INITIAL_DELAY; return; } case InternalState::INITIAL_DELAY: { if (initialCountdown.hasTimedOut()) { internalState = InternalState::READY; } return; } case InternalState::READY: { if (mode != MODE_OFF) { switch (submode) { case acs::SAFE: performSafe(); break; case acs::DETUMBLE: performDetumble(); break; case acs::PTG_IDLE: case acs::PTG_TARGET: case acs::PTG_TARGET_GS: case acs::PTG_NADIR: case acs::PTG_INERTIAL: performPointingCtrl(); break; } } break; } default: break; } { PoolReadGuard pg(&mgmDataRaw); if (pg.getReadResult() == returnvalue::OK) { copyMgmData(); } } { PoolReadGuard pg(&susDataRaw); if (pg.getReadResult() == returnvalue::OK) { copySusData(); } } { PoolReadGuard pg(&gyrDataRaw); if (pg.getReadResult() == returnvalue::OK) { copyGyrData(); } } } void AcsController::performSafe() { timeval now; Clock::getClock_timeval(&now); sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &validMekf); // Give desired satellite rate and sun direction to align double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); // IF MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; if (validMekf == returnvalue::OK) { safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } else { safeCtrl.safeNoMekf( now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } double dipolCmdUnits[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { double unitQuat[4] = {0, 0, 0, 1}; std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); ctrlValData.tgtQuat.setValid(false); std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); ctrlValData.errQuat.setValid(false); ctrlValData.errAng.value = errAng; ctrlValData.errAng.setValid(true); ctrlValData.setValidity(true, false); } } // Detumble check and switch if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (gyrDataProcessed.gyrVecTot.isValid() && VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else { detumbleCounter = 0; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; // Triggers detumble mode transition in subsystem triggerEvent(acs::SAFE_RATE_VIOLATION); } { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { int32_t zeroVec[4] = {0, 0, 0, 0}; std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetTorque.setValid(false); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } } // { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], // torqueDuration); // } } void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &validMekf); double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); double dipolCmdUnits[3] = {0, 0, 0}; actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } else if (gyrDataProcessed.gyrVecTot.isValid() && VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } else { detumbleCounter = 0; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; // Triggers safe mode transition in subsystem triggerEvent(acs::SAFE_RATE_RECOVERY); } int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; for (int i = 0; i < 3; ++i) { cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); } { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); actuatorCmdData.rwTargetTorque.setValid(false); std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } } // { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // torqueDuration); // } } void AcsController::performPointingCtrl() { timeval now; Clock::getClock_timeval(&now); sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &validMekf); double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 0}; uint8_t enableAntiStiction = true; double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol switch (submode) { case acs::PTG_IDLE: guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; case acs::PTG_TARGET: guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; case acs::PTG_TARGET_GS: guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; case acs::PTG_NADIR: guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; case acs::PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; } if (enableAntiStiction) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? int32_t rwSpeed[4] = { (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); } double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; for (int i = 0; i < 3; ++i) { cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); } int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; for (int i = 0; i < 4; ++i) { cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); } { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); actuatorCmdData.setValidity(true, true); } } // { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // torqueDuration); // } } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { // MGM Raw localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw); 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_ACT_STATUS, &imtqCalActStatus); poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0}); // MGM Processed localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer); localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf); poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0}); // SUS Raw 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_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw); 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_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw); poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0}); // SUS Processed localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer); localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk); poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0}); // GYR Raw localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw); localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw); localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw); localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw); poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 5.0}); // GYR Processed localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc); localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot); poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0}); // GPS Processed localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); // MEKF localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0}); // Ctrl Values localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); // Actuator CMD localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0}); return returnvalue::OK; } LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { switch (sid.ownerSetId) { case acsctrl::MGM_SENSOR_DATA: return &mgmDataRaw; case acsctrl::MGM_PROCESSED_DATA: return &mgmDataProcessed; case acsctrl::SUS_SENSOR_DATA: return &susDataRaw; case acsctrl::SUS_PROCESSED_DATA: return &susDataProcessed; case acsctrl::GYR_SENSOR_DATA: return &gyrDataRaw; case acsctrl::GYR_PROCESSED_DATA: return &gyrDataProcessed; case acsctrl::GPS_PROCESSED_DATA: return &gpsDataProcessed; case acsctrl::MEKF_DATA: return &mekfData; case acsctrl::CTRL_VAL_DATA: return &ctrlValData; case acsctrl::ACTUATOR_CMD_DATA: return &actuatorCmdData; default: return nullptr; } return nullptr; } ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) { if (mode == MODE_OFF) { if (submode == SUBMODE_NONE) { return returnvalue::OK; } else { return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_INERTIAL)) { return INVALID_SUBMODE; } else { return returnvalue::OK; } } return INVALID_MODE; } void AcsController::modeChanged(Mode_t mode, Submode_t submode) { return ExtendedControllerBase::modeChanged(mode, submode); } void AcsController::announceMode(bool recursive) { const char *modeStr = "UNKNOWN"; if (mode == HasModesIF::MODE_OFF) { modeStr = "OFF"; } else if (mode == HasModesIF::MODE_ON) { modeStr = "ON"; } else if (mode == DeviceHandlerIF::MODE_NORMAL) { modeStr = "NORMAL"; } const char *submodeStr = acs::getModeStr(static_cast(submode)); sif::info << "ACS controller is now in " << modeStr << " mode with " << submodeStr << " submode" << std::endl; return ExtendedControllerBase::announceMode(recursive); } void AcsController::copyMgmData() { ACS::SensorValues sensorValues; { PoolReadGuard pg(&sensorValues.mgm0Lis3Set); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float)); mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); } } { PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); } } { PoolReadGuard pg(&sensorValues.mgm2Lis3Set); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float)); mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); } } { PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); } } { PoolReadGuard pg(&sensorValues.imtqMgmSet); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value, 3 * sizeof(float)); mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid()); } } } void AcsController::copySusData() { { PoolReadGuard pg(&sensorValues.susSets[0]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[1]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[2]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[3]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[4]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[5]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[6]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[7]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[8]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[9]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[10]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); } } { PoolReadGuard pg(&sensorValues.susSets[11]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value, 6 * sizeof(uint16_t)); susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid()); } } } void AcsController::copyGyrData() { ACS::SensorValues sensorValues; { PoolReadGuard pg(&sensorValues.gyr0AdisSet); if (pg.getReadResult() == returnvalue::OK) { gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value; gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value; gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value; gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() && sensorValues.gyr0AdisSet.angVelocY.isValid() && sensorValues.gyr0AdisSet.angVelocZ.isValid()); } } { PoolReadGuard pg(&sensorValues.gyr1L3gSet); if (pg.getReadResult() == returnvalue::OK) { gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value; gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value; gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value; gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() && sensorValues.gyr1L3gSet.angVelocY.isValid() && sensorValues.gyr1L3gSet.angVelocZ.isValid()); } } { PoolReadGuard pg(&sensorValues.gyr2AdisSet); if (pg.getReadResult() == returnvalue::OK) { gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value; gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value; gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value; gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() && sensorValues.gyr2AdisSet.angVelocY.isValid() && sensorValues.gyr2AdisSet.angVelocZ.isValid()); } } { PoolReadGuard pg(&sensorValues.gyr3L3gSet); if (pg.getReadResult() == returnvalue::OK) { gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value; gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value; gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value; gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() && sensorValues.gyr3L3gSet.angVelocY.isValid() && sensorValues.gyr3L3gSet.angVelocZ.isValid()); } } } ReturnValue_t AcsController::initialize() { ReturnValue_t result = parameterHelper.initialize(); if (result != returnvalue::OK) { return result; } return ExtendedControllerBase::initialize(); }