#include "AcsController.h" AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF &sdcMan) : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), sdcMan(sdcMan), attitudeEstimation(&acsParameters), fusedRotationEstimation(&acsParameters), navigation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), ptgCtrl(&acsParameters), parameterHelper(this), mgmDataRaw(this), mgmDataProcessed(this), susDataRaw(this), susDataProcessed(this), gyrDataRaw(this), gyrDataProcessed(this), gpsDataProcessed(this), attitudeEstimationData(this), ctrlValData(this), actuatorCmdData(this), fusedRotRateData(this), fusedRotRateSourcesData(this) {} ReturnValue_t AcsController::initialize() { ReturnValue_t result = parameterHelper.initialize(); if (result != returnvalue::OK) { return result; } return ExtendedControllerBase::initialize(); } 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; } ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t *data, size_t size) { switch (actionId) { case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: { ReturnValue_t result = guidance.solarArrayDeploymentComplete(); if (result == returnvalue::FAILED) { return acsctrl::FILE_DELETION_FAILED; } return HasActionsIF::EXECUTION_FINISHED; } case RESET_MEKF: { navigation.resetMekf(&attitudeEstimationData); return HasActionsIF::EXECUTION_FINISHED; } case RESTORE_MEKF_NONFINITE_RECOVERY: { mekfLost = false; return HasActionsIF::EXECUTION_FINISHED; } case UPDATE_TLE: { if (size != 69 * 2) { return HasActionsIF::INVALID_PARAMETERS; } ReturnValue_t result = updateTle(data, data + 69, false); if (result != returnvalue::OK) { return result; } result = writeTleToFs(data); if (result != returnvalue::OK) { return result; } return HasActionsIF::EXECUTION_FINISHED; } case (READ_TLE): { uint8_t tle[69 * 2] = {}; uint8_t line2[69] = {}; ReturnValue_t result = readTleFromFs(tle, line2); if (result != returnvalue::OK) { return result; } std::memcpy(tle + 69, line2, 69); actionHelper.reportData(commandedBy, actionId, tle, 69 * 2); return HasActionsIF::EXECUTION_FINISHED; } case (UPDATE_MEKF_STANDARD_DEVIATIONS): navigation.updateMekfStandardDeviations(&acsParameters); return HasActionsIF::EXECUTION_FINISHED; default: { return HasActionsIF::INVALID_ACTION_ID; } } } 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 { 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(); } } switch (internalState) { case InternalState::STARTUP: { initialCountdown.resetTimer(); internalState = InternalState::INITIAL_DELAY; return; } case InternalState::INITIAL_DELAY: { if (initialCountdown.hasTimedOut()) { uint8_t line1[69] = {}; uint8_t line2[69] = {}; readTleFromFs(line1, line2); updateTle(line1, line2, true); internalState = InternalState::READY; } return; } case InternalState::READY: { if (mode != MODE_OFF) { performAttitudeControl(); } break; } default: break; } } void AcsController::performAttitudeControl() { Clock::getClock_timeval(&timeAbsolute); Clock::getClockMonotonic(&timeRelative); if (timeRelative.tv_sec != 0 and oldTimeRelative.tv_sec != 0) { timeDelta = timevalOperations::toDouble(timeRelative - oldTimeRelative); } oldTimeRelative = timeRelative; ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed); if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) { triggerEvent(acs::TLE_TOO_OLD); tleTooOldFlag = true; } else if (result != Sgp4Propagator::TLE_TOO_OLD) { tleTooOldFlag = false; } sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); fusedRotationEstimation.estimateFusedRotationRate( mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, timeDelta, &attitudeEstimationData, acsParameters.kalmanFilterParameters.allowStr); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO, static_cast(attitudeEstimationData.mekfStatus.value)); mekfInvalidFlag = true; } if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { triggerEvent(acs::MEKF_AUTOMATIC_RESET); navigation.resetMekf(&attitudeEstimationData); mekfLost = true; } } else if (mekfInvalidFlag) { triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } handleDetumbling(); switch (mode) { case acs::SAFE: switch (submode) { case SUBMODE_NONE: performSafe(); break; case acs::DETUMBLE: performDetumble(); break; } break; case acs::PTG_IDLE: case acs::PTG_TARGET: case acs::PTG_TARGET_GS: case acs::PTG_NADIR: case acs::PTG_INERTIAL: performPointingCtrl(); break; } } void AcsController::performSafe() { // get desired satellite rate, sun direction to align to and inertia double sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), fusedRotRateSourcesData.rotRateTotalSusMgm.isValid(), acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { case (acs::ControlModeStrategy::SAFECTRL_MEKF): safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, attitudeEstimationData.satRotRateMekf.value, susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::ControlModeStrategy::SAFECTRL_GYR): safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::ControlModeStrategy::SAFECTRL_SUSMGM): safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateSourcesData.rotRateTotalSusMgm.value, fusedRotRateSourcesData.rotRateParallelSusMgm.value, fusedRotRateSourcesData.rotRateOrthogonalSusMgm.value, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateSourcesData.rotRateTotalSusMgm.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING): errAng = NAN; safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl; break; } actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); updateCtrlValData(errAng, safeCtrlStrat); updateActuatorCmdData(cmdDipoleMtqs); commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } void AcsController::performDetumble() { acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); double magMomMtq[3] = {0, 0, 0}; switch (safeCtrlStrat) { case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL): detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainFull); break; case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainBdot); break; case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl; break; } actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); updateCtrlValData(safeCtrlStrat); updateActuatorCmdData(cmdDipoleMtqs); commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } void AcsController::performPointingCtrl() { bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid()); uint8_t useMekf = false; switch (mode) { case acs::PTG_IDLE: useMekf = acsParameters.idleModeControllerParameters.useMekf; break; case acs::PTG_TARGET: useMekf = acsParameters.targetModeControllerParameters.useMekf; break; case acs::PTG_TARGET_GS: useMekf = acsParameters.gsTargetModeControllerParameters.useMekf; break; case acs::PTG_NADIR: useMekf = acsParameters.nadirModeControllerParameters.useMekf; break; case acs::PTG_INERTIAL: useMekf = acsParameters.inertialModeControllerParameters.useMekf; break; } acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid, attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotalSource.isValid(), fusedRotRateData.rotRateSource.value, useMekf); if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { ptgCtrlLostCounter++; if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); ptgCtrlLostCounter = 0; } guidance.resetValues(); updateCtrlValData(ptgCtrlStrat); updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16); commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); return; } else { ptgCtrlLostCounter = 0; } double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0}; switch (ptgCtrlStrat) { case acs::ControlModeStrategy::PTGCTRL_MEKF: std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI)); std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB)); break; case acs::ControlModeStrategy::PTGCTRL_STR: quatBI[0] = sensorValues.strSet.caliQx.value; quatBI[1] = sensorValues.strSet.caliQy.value; quatBI[2] = sensorValues.strSet.caliQz.value; quatBI[3] = sensorValues.strSet.caliQw.value; std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB)); break; case acs::ControlModeStrategy::PTGCTRL_QUEST: std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI)); std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB)); break; default: sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl" << std::endl; break; } bool allRwAvailable = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv, &rwAvail); if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { triggerEvent(acs::MULTIPLE_RW_INVALID); multipleRwUnavailableCounter = 0; } multipleRwUnavailableCounter++; return; } multipleRwUnavailableCounter = 0; if (result == acsctrl::SINGLE_RW_UNAVAILABLE) { allRwAvailable = false; } // Variables required for guidance double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0, errorSatRotRate[3] = {0, 0, 0}; // Variables required for setting actuators double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0}, mgtDpDes[3] = {0, 0, 0}; switch (mode) { case acs::PTG_IDLE: guidance.targetQuatPtgIdle(timeAbsolute, timeDelta, susDataProcessed.sunIjkModel.value, gpsDataProcessed.gpsPosition.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.idleModeControllerParameters, 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_TARGET: guidance.targetQuatPtgTarget(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(allRwAvailable, &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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), 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(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.gsTargetModeControllerParameters, 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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_NADIR: guidance.targetQuatPtgNadir(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(allRwAvailable, &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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_INERTIAL: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, sizeof(targetQuat)); guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(allRwAvailable, &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, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; default: sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; break; } actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel, acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws); ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgCtrlStrat); updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs); commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); } void AcsController::handleDetumbling() { switch (detumbleState) { case DetumbleState::NO_DETUMBLE: if (fusedRotRateData.rotRateTotalSusMgm.isValid() and VectorOperations::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (detumbleCounter > 0) { detumbleCounter -= 1; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { if (mode == acs::AcsMode::SAFE) { detumbleState = DetumbleState::DETUMBLE_FROM_SAFE; break; } detumbleState = DetumbleState::DETUMBLE_FROM_PTG; } break; case DetumbleState::DETUMBLE_FROM_PTG: triggerEvent(acs::PTG_RATE_VIOLATION); detumbleTransitionCountdow.resetTimer(); detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION; break; case DetumbleState::PTG_TO_SAFE_TRANSITION: if (detumbleTransitionCountdow.hasTimedOut()) { triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 2); detumbleCounter = 0; detumbleState = DetumbleState::NO_DETUMBLE; break; } if (mode == acs::AcsMode::SAFE) { detumbleState = DetumbleState::DETUMBLE_FROM_SAFE; } break; case DetumbleState::DETUMBLE_FROM_SAFE: detumbleCounter = 0; // Triggers detumble mode transition in subsystem if (mode == acs::AcsMode::SAFE) { triggerEvent(acs::SAFE_RATE_VIOLATION); startTransition(mode, acs::SafeSubmode::DETUMBLE); detumbleState = DetumbleState::IN_DETUMBLE; break; } triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 3); detumbleState = DetumbleState::NO_DETUMBLE; break; case DetumbleState::IN_DETUMBLE: if (fusedRotRateData.rotRateTotalSusMgm.isValid() and VectorOperations::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } else if (detumbleCounter > 0) { detumbleCounter -= 1; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; // Triggers safe mode transition in subsystem triggerEvent(acs::RATE_RECOVERY); startTransition(mode, acs::SafeSubmode::DEFAULT); detumbleState = DetumbleState::NO_DETUMBLE; } break; default: sif::error << "AcsController: Invalid DetumbleState" << std::endl; } } void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) { if (not safeCtrlFailureFlag) { triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure); safeCtrlFailureFlag = true; } safeCtrlFailureCounter++; if (safeCtrlFailureCounter > 150) { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; } } ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration) { { PoolReadGuard pg(&dipoleSet); MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT, torquer::LOCK_CTX); torquer::NEW_ACTUATION_FLAG = true; dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration); } return returnvalue::OK; } ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime) { { PoolReadGuard pg(&dipoleSet); MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT, torquer::LOCK_CTX); torquer::NEW_ACTUATION_FLAG = true; dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration); } { PoolReadGuard pg(&rw1SpeedSet); rw1SpeedSet.setRwSpeed(rw1Speed, rampTime); } { PoolReadGuard pg(&rw2SpeedSet); rw2SpeedSet.setRwSpeed(rw2Speed, rampTime); } { PoolReadGuard pg(&rw3SpeedSet); rw3SpeedSet.setRwSpeed(rw3Speed, rampTime); } { PoolReadGuard pg(&rw4SpeedSet); rw4SpeedSet.setRwSpeed(rw4Speed, rampTime); } return returnvalue::OK; } void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) { updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole); } void AcsController::updateActuatorCmdData(const double *rwTargetTorque, const int32_t *rwTargetSpeed, const int16_t *mtqTargetDipole) { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t)); actuatorCmdData.setValidity(true, true); } } void AcsController::updateCtrlValData(acs::ControlModeStrategy ctrlStrat) { 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 = ctrlStrat; ctrlValData.safeStrat.setValid(true); ctrlValData.setValidity(true, false); } } void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat) { 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 = errAng; ctrlValData.errAng.setValid(true); std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double)); ctrlValData.tgtRotRate.setValid(false); ctrlValData.safeStrat.value = ctrlStrat; ctrlValData.safeStrat.setValid(true); ctrlValData.setValidity(true, false); } } void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng, const double *tgtRotRate, acs::ControlModeStrategy ctrlStrat) { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng; std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); ctrlValData.safeStrat.value = ctrlStrat; ctrlValData.setValidity(true, true); } } 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, 60.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(), enableHkSets, 60.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, 60.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(), enableHkSets, 60.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, 60.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(), enableHkSets, 60.0}); // GPS Processed localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 60.0}); // Attitude Estimation localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus); localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest); poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 30.0}); // Ctrl Values localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat); localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate); poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 30.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(), enableHkSets, 30.0}); // Fused Rot Rate localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SUSMGM, &rotRateTotSusMgm); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SOURCE, &rotRateTotSource); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 30.0}); // Fused Rot Rate Sources localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm); 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_QUEST, &rotRateTotalQuest); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 60.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 &attitudeEstimationData; case acsctrl::CTRL_VAL_DATA: return &ctrlValData; case acsctrl::ACTUATOR_CMD_DATA: return &actuatorCmdData; case acsctrl::FUSED_ROTATION_RATE_DATA: return &fusedRotRateData; case acsctrl::FUSED_ROTATION_RATE_SOURCES_DATA: return &fusedRotRateSourcesData; 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 (not((mode < acs::AcsMode::SAFE) or (mode > acs::AcsMode::PTG_INERTIAL))) { if (mode == acs::AcsMode::SAFE) { if (not((submode == SUBMODE_NONE) or (submode == acs::SafeSubmode::DETUMBLE))) { return INVALID_SUBMODE; } else { return returnvalue::OK; } } else if (not(submode == SUBMODE_NONE)) { return INVALID_SUBMODE; } else { return returnvalue::OK; } } return INVALID_MODE; } void AcsController::modeChanged(Mode_t mode, Submode_t submode) { guidance.resetValues(); if (mode == acs::AcsMode::SAFE) { { PoolReadGuard pg(&rw1SpeedSet); rw1SpeedSet.setRwSpeed(0, 10); } { PoolReadGuard pg(&rw2SpeedSet); rw2SpeedSet.setRwSpeed(0, 10); } { PoolReadGuard pg(&rw3SpeedSet); rw3SpeedSet.setRwSpeed(0, 10); } { PoolReadGuard pg(&rw4SpeedSet); rw4SpeedSet.setRwSpeed(0, 10); } } if (submode == acs::SafeSubmode::DETUMBLE) { detumbleState = DetumbleState::IN_DETUMBLE; } if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::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); } void AcsController::announceMode(bool recursive) { const char *modeStr = "UNKNOWN"; if (mode == HasModesIF::MODE_OFF) { modeStr = "OFF"; } else { modeStr = acs::getModeStr(static_cast(mode)); } const char *submodeStr = "UNKNOWN"; if (submode == HasModesIF::SUBMODE_NONE) { submodeStr = "NONE"; } if (mode == acs::AcsMode::SAFE) { acs::SafeSubmode safeSubmode = static_cast(this->submode); if (safeSubmode == acs::SafeSubmode::DETUMBLE) { submodeStr = "DETUMBLE"; } } sif::info << "ACS controller is now in " << modeStr << " mode with " << submodeStr << " submode" << std::endl; return ExtendedControllerBase::announceMode(recursive); } void AcsController::copyMgmData() { { 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()); } } } ReturnValue_t AcsController::updateTle(const uint8_t *line1, const uint8_t *line2, bool fromFile) { ReturnValue_t result = navigation.updateTle(line1, line2); if (result != returnvalue::OK) { if (not fromFile) { uint8_t fileLine1[69] = {}; uint8_t fileLine2[69] = {}; readTleFromFs(fileLine1, fileLine2); navigation.updateTle(fileLine1, fileLine2); } return result; } return returnvalue::OK; } ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { auto mntPrefix = sdcMan.getCurrentMountPrefix(); if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) { return returnvalue::FAILED; } std::string path = mntPrefix + TLE_FILE; // Clear existing TLE from file std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc); if (tleFile.is_open()) { tleFile.write(reinterpret_cast(tle), 69); tleFile << "\n"; tleFile.write(reinterpret_cast(tle + 69), 69); } else { return acsctrl::WRITE_FILE_FAILED; } tleFile.close(); return returnvalue::OK; } ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) { auto mntPrefix = sdcMan.getCurrentMountPrefix(); if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) { return returnvalue::FAILED; } std::string path = mntPrefix + TLE_FILE; std::error_code e; if (std::filesystem::exists(path, e)) { // Read existing TLE from file std::fstream tleFile = std::fstream(path.c_str(), std::fstream::in); if (tleFile.is_open()) { std::string tleLine1, tleLine2; getline(tleFile, tleLine1); std::memcpy(line1, tleLine1.c_str(), 69); getline(tleFile, tleLine2); std::memcpy(line2, tleLine2.c_str(), 69); } else { triggerEvent(acs::TLE_FILE_READ_FAILED); return acsctrl::READ_FILE_FAILED; } tleFile.close(); } else { triggerEvent(acs::TLE_FILE_READ_FAILED); return acsctrl::READ_FILE_FAILED; } return returnvalue::OK; } void AcsController::copyGyrData() { { 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()); } } }