Merge branch 'eggert/acs' into marquardt/ptgCtrl
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
# Conflicts: # mission/controller/AcsController.cpp # mission/controller/AcsController.h # mission/controller/acs/AcsParameters.h # mission/controller/acs/ActuatorCmd.h # mission/controller/acs/Guidance.cpp # mission/controller/acs/Guidance.h # mission/controller/acs/MultiplicativeKalmanFilter.cpp # mission/controller/acs/OutputValues.h # mission/controller/acs/SensorProcessing.cpp # mission/controller/acs/SensorProcessing.h # mission/controller/acs/control/Detumble.cpp # mission/controller/acs/control/Detumble.h # mission/controller/acs/control/PtgCtrl.cpp # mission/controller/acs/util/MathOperations.h
This commit is contained in:
@@ -2,18 +2,28 @@
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
#include "mission/devices/torquer.h"
|
||||
|
||||
AcsController::AcsController(object_id_t objectId)
|
||||
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
|
||||
: ExtendedControllerBase(objectId),
|
||||
sensorProcessing(&acsParameters),
|
||||
navigation(&acsParameters),
|
||||
actuatorCmd(&acsParameters),
|
||||
guidance(&acsParameters),
|
||||
safeCtrl(&acsParameters),
|
||||
safeCtrl(&acsParameters),
|
||||
detumble(&acsParameters),
|
||||
ptgCtrl(&acsParameters),
|
||||
detumbleCounter{0},
|
||||
mgmData(this),
|
||||
susData(this) {}
|
||||
mgmDataRaw(this),
|
||||
mgmDataProcessed(this),
|
||||
susDataRaw(this),
|
||||
susDataProcessed(this),
|
||||
gyrDataRaw(this),
|
||||
gyrDataProcessed(this),
|
||||
gpsDataProcessed(this),
|
||||
mekfData(this),
|
||||
ctrlValData(this),
|
||||
actuatorCmdData(this) {}
|
||||
|
||||
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
|
||||
return returnvalue::OK;
|
||||
@@ -38,21 +48,14 @@ void AcsController::performControlOperation() {
|
||||
case SUBMODE_SAFE:
|
||||
performSafe();
|
||||
break;
|
||||
|
||||
case SUBMODE_DETUMBLE:
|
||||
performDetumble();
|
||||
break;
|
||||
|
||||
case SUBMODE_PTG_SUN:
|
||||
case SUBMODE_PTG_TARGET:
|
||||
performPointingCtrl();
|
||||
break;
|
||||
|
||||
case SUBMODE_PTG_SUN:
|
||||
performPointingCtrl();
|
||||
break;
|
||||
|
||||
case SUBMODE_PTG_NADIR:
|
||||
performPointingCtrl();
|
||||
case SUBMODE_PTG_INERTIAL:
|
||||
performPointingCtrl();
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -63,109 +66,191 @@ void AcsController::performControlOperation() {
|
||||
}
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&mgmData);
|
||||
PoolReadGuard pg(&mgmDataRaw);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
copyMgmData();
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susData);
|
||||
PoolReadGuard pg(&susDataRaw);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
copySusData();
|
||||
}
|
||||
}
|
||||
|
||||
// DEBUG : REMOVE AFTER COMPLETION
|
||||
mode = MODE_ON;
|
||||
submode = SUBMODE_DETUMBLE;
|
||||
// DEBUG END
|
||||
{
|
||||
PoolReadGuard pg(&gyrDataRaw);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
copyGyrData();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::performSafe() {
|
||||
ACS::SensorValues sensorValues;
|
||||
ACS::OutputValues outputValues;
|
||||
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &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};
|
||||
bool magMomMtqValid = false;
|
||||
if ( !validMekf ) { // Valid = 0, Failed = 1
|
||||
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
|
||||
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
|
||||
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
|
||||
(outputValues.satRateMekf), &(outputValues.satRateMekfValid),
|
||||
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
|
||||
}
|
||||
else {
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
|
||||
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
|
||||
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
|
||||
outputValues.magneticFieldVectorDerivative, &(outputValues.magneticFieldVectorDerivativeValid),
|
||||
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
|
||||
// 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);
|
||||
double dipolCmdUnits[3] = {0, 0, 0};
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&ctrlValData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double zeroQuat[4] = {0, 0, 0, 0};
|
||||
std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double));
|
||||
ctrlValData.tgtQuat.setValid(false);
|
||||
std::memcpy(ctrlValData.errQuat.value, zeroQuat, 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<double>::norm(mekfData.satRotRateMekf.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
|
||||
VectorOperations<double>::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) {
|
||||
int32_t zeroVec[4] = {0, 0, 0, 0};
|
||||
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
|
||||
actuatorCmdData.rwTargetTorque.setValid(false);
|
||||
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
|
||||
actuatorCmdData.rwTargetSpeed.setValid(false);
|
||||
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t));
|
||||
actuatorCmdData.mtqTargetDipole.setValid(true);
|
||||
actuatorCmdData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
// {
|
||||
// PoolReadGuard pg(&dipoleSet);
|
||||
// MutexGuard mg(torquer::lazyLock());
|
||||
// torquer::NEW_ACTUATION_FLAG = true;
|
||||
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration);
|
||||
// }
|
||||
}
|
||||
|
||||
void AcsController::performDetumble() {
|
||||
ACS::SensorValues sensorValues;
|
||||
ACS::OutputValues outputValues;
|
||||
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
|
||||
&outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
|
||||
&outputValues.magFieldEstValid, magMomMtq);
|
||||
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 (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
if (mekfData.satRotRateMekf.isValid() &&
|
||||
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
}
|
||||
|
||||
else if (outputValues.satRateEstValid &&
|
||||
VectorOperations<double>::norm(outputValues.satRateEst, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
|
||||
VectorOperations<double>::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) {
|
||||
int32_t zeroVec[4] = {0, 0, 0, 0};
|
||||
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
|
||||
actuatorCmdData.rwTargetTorque.setValid(false);
|
||||
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 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;
|
||||
ACS::OutputValues outputValues;
|
||||
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
||||
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
switch (submode) {
|
||||
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
|
||||
switch (submode) {
|
||||
case SUBMODE_PTG_TARGET:
|
||||
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
||||
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
|
||||
break;
|
||||
case SUBMODE_PTG_SUN:
|
||||
guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
||||
@@ -176,9 +261,9 @@ void AcsController::performPointingCtrl() {
|
||||
case SUBMODE_PTG_INERTIAL:
|
||||
guidance.inertialQuatPtg(targetQuat, refSatRate);
|
||||
break;
|
||||
}
|
||||
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
|
||||
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
|
||||
} 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;
|
||||
@@ -203,45 +288,149 @@ void AcsController::performPointingCtrl() {
|
||||
&(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(
|
||||
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
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);
|
||||
// }
|
||||
}
|
||||
|
||||
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||
LocalDataPoolManager &poolManager) {
|
||||
// MGM
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec);
|
||||
// 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({mgmData.getSid(), false, 5.0});
|
||||
// SUS
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec);
|
||||
poolManager.subscribeForRegularPeriodicPacket({susData.getSid(), false, 5.0});
|
||||
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);
|
||||
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) {
|
||||
if (sid == mgmData.getSid()) {
|
||||
return &mgmData;
|
||||
} else if (sid == susData.getSid()) {
|
||||
return &susData;
|
||||
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;
|
||||
}
|
||||
@@ -255,7 +444,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
return INVALID_SUBMODE;
|
||||
}
|
||||
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
|
||||
if ((submode > 5) || (submode < 2)) {
|
||||
if ((submode > 6) || (submode < 2)) {
|
||||
return INVALID_SUBMODE;
|
||||
} else {
|
||||
return returnvalue::OK;
|
||||
@@ -269,110 +458,195 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
|
||||
void AcsController::announceMode(bool recursive) {}
|
||||
|
||||
void AcsController::copyMgmData() {
|
||||
ACS::SensorValues sensorValues;
|
||||
{
|
||||
PoolReadGuard pg(&mgm0Lis3Set);
|
||||
PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value,
|
||||
3 * sizeof(float));
|
||||
mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&mgm1Rm3100Set);
|
||||
PoolReadGuard pg(&sensorValues.mgm1Rm3100Set);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value,
|
||||
3 * sizeof(float));
|
||||
mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&mgm2Lis3Set);
|
||||
PoolReadGuard pg(&sensorValues.mgm2Lis3Set);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value,
|
||||
3 * sizeof(float));
|
||||
mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&mgm3Rm3100Set);
|
||||
PoolReadGuard pg(&sensorValues.mgm3Rm3100Set);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value,
|
||||
3 + sizeof(float));
|
||||
mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&imtqMgmSet);
|
||||
PoolReadGuard pg(&sensorValues.imtqMgmSet);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float));
|
||||
mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value;
|
||||
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(&susSets[0]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[0]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus0.value, susSets[0].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[1]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[1]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus1.value, susSets[1].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[2]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[2]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus2.value, susSets[2].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[3]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[3]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus3.value, susSets[3].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[4]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[4]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus4.value, susSets[4].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[5]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[5]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus5.value, susSets[5].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[6]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[6]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus6.value, susSets[6].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[7]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[7]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus7.value, susSets[7].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[8]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[8]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus8.value, susSets[8].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[9]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[9]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[10]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[10]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus10.value, susSets[10].channels.value, 6 * sizeof(uint16_t));
|
||||
std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value,
|
||||
6 * sizeof(uint16_t));
|
||||
susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid());
|
||||
}
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(&susSets[11]);
|
||||
PoolReadGuard pg(&sensorValues.susSets[11]);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susData.sus11.value, susSets[11].channels.value, 6 * sizeof(uint16_t));
|
||||
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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user