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