added and implemented all ACS related DataPools to AcsController
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good

This commit is contained in:
Marius Eggert 2022-10-24 10:41:28 +02:00
parent 8b23fd3dd2
commit 4bd2a2dac8
5 changed files with 352 additions and 171 deletions

View File

@ -12,8 +12,15 @@ AcsController::AcsController(object_id_t objectId)
detumble(&acsParameters), detumble(&acsParameters),
ptgCtrl(&acsParameters), ptgCtrl(&acsParameters),
detumbleCounter{0}, detumbleCounter{0},
mgmData(this), mgmDataRaw(this),
susData(this) {} mgmDataProcessed(this),
susDataRaw(this),
susDataProcessed(this),
gyrDataRaw(this),
gyrDataProcessed(this),
gpsDataProcessed(this),
mekfData(this),
actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
return returnvalue::OK; return returnvalue::OK;
@ -38,11 +45,9 @@ void AcsController::performControlOperation() {
case SUBMODE_SAFE: case SUBMODE_SAFE:
performSafe(); performSafe();
break; break;
case SUBMODE_DETUMBLE: case SUBMODE_DETUMBLE:
performDetumble(); performDetumble();
break; break;
case SUBMODE_PTG_GS: case SUBMODE_PTG_GS:
performPointingCtrl(); performPointingCtrl();
break; break;
@ -55,17 +60,23 @@ void AcsController::performControlOperation() {
} }
{ {
PoolReadGuard pg(&mgmData); PoolReadGuard pg(&mgmDataRaw);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
copyMgmData(); copyMgmData();
} }
} }
{ {
PoolReadGuard pg(&susData); PoolReadGuard pg(&susDataRaw);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
copySusData(); copySusData();
} }
} }
{
PoolReadGuard pg(&gyrDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyGyrData();
}
}
// DEBUG : REMOVE AFTER COMPLETION // DEBUG : REMOVE AFTER COMPLETION
mode = MODE_ON; mode = MODE_ON;
@ -81,10 +92,13 @@ void AcsController::performSafe() {
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
ACS::OutputValues outputValues; ACS::OutputValues outputValues;
timeval now; // We need to give the actual time here timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf); // DOES THIS WORK WITH VALID? navigation.useMekf(&sensorValues, &outputValues, &validMekf); // DOES THIS WORK WITH VALID?
// Give desired satellite rate and sun direction to align // Give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
@ -140,10 +154,12 @@ void AcsController::performDetumble() {
timeval now; timeval now;
Clock::getClock_timeval(&now); Clock::getClock_timeval(&now);
// Clock::getUptime(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf); navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(outputValues.magneticFieldVectorDerivative, detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
&outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst, &outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
@ -172,11 +188,13 @@ void AcsController::performPointingCtrl() {
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
ACS::OutputValues outputValues; ACS::OutputValues outputValues;
timeval now; // Übergabe ? timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf); navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
@ -204,38 +222,106 @@ void AcsController::performPointingCtrl() {
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) { LocalDataPoolManager &poolManager) {
// MGM // MGM Raw
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0});
// SUS // MGM Processed
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec); poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0});
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec); // SUS Raw
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw);
poolManager.subscribeForRegularPeriodicPacket({susData.getSid(), false, 5.0}); 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; return returnvalue::OK;
} }
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
if (sid == mgmData.getSid()) { switch (sid.ownerSetId) {
return &mgmData; case acsctrl::MGM_SENSOR_DATA:
} else if (sid == susData.getSid()) { return &mgmDataRaw;
return &susData; 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;
} }
return nullptr;
} }
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
@ -261,112 +347,195 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
void AcsController::announceMode(bool recursive) {} void AcsController::announceMode(bool recursive) {}
void AcsController::copyMgmData() { void AcsController::copyMgmData() {
ACS::SensorValues sensorValues;
{ {
PoolReadGuard pg(&mgm0Lis3Set); PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
if (pg.getReadResult() == returnvalue::OK) { 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) { 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) { 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) { 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) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float)); std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value,
mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.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() { void AcsController::copySusData() {
ACS::SensorValues sensorValues;
{ {
PoolReadGuard pg(&susSets[0]); PoolReadGuard pg(&sensorValues.susSets[0]);
if (pg.getReadResult() == returnvalue::OK) { 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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) { 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,
sif::debug << susData.sus9.isValid() << std::endl; 6 * sizeof(uint16_t));
sif::debug << susSets[9].channels.isValid() << std::endl; susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid());
} }
} }
{ {
PoolReadGuard pg(&susSets[10]); PoolReadGuard pg(&sensorValues.susSets[10]);
if (pg.getReadResult() == returnvalue::OK) { 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) { 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());
} }
} }
} }

View File

@ -8,6 +8,7 @@
#include "acs/ActuatorCmd.h" #include "acs/ActuatorCmd.h"
#include "acs/Guidance.h" #include "acs/Guidance.h"
#include "acs/Navigation.h" #include "acs/Navigation.h"
#include "acs/OutputValues.h"
#include "acs/SensorProcessing.h" #include "acs/SensorProcessing.h"
#include "acs/control/Detumble.h" #include "acs/control/Detumble.h"
#include "acs/control/PtgCtrl.h" #include "acs/control/PtgCtrl.h"
@ -64,62 +65,91 @@ class AcsController : public ExtendedControllerBase {
void modeChanged(Mode_t mode, Submode_t submode); void modeChanged(Mode_t mode, Submode_t submode);
void announceMode(bool recursive); void announceMode(bool recursive);
/* ACS Datasets */
// MGMs // MGMs
acsctrl::MgmDataRaw mgmData; acsctrl::MgmDataRaw mgmDataRaw;
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);
MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = PoolEntry<float> mgm1VecRaw = PoolEntry<float>(3);
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); PoolEntry<float> mgm2VecRaw = PoolEntry<float>(3);
RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = PoolEntry<float> mgm3VecRaw = PoolEntry<float>(3);
RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); PoolEntry<float> imtqMgmVecRaw = PoolEntry<float>(3);
MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set =
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER);
PoolEntry<float> mgm0PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm1PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm2PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm3PoolVec = PoolEntry<float>(3);
PoolEntry<float> imtqMgmPoolVec = PoolEntry<float>(3);
PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>(); PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>();
void copyMgmData(); void copyMgmData();
// Sun Sensors acsctrl::MgmDataProcessed mgmDataProcessed;
PoolEntry<float> mgm0VecProc = PoolEntry<float>(3);
acsctrl::SusDataRaw susData; PoolEntry<float> mgm1VecProc = PoolEntry<float>(3);
PoolEntry<float> mgm2VecProc = PoolEntry<float>(3);
std::array<SUS::SusDataset, 12> susSets{ PoolEntry<float> mgm3VecProc = PoolEntry<float>(3);
SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), PoolEntry<float> mgm4VecProc = PoolEntry<float>(3);
SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), PoolEntry<float> mgmVecTot = PoolEntry<float>(3);
SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), PoolEntry<float> mgmVecTotDer = PoolEntry<float>(3);
SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), PoolEntry<double> magIgrf = PoolEntry<double>(3);
SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF),
SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB),
SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF),
SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB),
SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB),
SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF),
SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF),
SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
};
PoolEntry<uint16_t> sus0PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus1PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus2PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus3PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus4PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus5PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus6PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus7PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus8PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus9PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus10PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus11PoolVec = PoolEntry<uint16_t>(6);
// SUSs
acsctrl::SusDataRaw susDataRaw;
PoolEntry<uint16_t> sus0ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus1ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus2ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus3ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus4ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus5ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus6ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus7ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus8ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus9ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus10ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus11ValRaw = PoolEntry<uint16_t>(6);
void copySusData(); void copySusData();
acsctrl::SusDataProcessed susDataProcessed;
PoolEntry<float> sus0VecProc = PoolEntry<float>(3);
PoolEntry<float> sus1VecProc = PoolEntry<float>(3);
PoolEntry<float> sus2VecProc = PoolEntry<float>(3);
PoolEntry<float> sus3VecProc = PoolEntry<float>(3);
PoolEntry<float> sus4VecProc = PoolEntry<float>(3);
PoolEntry<float> sus5VecProc = PoolEntry<float>(3);
PoolEntry<float> sus6VecProc = PoolEntry<float>(3);
PoolEntry<float> sus7VecProc = PoolEntry<float>(3);
PoolEntry<float> sus8VecProc = PoolEntry<float>(3);
PoolEntry<float> sus9VecProc = PoolEntry<float>(3);
PoolEntry<float> sus10VecProc = PoolEntry<float>(3);
PoolEntry<float> sus11VecProc = PoolEntry<float>(3);
PoolEntry<float> susVecTot = PoolEntry<float>(3);
PoolEntry<float> susVecTotDer = PoolEntry<float>(3);
PoolEntry<float> sunIjk = PoolEntry<float>(3);
// GYRs
acsctrl::GyrDataRaw gyrDataRaw;
PoolEntry<double> gyr0VecRaw = PoolEntry<double>(3);
PoolEntry<float> gyr1VecRaw = PoolEntry<float>(3);
PoolEntry<double> gyr2VecRaw = PoolEntry<double>(3);
PoolEntry<float> gyr3VecRaw = PoolEntry<float>(3);
void copyGyrData();
acsctrl::GyrDataProcessed gyrDataProcessed;
PoolEntry<double> gyr0VecProc = PoolEntry<double>(3);
PoolEntry<double> gyr1VecProc = PoolEntry<double>(3);
PoolEntry<double> gyr2VecProc = PoolEntry<double>(3);
PoolEntry<double> gyr3VecProc = PoolEntry<double>(3);
PoolEntry<double> gyrVecTot = PoolEntry<double>(3);
// GPS
acsctrl::GpsDataProcessed gpsDataProcessed;
PoolEntry<double> gcLatitude = PoolEntry<double>();
PoolEntry<double> gdLongitude = PoolEntry<double>();
// MEKF
acsctrl::MekfData mekfData;
PoolEntry<double> quatMekf = PoolEntry<double>(4);
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
// Actuator CMD
acsctrl::ActuatorCmdData actuatorCmdData;
PoolEntry<double> rwTargetTorque = PoolEntry<double>(4);
PoolEntry<double> rwTargetSpeed = PoolEntry<double>(4);
PoolEntry<double> mtqTargetDipole = PoolEntry<double>(3);
// Initial delay to make sure all pool variables have been initialized their owners // Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY); Countdown initialCountdown = Countdown(INIT_DELAY);
}; };

View File

@ -1,19 +1,9 @@
/*
* OutputValues.cpp
*
* Created on: 30 Mar 2022
* Author: rooob
*/
#include "OutputValues.h" #include "OutputValues.h"
namespace ACS { namespace ACS {
OutputValues::OutputValues(){ OutputValues::OutputValues() {}
} OutputValues::~OutputValues() {}
OutputValues::~OutputValues(){ } // namespace ACS
}
}

View File

@ -1,51 +1,44 @@
/* #include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
* OutputValues.h
* #include "../controllerdefinitions/AcsCtrlDefinitions.h"
* Created on: 30 Mar 2022
* Author: rooob
*/
#ifndef OUTPUTVALUES_H_ #ifndef OUTPUTVALUES_H_
#define OUTPUTVALUES_H_ #define OUTPUTVALUES_H_
namespace ACS { namespace ACS {
class OutputValues { class OutputValues {
public:
OutputValues();
virtual ~OutputValues();
public: double magFieldEst[3]; // sensor fusion (G) // output value
bool magFieldEstValid;
double magFieldModel[3]; // igrf (IJK) //
bool magFieldModelValid;
double magneticFieldVectorDerivative[3];
bool magneticFieldVectorDerivativeValid;
OutputValues(); bool mgmUpdated; // ToDo: ????
virtual ~OutputValues();
double magFieldEst[3]; // sensor fusion (G) double sunDirEst[3]; // sensor fusion (G)
bool magFieldEstValid; bool sunDirEstValid;
double magFieldModel[3]; //igrf (IJK) double sunDirModel[3]; // sun model (IJK)
bool magFieldModelValid; bool sunDirModelValid;
double magneticFieldVectorDerivative[3];
bool magneticFieldVectorDerivativeValid;
bool mgmUpdated; double quatMekfBJ[4]; // mekf
bool quatMekfBJValid;
double sunDirEst[3]; // sensor fusion (G) double satRateEst[3];
bool sunDirEstValid; bool satRateEstValid;
double sunDirModel[3]; //sun model (IJK) double satRateMekf[3]; // after mekf with correction of bias
bool sunDirModelValid; bool satRateMekfValid;
double sunVectorDerivative[3];
bool sunVectorDerivativeValid;
double quatMekfBJ[4]; //mekf double gcLatitude; // geocentric latitude, radian
bool quatMekfBJValid; double gdLongitude; // Radian longitude
double satRateEst[3];
bool satRateEstValid;
double satRateMekf[3]; // after mekf with correction of bias
bool satRateMekfValid;
double sunVectorDerivative[3];
bool sunVectorDerivativeValid;
double gcLatitude; // geocentric latitude, radian
double gdLongitude; // Radian longitude
}; };
} } // namespace ACS
#endif /*OUTPUTVALUES_H_*/ #endif /*OUTPUTVALUES_H_*/

View File

@ -15,7 +15,7 @@ enum SetIds : uint32_t {
SUS_PROCESSED_DATA, SUS_PROCESSED_DATA,
GYR_SENSOR_DATA, GYR_SENSOR_DATA,
GYR_PROCESSED_DATA, GYR_PROCESSED_DATA,
GPS_SENSOR_DATA, GPS_PROCESSED_DATA,
MEKF_DATA, MEKF_DATA,
ACTUATOR_CMD_DATA ACTUATOR_CMD_DATA
}; };
@ -214,8 +214,7 @@ class GyrDataProcessed : public StaticLocalDataSet<GYR_SET_PROCESSED_ENTRIES> {
class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> { class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
public: public:
GpsDataProcessed(HasLocalDataPoolIF* hkOwner) GpsDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GPS_PROCESSED_DATA) {}
: StaticLocalDataSet(hkOwner, GPS_SET_PROCESSED_ENTRIES) {}
lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this); lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this); lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);