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

frmt
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-01-16 15:43:40 +01:00
parent 8fa1d69db3
commit 673c24a34d
1 changed files with 544 additions and 548 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
mission/controller/AcsController.cpp
View File

@ -26,7 +26,7 @@ AcsController::AcsController(object_id_t objectId)
ctrlValData(this),
actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
ReturnValue_t AcsController::handleCommandMessage(CommandMessage* message) {
ReturnValue_t result = actionHelper.handleActionMessage(message);
if (result == returnvalue::OK) {
return result;
@ -103,9 +103,9 @@ void AcsController::performControlOperation() {
copyGyrData();
}
}
}
}
void AcsController::performSafe() {
void AcsController::performSafe() {
ACS::SensorValues sensorValues;
timeval now;
@ -125,19 +125,17 @@ void AcsController::performControlOperation() {
bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) {
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value,
mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value,
susDataProcessed.isValid(), mekfData.satRotRateMekf.value,
mekfData.satRotRateMekf.isValid(), sunTargetDir, satRateSafe, &errAng,
magMomMtq, &magMomMtqValid);
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
} else {
safeCtrl.safeNoMekf(
now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
susDataProcessed.susVecTotDerivative.value,
susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), sunTargetDir, satRateSafe, &errAng,
magMomMtq, &magMomMtqValid);
susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
}
double dipolCmdUnits[3] = {0, 0, 0};
@ -194,9 +192,9 @@ void AcsController::performControlOperation() {
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
// torqueDuration);
// }
}
}
void AcsController::performDetumble() {
void AcsController::performDetumble() {
ACS::SensorValues sensorValues;
timeval now;
@ -209,9 +207,9 @@ void AcsController::performControlOperation() {
&mekfData, &validMekf);
double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
@ -254,9 +252,9 @@ void AcsController::performControlOperation() {
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
}
}
void AcsController::performPointingCtrl() {
void AcsController::performPointingCtrl() {
ACS::SensorValues sensorValues;
timeval now;
@ -275,12 +273,12 @@ void AcsController::performControlOperation() {
targetQuat, refSatRate);
break;
case SUBMODE_PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
targetQuat, refSatRate);
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate);
break;
case SUBMODE_PTG_TARGET_GS:
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
now, targetQuat, refSatRate);
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
targetQuat, refSatRate);
break;
case SUBMODE_PTG_NADIR:
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
@ -313,8 +311,7 @@ void AcsController::performControlOperation() {
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
}
double cmdSpeedRws[4] = {0, 0, 0,
0}; // Should be given to the actuator reaction wheel as input
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value),
@ -352,10 +349,10 @@ void AcsController::performControlOperation() {
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
}
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool & localDataPoolMap,
LocalDataPoolManager & poolManager) {
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
// MGM Raw
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
@ -439,9 +436,9 @@ void AcsController::performControlOperation() {
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0});
return returnvalue::OK;
}
}
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
LocalPoolDataSetBase* AcsController::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
case acsctrl::MGM_SENSOR_DATA:
return &mgmDataRaw;
@ -467,10 +464,10 @@ void AcsController::performControlOperation() {
return nullptr;
}
return nullptr;
}
}
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t * msToReachTheMode) {
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) {
if (mode == MODE_OFF) {
if (submode == SUBMODE_NONE) {
return returnvalue::OK;
@ -485,9 +482,9 @@ void AcsController::performControlOperation() {
}
}
return INVALID_MODE;
}
}
void AcsController::copyMgmData() {
void AcsController::copyMgmData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
@ -528,13 +525,12 @@ void AcsController::performControlOperation() {
3 * sizeof(float));
mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
mgmDataRaw.actuationCalStatus.setValid(
sensorValues.imtqMgmSet.coilActuationStatus.isValid());
}
mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
}
}
}
void AcsController::copySusData() {
void AcsController::copySusData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.susSets[0]);
@ -632,9 +628,9 @@ void AcsController::performControlOperation() {
susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid());
}
}
}
}
void AcsController::copyGyrData() {
void AcsController::copyGyrData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.gyr0AdisSet);
@ -680,4 +676,4 @@ void AcsController::performControlOperation() {
sensorValues.gyr3L3gSet.angVelocZ.isValid());
}
}
}
}