From adef468c0bc902923f396a92bda4d68a99e7c880 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Mon, 23 Jan 2023 16:34:52 +0100 Subject: [PATCH] applyed CppStyle Format --- mission/controller/AcsController.cpp | 1270 ++++++++++---------- mission/controller/acs/AcsParameters.h | 59 +- mission/controller/acs/Guidance.cpp | 31 +- mission/controller/acs/Guidance.h | 8 +- mission/controller/acs/control/PtgCtrl.cpp | 15 +- mission/controller/acs/control/PtgCtrl.h | 14 +- 6 files changed, 714 insertions(+), 683 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f829c3e3..61ddae44 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -26,7 +26,7 @@ AcsController::AcsController(object_id_t objectId) ctrlValData(this), actuatorCmdData(this) {} -ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { +ReturnValue_t AcsController::handleCommandMessage(CommandMessage* message) { ReturnValue_t result = actionHelper.handleActionMessage(message); if (result == returnvalue::OK) { return result; @@ -103,642 +103,662 @@ void AcsController::performControlOperation() { copyGyrData(); } } - } +} - void AcsController::performSafe() { - ACS::SensorValues sensorValues; +void AcsController::performSafe() { + ACS::SensorValues sensorValues; - timeval now; - Clock::getClock_timeval(&now); + 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); + 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) { - submode = SUBMODE_DETUMBLE; - detumbleCounter = 0; - triggerEvent(SAFE_RATE_VIOLATION); - } - - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 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() { - ACS::SensorValues sensorValues; - - 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( + // 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(), - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + } - 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++; + 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) { + submode = SUBMODE_DETUMBLE; + detumbleCounter = 0; + triggerEvent(SAFE_RATE_VIOLATION); + } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 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() { + ACS::SensorValues sensorValues; + + 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) { + submode = SUBMODE_SAFE; + detumbleCounter = 0; + } + + 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() { + ACS::SensorValues sensorValues; + + 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 SUBMODE_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 SUBMODE_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 SUBMODE_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 SUBMODE_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 SUBMODE_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.subscribeForRegularPeriodicPacket({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.subscribeForRegularPeriodicPacket({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.subscribeForRegularPeriodicPacket({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 { - detumbleCounter = 0; + return INVALID_SUBMODE; } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - submode = SUBMODE_SAFE; - detumbleCounter = 0; - } - - 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() { - ACS::SensorValues sensorValues; - - 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 SUBMODE_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 SUBMODE_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 SUBMODE_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 SUBMODE_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 SUBMODE_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.subscribeForRegularPeriodicPacket({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.subscribeForRegularPeriodicPacket({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.subscribeForRegularPeriodicPacket({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 > 8) || (submode < 2)) { - return INVALID_SUBMODE; - } else { - return returnvalue::OK; - } - } - return INVALID_MODE; - } - - 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()); - } + } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { + if ((submode > 8) || (submode < 2)) { + return INVALID_SUBMODE; + } else { + return returnvalue::OK; } } + return INVALID_MODE; +} - void AcsController::copySusData() { - ACS::SensorValues sensorValues; - { - 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::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()); } } - - 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()); - } + { + 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() { + ACS::SensorValues sensorValues; + { + 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()); + } + } +} diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 80a3a65f..a0e8af0f 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -816,49 +816,48 @@ class AcsParameters : public HasParametersIF { } safeModeControllerParameters; struct PointingLawParameters { + double zeta = 0.3; + double om = 0.3; + double omMax = 1 * M_PI / 180; + double qiMin = 0.1; + double gainNullspace = 0.01; - double zeta = 0.3; - double om = 0.3; - double omMax = 1 * M_PI / 180; - double qiMin = 0.1; - double gainNullspace = 0.01; - - double desatMomentumRef[3] = {0, 0, 0}; - double deSatGainFactor = 1000; - uint8_t desatOn = true; - uint8_t enableAntiStiction = true; + double desatMomentumRef[3] = {0, 0, 0}; + double deSatGainFactor = 1000; + uint8_t desatOn = true; + uint8_t enableAntiStiction = true; } pointingLawParameters; struct TargetModeControllerParameters : PointingLawParameters { - double refDirection[3] = {-1, 0, 0}; // Antenna - double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to - // give this as an input- currently en calculation is done - double quatRef[4] = {0, 0, 0, 1}; - int8_t timeElapsedMax = 10; // rot rate calculations + double refDirection[3] = {-1, 0, 0}; // Antenna + double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to + // give this as an input- currently en calculation is done + double quatRef[4] = {0, 0, 0, 1}; + int8_t timeElapsedMax = 10; // rot rate calculations - // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] + // Default is Stuttgart GS + double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude + double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude + double altitudeTgt = 500; // [m] - // For one-axis control: - uint8_t avoidBlindStr = true; - double blindAvoidStart = 1.5; - double blindAvoidStop = 2.5; - double blindRotRate = 1 * M_PI / 180; + // For one-axis control: + uint8_t avoidBlindStr = true; + double blindAvoidStart = 1.5; + double blindAvoidStop = 2.5; + double blindRotRate = 1 * M_PI / 180; } targetModeControllerParameters; struct NadirModeControllerParameters : PointingLawParameters { - double refDirection[3] = {-1, 0, 0}; // Antenna - double quatRef[4] = {0, 0, 0, 1}; - int8_t timeElapsedMax = 10; // rot rate calculations + double refDirection[3] = {-1, 0, 0}; // Antenna + double quatRef[4] = {0, 0, 0, 1}; + int8_t timeElapsedMax = 10; // rot rate calculations } nadirModeControllerParameters; struct InertialModeControllerParameters : PointingLawParameters { - double tgtQuat[4] = {0, 0, 0, 1}; - double refRotRate[3] = {0, 0, 0}; - double quatRef[4] = {0, 0, 0, 1}; + double tgtQuat[4] = {0, 0, 0, 1}; + double refRotRate[3] = {0, 0, 0}; + double quatRef[4] = {0, 0, 0, 1}; } inertialModeControllerParameters; struct StrParameters { diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4ba72674..48c148c9 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -42,7 +42,8 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.latitudeTgt, + acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS @@ -172,14 +173,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: } } -void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate) { +void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *refSatRate) { //------------------------------------------------------------------------------------- // Calculation of reference rotation rate //------------------------------------------------------------------------------------- - double timeElapsed = - now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternion.tv_sec + - timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); + double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - + (timeSavedQuaternion.tv_sec + + timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); if (timeElapsed < timeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); @@ -226,7 +227,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.latitudeTgt, + acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; @@ -307,7 +309,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat double groundStationCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.latitudeTgt, + acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; @@ -588,13 +591,15 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, } void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { - std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, 4 * sizeof(double)); - std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, 3 * sizeof(double)); + std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, + 4 * sizeof(double)); + std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, + 3 * sizeof(double)); } -void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { - +void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], + double refSatRate[3], double quatErrorComplete[4], double quatError[3], + double deltaRate[3]) { double satRate[3] = {0, 0, 0}; std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index e399af8a..5203ee48 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -58,10 +58,12 @@ class Guidance { // @note: compares target Quaternion and reference quaternion, also actual satellite rate and // desired - void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]); + void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], + double refSatRate[3], double quatErrorComplete[4], double quatError[3], + double deltaRate[3]); - void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate); + void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *refSatRate); // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 6d41e85d..352c0319 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -26,8 +26,9 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { rwMatrices = &(acsParameters_->rwMatrices); } -void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate, - const double *rwPseudoInv, double *torqueRws) { +void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *qError, const double *deltaRate, const double *rwPseudoInv, + double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix //------------------------------------------------------------------------------------------------ @@ -106,7 +107,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameter VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } -void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, bool magFieldEstValid, double *satRate, +void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes) { if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { @@ -127,8 +129,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLaw VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract( - momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3); + VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, + deltaMomentum, 3); // resulting magnetic dipole command double crossMomentumMagField[3] = {0, 0, 0}; VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); @@ -137,7 +139,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLaw VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); } -void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, +void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index b036d180..559216f7 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -41,14 +41,16 @@ class PtgCtrl { /* @brief: Calculates the needed torque for the pointing control mechanism * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ - void ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate, - const double *rwPseudoInv, double *torqueRws); + void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, + const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, - bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, - int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes); + void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + double *magFieldEst, bool magFieldEstValid, double *satRate, + int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, + double *mgtDpDes); - void ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs); /* @brief: Commands the stiction torque in case wheel speed is to low