eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

Merge branch 'develop' into acs-flp-safe
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-04-05 11:05:53 +02:00
parent 96bd188e57 858c6c301e
commit 4ca8c38c98
335 changed files with 4742 additions and 3096 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
mission/controller/AcsController.cpp
View File

@ -1,11 +1,12 @@
#include "AcsController.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/acsDefs.h>
#include <mission/acs/defs.h>
#include <mission/config/torquer.h>
AcsController::AcsController(object_id_t objectId)
AcsController::AcsController(object_id_t objectId, bool enableHkSets)
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
guidance(&acsParameters),
safeCtrl(&acsParameters),
ptgCtrl(&acsParameters),
@ -591,7 +592,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
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});
poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 10.0});
// MGM Processed
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
@ -601,7 +602,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
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});
poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 10.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);
@ -615,7 +616,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
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});
poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 10.0});
// SUS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
@ -632,43 +633,43 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
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});
poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 10.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.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 5.0});
poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 10.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.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0});
poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 10.0});
// GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
// MEKF
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0});
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 10.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});
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
return returnvalue::OK;
}