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

this is a mess
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-11-23 16:56:36 +01:00
parent 886dd17e4a
commit 4a67f9ffe5
14 changed files with 234 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
mission/controller/AcsController.cpp
View File

@ -7,6 +7,7 @@
AcsController::AcsController(object_id_t objectId, bool enableHkSets)
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
attitudeEstimation(&acsParameters),
fusedRotationEstimation(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
@ -19,10 +20,11 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
gyrDataRaw(this),
gyrDataProcessed(this),
gpsDataProcessed(this),
mekfData(this),
attitudeEstimationData(this),
ctrlValData(this),
actuatorCmdData(this),
fusedRotRateData(this),
fusedRotRateSourcesData(this),
tleData(this) {}
ReturnValue_t AcsController::initialize() {
@ -56,7 +58,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
return HasActionsIF::EXECUTION_FINISHED;
}
case RESET_MEKF: {
navigation.resetMekf(&mekfData);
navigation.resetMekf(&attitudeEstimationData);
return HasActionsIF::EXECUTION_FINISHED;
}
case RESTORE_MEKF_NONFINITE_RECOVERY: {
@ -164,20 +166,23 @@ void AcsController::performAttitudeControl() {
sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
&susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
fusedRotationEstimation.estimateFusedRotationRate(
&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
&susDataProcessed, &attitudeEstimationData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO, static_cast<uint32_t>(mekfData.mekfStatus.value));
triggerEvent(acs::MEKF_INVALID_INFO,
static_cast<uint32_t>(attitudeEstimationData.mekfStatus.value));
mekfInvalidFlag = true;
}
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData);
navigation.resetMekf(&attitudeEstimationData);
mekfLost = true;
}
} else if (mekfInvalidFlag) {
@ -220,9 +225,10 @@ void AcsController::performSafe() {
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) {
case (acs::ControlModeStrategy::SAFECTRL_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
magMomMtq, errAng);
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value,
attitudeEstimationData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value,
sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
@ -275,8 +281,8 @@ void AcsController::performSafe() {
// detumble check and switch
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
@ -336,8 +342,8 @@ void AcsController::performDetumble() {
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
@ -369,20 +375,24 @@ void AcsController::performDetumble() {
}
void AcsController::performPointingCtrl() {
bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
acs::ControlModeStrategy ptgCtrlStrat =
ptgCtrl.pointingCtrlStrategy(magFieldValid, mekfValid, satRotRateValid, sunDirValid,
fusedRateTotalValid, mekfEnabled, gyrEnabled, dampingEnabled);
ptgCtrl.pointingCtrlStrategy(mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
strValid, fusedRotRateData.rotRateTotal.isValid(), mekfEnabled,
acsParameters.strParameters.useStrForRotRate);
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
ptgCtrlLostCounter++;
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
ptgCtrlLostCounter = 0;
}
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
acsParameters.rwHandlingParameters.rampTime);
}
// if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
// ptgCtrlLostCounter++;
// if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
// triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
// ptgCtrlLostCounter = 0;
// }
// commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration,
// cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
// acsParameters.rwHandlingParameters.rampTime);
// }
uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -410,8 +420,9 @@ void AcsController::performPointingCtrl() {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
@ -422,7 +433,7 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
@ -432,8 +443,9 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
acsParameters.targetModeControllerParameters.quatRef,
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
@ -446,7 +458,7 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
@ -455,8 +467,9 @@ void AcsController::performPointingCtrl() {
case acs::PTG_TARGET_GS:
guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
@ -467,7 +480,7 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
@ -477,8 +490,9 @@ void AcsController::performPointingCtrl() {
guidance.targetQuatPtgNadirThreeAxes(
timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
acsParameters.nadirModeControllerParameters.quatRef,
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
@ -491,7 +505,7 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
@ -500,8 +514,9 @@ void AcsController::performPointingCtrl() {
case acs::PTG_INERTIAL:
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
sizeof(targetQuat));
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
guidance.comparePtg(attitudeEstimationData.quatMekf.value,
attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate,
acsParameters.inertialModeControllerParameters.quatRef,
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
@ -514,7 +529,7 @@ void AcsController::performPointingCtrl() {
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
@ -732,11 +747,12 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
// MEKF
// Attitude Estimation
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0});
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
@ -753,7 +769,15 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
// Fused Rot Rate Sources
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
// TLE Data
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1);
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2);
@ -777,7 +801,7 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
case acsctrl::GPS_PROCESSED_DATA:
return &gpsDataProcessed;
case acsctrl::MEKF_DATA:
return &mekfData;
return &attitudeEstimationData;
case acsctrl::CTRL_VAL_DATA:
return &ctrlValData;
case acsctrl::ACTUATOR_CMD_DATA: