From 673c24a34d9b1e075f0b5a65934828aeb55f6615 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 15:43:40 +0100 Subject: [PATCH] frmt --- mission/controller/AcsController.cpp | 1092 +++++++++++++------------- 1 file changed, 544 insertions(+), 548 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 362a5e1b..85ef8d7a 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,581 +103,577 @@ 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); - - 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); - // } + sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - void AcsController::performPointingCtrl() { - ACS::SensorValues sensorValues; + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); - 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}; - switch (submode) { - case SUBMODE_IDLE: - guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - break; - case SUBMODE_PTG_TARGET: - guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, - targetQuat, refSatRate); - break; - case SUBMODE_PTG_TARGET_GS: - guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, - now, targetQuat, refSatRate); - break; - case SUBMODE_PTG_NADIR: - guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); - break; - case SUBMODE_PTG_INERTIAL: - guidance.inertialQuatPtg(targetQuat, refSatRate); - break; + { + 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); } - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, - deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed - guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); - 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}, mode = 0; - ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - double rwTrqNs[4] = {0, 0, 0, 0}; - ptgCtrl.ptgNullspace( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + } - if (acsParameters.pointingModeControllerParameters.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); + // 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); + // } +} - 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); - double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol - ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), - mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), +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}; + switch (submode) { + case SUBMODE_IDLE: + guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); + break; + case SUBMODE_PTG_TARGET: + guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + break; + case SUBMODE_PTG_TARGET_GS: + guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); + break; + case SUBMODE_PTG_NADIR: + guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + break; + case SUBMODE_PTG_INERTIAL: + guidance.inertialQuatPtg(targetQuat, refSatRate); + break; + } + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed + guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); + 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}, mode = 0; + ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + double rwTrqNs[4] = {0, 0, 0, 0}; + ptgCtrl.ptgNullspace( + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + + if (acsParameters.pointingModeControllerParameters.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), mgtDpDes); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); + &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); + double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + ptgCtrl.ptgDesaturation(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); + 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); - // } + 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]); } - 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; + { + 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); + // } +} - 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::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; +} - 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; +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; +} - 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()); - } +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; } - { - 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()); } } + { + 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::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()); - } +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()); + } + } +}