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

added function for updating ctrlValData
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-17 13:28:29 +01:00
parent 2f7abb2da8
commit d709e190e2
2 changed files with 31 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
mission/controller/AcsController.cpp
View File

@ -152,20 +152,6 @@ void AcsController::performSafe() {
int16_t cmdDipolMtqs[3] = {0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
double unitQuat[4] = {0, 0, 0, 1};
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
}
// Detumble check and switch // Detumble check and switch
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) > VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
@ -184,6 +170,7 @@ void AcsController::performSafe() {
triggerEvent(acs::SAFE_RATE_VIOLATION); triggerEvent(acs::SAFE_RATE_VIOLATION);
} }
updateCtrlValData(errAng);
updateActuatorCmdData(cmdDipolMtqs); updateActuatorCmdData(cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
@ -224,7 +211,7 @@ void AcsController::performDetumble() {
triggerEvent(acs::SAFE_RATE_RECOVERY); triggerEvent(acs::SAFE_RATE_RECOVERY);
} }
updateActuatorCmdData(nullptr, nullptr, cmdDipolMtqs); updateActuatorCmdData(cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime); // acsParameters.rwHandlingParameters.rampTime);
@ -283,7 +270,6 @@ void AcsController::performPointingCtrl() {
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
break; break;
case acs::PTG_TARGET: case acs::PTG_TARGET:
@ -444,6 +430,33 @@ void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTa
} }
} }
void AcsController::updateCtrlValData(double errAng) {
double unitQuat[4] = {0, 0, 0, 1};
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
}
}
void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) {
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(true, true);
}
}
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) { LocalDataPoolManager &poolManager) {
// MGM Raw // MGM Raw
@ -788,4 +801,3 @@ void AcsController::copyGyrData() {
} }
} }
} }

2
mission/controller/AcsController.h
View File

@ -82,6 +82,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void updateActuatorCmdData(int16_t mtqTargetDipole[3]); void updateActuatorCmdData(int16_t mtqTargetDipole[3]);
void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4], void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]); int16_t mtqTargetDipole[3]);
void updateCtrlValData(double errAng);
void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng);
/* ACS Sensor Values */ /* ACS Sensor Values */
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;