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

removed unit quats as they are confusing
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-11 10:55:48 +02:00
parent 18be21a310
commit c66799b24f
4 changed files with 16 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
mission/controller/AcsController.cpp
View File

@ -576,13 +576,13 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.tgtRotRate.setValid(false);
ctrlValData.safeStrat.value = safeModeStrat;
ctrlValData.safeStrat.setValid(true);
@ -606,10 +606,10 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
void AcsController::disableCtrlValData() {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
ctrlValData.errAng.value = 0;
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
ctrlValData.setValidity(false, true);
}
}
@ -691,6 +691,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);