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

Final Version of the ACS Controller #367

Merged
muellerr merged 78 commits from eggert/acs into develop 2023-02-08 13:50:11 +01:00
Conversation 1 Commits 78 Files Changed 27 +3160 -4253
116 changed files with 3160 additions and 4253 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 673c24a34d - Show all commits

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());
} }
} }
} }