eive-obsw/mission/controller/AcsController.cpp
Marius Eggert 4bd2a2dac8
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
added and implemented all ACS related DataPools to AcsController
2022-10-24 10:41:28 +02:00

542 lines
22 KiB
C++

#include "AcsController.h"
#include <fsfw/datapool/PoolReadGuard.h>
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
sensorProcessing(&acsParameters),
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
mgmDataRaw(this),
mgmDataProcessed(this),
susDataRaw(this),
susDataProcessed(this),
gyrDataRaw(this),
gyrDataProcessed(this),
gpsDataProcessed(this),
mekfData(this),
actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
return returnvalue::OK;
}
void AcsController::performControlOperation() {
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
internalState = InternalState::INITIAL_DELAY;
return;
}
case InternalState::INITIAL_DELAY: {
if (initialCountdown.hasTimedOut()) {
internalState = InternalState::READY;
}
return;
}
case InternalState::READY: {
if (mode != MODE_OFF) {
switch (submode) {
case SUBMODE_SAFE:
performSafe();
break;
case SUBMODE_DETUMBLE:
performDetumble();
break;
case SUBMODE_PTG_GS:
performPointingCtrl();
break;
}
}
break;
}
default:
break;
}
{
PoolReadGuard pg(&mgmDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyMgmData();
}
}
{
PoolReadGuard pg(&susDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copySusData();
}
}
{
PoolReadGuard pg(&gyrDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyGyrData();
}
}
// DEBUG : REMOVE AFTER COMPLETION
mode = MODE_ON;
submode = SUBMODE_DETUMBLE;
// DEBUG END
}
void AcsController::performSafe() {
// Concept: SAFE MODE WITH MEKF
// -do the sensor processing, maybe is does make more sense do call this class function in
// another place since we have to do it for every mode regardless of safe or not
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf); // DOES THIS WORK WITH VALID?
// 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 == returnvalue::OK) {
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
(outputValues.satRateMekf), &(outputValues.satRateMekfValid), sunTargetDir,
satRateSafe, magMomMtq, &magMomMtqValid);
} else {
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
outputValues.magneticFieldVectorDerivative,
&(outputValues.magneticFieldVectorDerivativeValid), sunTargetDir,
satRateSafe, magMomMtq, &magMomMtqValid);
}
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
// Detumble check and switch
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
else if (outputValues.satRateEstValid &&
VectorOperations<double>::norm(outputValues.satRateEst, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
else {
detumbleCounter = 0;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_DETUMBLE;
detumbleCounter = 0;
}
// commanding.magnetorquesDipol();
}
void AcsController::performDetumble() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
// Clock::getUptime(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
&outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
&outputValues.magFieldEstValid, magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
else if (outputValues.satRateEstValid &&
VectorOperations<double>::norm(outputValues.satRateEst, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_SAFE;
detumbleCounter = 0;
}
}
void AcsController::performPointingCtrl() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
guidance.comparePtg(targetQuat, &outputValues, refSatRate, 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.ptgGroundstation(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 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), torquePtgRws,
rwTrqNs, 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);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
}
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_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});
// 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::ACTUATOR_CMD_DATA:
return &actuatorCmdData;
default:
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 > 5) || (submode < 2)) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;
}
}
return INVALID_MODE;
}
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
void AcsController::announceMode(bool recursive) {}
void AcsController::copyMgmData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid());
}
}
{
PoolReadGuard pg(&sensorValues.mgm1Rm3100Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid());
}
}
{
PoolReadGuard pg(&sensorValues.mgm2Lis3Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid());
}
}
{
PoolReadGuard pg(&sensorValues.mgm3Rm3100Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value,
3 + sizeof(float));
mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid());
}
}
{
PoolReadGuard pg(&sensorValues.imtqMgmSet);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value,
3 * sizeof(float));
mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
}
}
}
void AcsController::copySusData() {
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());
}
}
}