From 4bd2a2dac847ddde2037ebddf70a8c29a55552ff Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 24 Oct 2022 10:41:28 +0200 Subject: [PATCH] added and implemented all ACS related DataPools to AcsController --- mission/controller/AcsController.cpp | 311 ++++++++++++++---- mission/controller/AcsController.h | 128 ++++--- mission/controller/acs/OutputValues.cpp | 16 +- mission/controller/acs/OutputValues.h | 63 ++-- .../AcsCtrlDefinitions.h | 5 +- 5 files changed, 352 insertions(+), 171 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 22c871fd..b42d9c83 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -12,8 +12,15 @@ AcsController::AcsController(object_id_t objectId) 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), + actuatorCmdData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return returnvalue::OK; @@ -38,11 +45,9 @@ void AcsController::performControlOperation() { case SUBMODE_SAFE: performSafe(); break; - case SUBMODE_DETUMBLE: performDetumble(); break; - case SUBMODE_PTG_GS: performPointingCtrl(); break; @@ -55,17 +60,23 @@ 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(); } } + { + PoolReadGuard pg(&gyrDataRaw); + if (pg.getReadResult() == returnvalue::OK) { + copyGyrData(); + } + } // DEBUG : REMOVE AFTER COMPLETION mode = MODE_ON; @@ -81,10 +92,13 @@ void AcsController::performSafe() { ACS::SensorValues sensorValues; 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); 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); @@ -140,10 +154,12 @@ void AcsController::performDetumble() { 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, @@ -172,11 +188,13 @@ void AcsController::performPointingCtrl() { ACS::SensorValues sensorValues; ACS::OutputValues outputValues; - timeval now; // Übergabe ? + 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}; @@ -204,38 +222,106 @@ void AcsController::performPointingCtrl() { 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_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) { - 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::ACTUATOR_CMD_DATA: + return &actuatorCmdData; + default: + return nullptr; } - return nullptr; } 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::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)); - sif::debug << susData.sus9.isValid() << std::endl; - sif::debug << susSets[9].channels.isValid() << std::endl; + 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()); } } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index a8ae9daa..87aaf37c 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -8,6 +8,7 @@ #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" #include "acs/Navigation.h" +#include "acs/OutputValues.h" #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" #include "acs/control/PtgCtrl.h" @@ -64,62 +65,91 @@ class AcsController : public ExtendedControllerBase { void modeChanged(Mode_t mode, Submode_t submode); void announceMode(bool recursive); + /* ACS Datasets */ // MGMs - acsctrl::MgmDataRaw mgmData; - - MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); - 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 mgm0PoolVec = PoolEntry(3); - PoolEntry mgm1PoolVec = PoolEntry(3); - PoolEntry mgm2PoolVec = PoolEntry(3); - PoolEntry mgm3PoolVec = PoolEntry(3); - PoolEntry imtqMgmPoolVec = PoolEntry(3); + acsctrl::MgmDataRaw mgmDataRaw; + PoolEntry mgm0VecRaw = PoolEntry(3); + PoolEntry mgm1VecRaw = PoolEntry(3); + PoolEntry mgm2VecRaw = PoolEntry(3); + PoolEntry mgm3VecRaw = PoolEntry(3); + PoolEntry imtqMgmVecRaw = PoolEntry(3); PoolEntry imtqCalActStatus = PoolEntry(); - void copyMgmData(); - // Sun Sensors - - acsctrl::SusDataRaw susData; - - std::array susSets{ - SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), - SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), - SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), - SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), - 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 sus0PoolVec = PoolEntry(6); - PoolEntry sus1PoolVec = PoolEntry(6); - PoolEntry sus2PoolVec = PoolEntry(6); - PoolEntry sus3PoolVec = PoolEntry(6); - PoolEntry sus4PoolVec = PoolEntry(6); - PoolEntry sus5PoolVec = PoolEntry(6); - PoolEntry sus6PoolVec = PoolEntry(6); - PoolEntry sus7PoolVec = PoolEntry(6); - PoolEntry sus8PoolVec = PoolEntry(6); - PoolEntry sus9PoolVec = PoolEntry(6); - PoolEntry sus10PoolVec = PoolEntry(6); - PoolEntry sus11PoolVec = PoolEntry(6); + acsctrl::MgmDataProcessed mgmDataProcessed; + PoolEntry mgm0VecProc = PoolEntry(3); + PoolEntry mgm1VecProc = PoolEntry(3); + PoolEntry mgm2VecProc = PoolEntry(3); + PoolEntry mgm3VecProc = PoolEntry(3); + PoolEntry mgm4VecProc = PoolEntry(3); + PoolEntry mgmVecTot = PoolEntry(3); + PoolEntry mgmVecTotDer = PoolEntry(3); + PoolEntry magIgrf = PoolEntry(3); + // SUSs + acsctrl::SusDataRaw susDataRaw; + PoolEntry sus0ValRaw = PoolEntry(6); + PoolEntry sus1ValRaw = PoolEntry(6); + PoolEntry sus2ValRaw = PoolEntry(6); + PoolEntry sus3ValRaw = PoolEntry(6); + PoolEntry sus4ValRaw = PoolEntry(6); + PoolEntry sus5ValRaw = PoolEntry(6); + PoolEntry sus6ValRaw = PoolEntry(6); + PoolEntry sus7ValRaw = PoolEntry(6); + PoolEntry sus8ValRaw = PoolEntry(6); + PoolEntry sus9ValRaw = PoolEntry(6); + PoolEntry sus10ValRaw = PoolEntry(6); + PoolEntry sus11ValRaw = PoolEntry(6); void copySusData(); + acsctrl::SusDataProcessed susDataProcessed; + PoolEntry sus0VecProc = PoolEntry(3); + PoolEntry sus1VecProc = PoolEntry(3); + PoolEntry sus2VecProc = PoolEntry(3); + PoolEntry sus3VecProc = PoolEntry(3); + PoolEntry sus4VecProc = PoolEntry(3); + PoolEntry sus5VecProc = PoolEntry(3); + PoolEntry sus6VecProc = PoolEntry(3); + PoolEntry sus7VecProc = PoolEntry(3); + PoolEntry sus8VecProc = PoolEntry(3); + PoolEntry sus9VecProc = PoolEntry(3); + PoolEntry sus10VecProc = PoolEntry(3); + PoolEntry sus11VecProc = PoolEntry(3); + PoolEntry susVecTot = PoolEntry(3); + PoolEntry susVecTotDer = PoolEntry(3); + PoolEntry sunIjk = PoolEntry(3); + + // GYRs + acsctrl::GyrDataRaw gyrDataRaw; + PoolEntry gyr0VecRaw = PoolEntry(3); + PoolEntry gyr1VecRaw = PoolEntry(3); + PoolEntry gyr2VecRaw = PoolEntry(3); + PoolEntry gyr3VecRaw = PoolEntry(3); + void copyGyrData(); + + acsctrl::GyrDataProcessed gyrDataProcessed; + PoolEntry gyr0VecProc = PoolEntry(3); + PoolEntry gyr1VecProc = PoolEntry(3); + PoolEntry gyr2VecProc = PoolEntry(3); + PoolEntry gyr3VecProc = PoolEntry(3); + PoolEntry gyrVecTot = PoolEntry(3); + + // GPS + acsctrl::GpsDataProcessed gpsDataProcessed; + PoolEntry gcLatitude = PoolEntry(); + PoolEntry gdLongitude = PoolEntry(); + + // MEKF + acsctrl::MekfData mekfData; + PoolEntry quatMekf = PoolEntry(4); + PoolEntry satRotRateMekf = PoolEntry(3); + + // Actuator CMD + acsctrl::ActuatorCmdData actuatorCmdData; + PoolEntry rwTargetTorque = PoolEntry(4); + PoolEntry rwTargetSpeed = PoolEntry(4); + PoolEntry mtqTargetDipole = PoolEntry(3); + // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/controller/acs/OutputValues.cpp b/mission/controller/acs/OutputValues.cpp index d035f621..42730fb3 100644 --- a/mission/controller/acs/OutputValues.cpp +++ b/mission/controller/acs/OutputValues.cpp @@ -1,19 +1,9 @@ -/* - * OutputValues.cpp - * - * Created on: 30 Mar 2022 - * Author: rooob - */ #include "OutputValues.h" namespace ACS { -OutputValues::OutputValues(){ +OutputValues::OutputValues() {} -} +OutputValues::~OutputValues() {} -OutputValues::~OutputValues(){ - -} - -} +} // namespace ACS diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h index c9fc71f1..bd182f42 100644 --- a/mission/controller/acs/OutputValues.h +++ b/mission/controller/acs/OutputValues.h @@ -1,51 +1,44 @@ -/* - * OutputValues.h - * - * Created on: 30 Mar 2022 - * Author: rooob - */ +#include + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" #ifndef OUTPUTVALUES_H_ #define OUTPUTVALUES_H_ - namespace ACS { 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(); - virtual ~OutputValues(); + bool mgmUpdated; // ToDo: ???? - double magFieldEst[3]; // sensor fusion (G) - bool magFieldEstValid; - double magFieldModel[3]; //igrf (IJK) - bool magFieldModelValid; - double magneticFieldVectorDerivative[3]; - bool magneticFieldVectorDerivativeValid; + double sunDirEst[3]; // sensor fusion (G) + bool sunDirEstValid; + double sunDirModel[3]; // sun model (IJK) + bool sunDirModelValid; - bool mgmUpdated; + double quatMekfBJ[4]; // mekf + bool quatMekfBJValid; - double sunDirEst[3]; // sensor fusion (G) - bool sunDirEstValid; - double sunDirModel[3]; //sun model (IJK) - bool sunDirModelValid; + double satRateEst[3]; + bool satRateEstValid; + double satRateMekf[3]; // after mekf with correction of bias + bool satRateMekfValid; + double sunVectorDerivative[3]; + bool sunVectorDerivativeValid; - double quatMekfBJ[4]; //mekf - bool quatMekfBJValid; - - 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 + double gcLatitude; // geocentric latitude, radian + double gdLongitude; // Radian longitude }; -} - +} // namespace ACS #endif /*OUTPUTVALUES_H_*/ diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 4a608f71..bb0a40ae 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -15,7 +15,7 @@ enum SetIds : uint32_t { SUS_PROCESSED_DATA, GYR_SENSOR_DATA, GYR_PROCESSED_DATA, - GPS_SENSOR_DATA, + GPS_PROCESSED_DATA, MEKF_DATA, ACTUATOR_CMD_DATA }; @@ -214,8 +214,7 @@ class GyrDataProcessed : public StaticLocalDataSet { class GpsDataProcessed : public StaticLocalDataSet { public: - GpsDataProcessed(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, GPS_SET_PROCESSED_ENTRIES) {} + GpsDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GPS_PROCESSED_DATA) {} lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this);