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

ctrlValData gets updated in every perform function
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-21 11:13:06 +01:00
parent 1a8f9ab4a2
commit b3b736a7bd
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
mission/controller/AcsController.cpp
View File

@ -211,6 +211,7 @@ void AcsController::performDetumble() {
triggerEvent(acs::SAFE_RATE_RECOVERY); triggerEvent(acs::SAFE_RATE_RECOVERY);
} }
disableCtrlValData();
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,
@ -370,6 +371,7 @@ void AcsController::performPointingCtrl() {
int16_t cmdDipolMtqs[3] = {0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
updateCtrlValData(targetQuat, errorQuat, errorAngle);
updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
@ -440,6 +442,7 @@ void AcsController::updateCtrlValData(double errAng) {
} }
} }
} }
void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) { void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) {
{ {
PoolReadGuard pg(&ctrlValData); PoolReadGuard pg(&ctrlValData);
@ -452,6 +455,20 @@ void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], doub
} }
} }
void AcsController::disableCtrlValData() {
double unitQuat[4] = {0, 0, 0, 1};
double errAng = 0;
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(false, true);
}
}
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) { LocalDataPoolManager &poolManager) {
// MGM Raw // MGM Raw

1
mission/controller/AcsController.h
View File

@ -84,6 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
int16_t mtqTargetDipole[3]); int16_t mtqTargetDipole[3]);
void updateCtrlValData(double errAng); void updateCtrlValData(double errAng);
void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng); void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng);
void disableCtrlValData();
/* ACS Sensor Values */ /* ACS Sensor Values */
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;