eive/eive-obsw
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
mission/acs/defs.h
View File

@ -41,8 +41,9 @@ enum ControlModeStrategy : uint8_t {
SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_FULL = 20,
SAFECTRL_DETUMBLE_DETERIORATED = 21, SAFECTRL_DETUMBLE_DETERIORATED = 21,
// Added in vNext // Added in vNext
PTGCTRL_ACTIVE_MEKF = 100, PTGCTRL_MEKF = 100,
PTGCTRL_WITHOUT_MEKF = 101, PTGCTRL_STR = 101,
PTGCTRL_QUEST = 102,
}; };
enum GpsSource : uint8_t { enum GpsSource : uint8_t {

112
mission/controller/AcsController.cpp
View File

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

16
mission/controller/AcsController.h
View File

@ -14,6 +14,7 @@
#include <mission/acs/rwHelpers.h> #include <mission/acs/rwHelpers.h>
#include <mission/acs/susMax1227Helpers.h> #include <mission/acs/susMax1227Helpers.h>
#include <mission/controller/acs/ActuatorCmd.h> #include <mission/controller/acs/ActuatorCmd.h>
#include <mission/controller/acs/AttitudeEstimation.h>
#include <mission/controller/acs/FusedRotationEstimation.h> #include <mission/controller/acs/FusedRotationEstimation.h>
#include <mission/controller/acs/Guidance.h> #include <mission/controller/acs/Guidance.h>
#include <mission/controller/acs/MultiplicativeKalmanFilter.h> #include <mission/controller/acs/MultiplicativeKalmanFilter.h>
@ -57,6 +58,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
AcsParameters acsParameters; AcsParameters acsParameters;
SensorProcessing sensorProcessing; SensorProcessing sensorProcessing;
AttitudeEstimation attitudeEstimation;
FusedRotationEstimation fusedRotationEstimation; FusedRotationEstimation fusedRotationEstimation;
Navigation navigation; Navigation navigation;
ActuatorCmd actuatorCmd; ActuatorCmd actuatorCmd;
@ -218,11 +220,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> gpsVelocity = PoolEntry<double>(3); PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>(); PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
// MEKF // Attitude Estimation
acsctrl::MekfData mekfData; acsctrl::AttitudeEstimationData attitudeEstimationData;
PoolEntry<double> quatMekf = PoolEntry<double>(4); PoolEntry<double> quatMekf = PoolEntry<double>(4);
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3); PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>(); PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
PoolEntry<double> quatQuest = PoolEntry<double>(4);
// Ctrl Values // Ctrl Values
acsctrl::CtrlValData ctrlValData; acsctrl::CtrlValData ctrlValData;
@ -243,6 +246,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3); PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
PoolEntry<double> rotRateParallel = PoolEntry<double>(3); PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
PoolEntry<double> rotRateTotal = PoolEntry<double>(3); PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
// Fused Rot Rate Sources
acsctrl::FusedRotRateSourcesData fusedRotRateSourcesData;
PoolEntry<double> rotRateOrthogonalSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateParallelSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalSusMgm = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalQuest = PoolEntry<double>(3);
PoolEntry<double> rotRateTotalStr = PoolEntry<double>(3);
// TLE Dataset // TLE Dataset
acsctrl::TleData tleData; acsctrl::TleData tleData;

6
mission/controller/acs/AcsParameters.cpp
View File

@ -29,6 +29,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x2: case 0x2:
parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse); parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse);
break; break;
case 0x3:
parameterWrapper->set(onBoardParams.fusedRateFromStr);
break;
case 0x4:
parameterWrapper->set(onBoardParams.fusedRateFromQuest);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }

3
mission/controller/acs/AcsParameters.h
View File

@ -20,6 +20,8 @@ class AcsParameters : public HasParametersIF {
double sampleTime = 0.4; // [s] double sampleTime = 0.4; // [s]
uint16_t ptgCtrlLostTimer = 750; uint16_t ptgCtrlLostTimer = 750;
uint8_t fusedRateSafeDuringEclipse = true; uint8_t fusedRateSafeDuringEclipse = true;
uint8_t fusedRateFromStr = false;
uint8_t fusedRateFromQuest = false;
} onBoardParams; } onBoardParams;
struct InertiaEIVE { struct InertiaEIVE {
@ -910,7 +912,6 @@ class AcsParameters : public HasParametersIF {
struct StrParameters { struct StrParameters {
double exclusionAngle = 20 * M_PI / 180; double exclusionAngle = 20 * M_PI / 180;
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame
uint8_t useStrForRotRate = true;
} strParameters; } strParameters;
struct GpsParameters { struct GpsParameters {

100
mission/controller/acs/FusedRotationEstimation.cpp
View File

@ -7,32 +7,73 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
void FusedRotationEstimation::estimateFusedRotationRate( void FusedRotationEstimation::estimateFusedRotationRate(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
if (acsParameters->strParameters.useStrForRotRate and acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and acsctrl::FusedRotRateData *fusedRotRateData) {
estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData);
estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData);
// Fused Rotation Estimation of STR
if ((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
if (VectorOperations<double>::norm(quatOld, 4) != 0 and timeDelta != 0) { if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData); estimateFusedRotationRateQuat(quatNew, timeDelta, true, fusedRotRateSourcesData);
} else { } else {
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, {
fusedRotRateData); PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
} }
std::memcpy(quatOld, quatNew, sizeof(quatOld)); std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
} else { } else {
std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld)); std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, {
fusedRotRateData); PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
} }
// Fused Rotation Estimation of QUEST
if ((attitudeEstimationData->quatQuest.isValid())) {
if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateQuat(attitudeEstimationData->quatQuest.value, timeDelta, true,
fusedRotRateSourcesData);
} else {
{
PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
} else {
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
{
PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
// Fused Rotation Estimation of SUS&MGM
} }
void FusedRotationEstimation::estimateFusedRotationRateStr( void FusedRotationEstimation::estimateFusedRotationRateQuat(
double *quatNew, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { double *quatNew, const double timeDelta, const bool str,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
double quatOldInv[4] = {0, 0, 0, 0}; double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0}; double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOld, quatOldInv); if (str) {
QuaternionOperations::inverse(quatOldStr, quatOldInv);
} else {
QuaternionOperations::inverse(quatOldQuest, quatOldInv);
}
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
QuaternionOperations::normalize(quatDelta); QuaternionOperations::normalize(quatDelta);
@ -40,25 +81,28 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
double angle = QuaternionOperations::getAngle(quatDelta); double angle = QuaternionOperations::getAngle(quatDelta);
if (angle == 0.0) { if (angle == 0.0) {
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); if (str) {
fusedRotRateData->rotRateOrthogonal.setValid(false); std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
fusedRotRateData->rotRateParallel.setValid(false); } else {
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
fusedRotRateData->rotRateTotal.setValid(true); 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
} }
} }
VectorOperations<double>::normalize(quatDelta, rotVec, 3); VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3); VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); if (str) {
fusedRotRateData->rotRateOrthogonal.setValid(false); std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
fusedRotRateData->rotRateParallel.setValid(false); } else {
std::memcpy(fusedRotRateData->rotRateTotal.value, rotVec, 3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true); fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
} }
} }

25
mission/controller/acs/FusedRotationEstimation.h
View File

@ -15,28 +15,33 @@ class FusedRotationEstimation {
void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
ACS::SensorValues *sensorValues, const double timeDelta, ACS::SensorValues *sensorValues,
acsctrl::AttitudeEstimationData *attitudeEstimationData,
const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
acsctrl::FusedRotRateData *fusedRotRateData); acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateStr(double *quatNew, const double timeDelta,
acsctrl::FusedRotRateData *fusedRotRateData);
protected: protected:
private: private:
static constexpr double ZERO_VEC3[3] = {0, 0, 0}; static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
AcsParameters *acsParameters; AcsParameters *acsParameters;
double quatOld[3] = {0, 0, 0, 0}; double quatOldQuest[4] = {0, 0, 0, 0};
double quatOldStr[4] = {0, 0, 0, 0};
double rotRateOldB[3] = {0, 0, 0}; double rotRateOldB[3] = {0, 0, 0};
void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData); acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData,
const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateStr(ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
}; };
#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */ #endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */

10
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -19,7 +19,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
ReturnValue_t MultiplicativeKalmanFilter::init( ReturnValue_t MultiplicativeKalmanFilter::init(
const double *magneticField_, const bool validMagField_, const double *sunDir_, const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) { // valids for "model measurements"? AcsParameters *acsParameters) { // valids for "model measurements"?
// check for valid mag/sun // check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) { if (validMagField_ && validSS && validSSModel && validMagModel) {
@ -191,7 +191,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_, const bool validMagField_, const bool validGYRs_, const double *magneticField_, const bool validMagField_,
const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters) { AcsParameters *acsParameters) {
// Check for GYR Measurements // Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor int MDF = 0; // Matrix Dimension Factor
@ -1090,7 +1090,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
return MEKF_RUNNING; return MEKF_RUNNING;
} }
ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::AttitudeEstimationData *mekfData) {
double resetQuaternion[4] = {0, 0, 0, 1}; double resetQuaternion[4] = {0, 0, 0, 1};
double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
@ -1100,7 +1100,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
return MEKF_UNINITIALIZED; return MEKF_UNINITIALIZED;
} }
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData,
MekfStatus mekfStatus) { MekfStatus mekfStatus) {
{ {
PoolReadGuard pg(mekfData); PoolReadGuard pg(mekfData);
@ -1115,7 +1115,7 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek
} }
} }
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
double quat[4], double satRotRate[3]) { double quat[4], double satRotRate[3]) {
{ {
PoolReadGuard pg(mekfData); PoolReadGuard pg(mekfData);

10
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

@ -21,7 +21,7 @@ class MultiplicativeKalmanFilter {
MultiplicativeKalmanFilter(); MultiplicativeKalmanFilter();
virtual ~MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter();
ReturnValue_t reset(acsctrl::MekfData *mekfData); ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
/* @brief: init() - This function initializes the Kalman Filter and will provide the first /* @brief: init() - This function initializes the Kalman Filter and will provide the first
* quaternion through the QUEST algorithm * quaternion through the QUEST algorithm
@ -32,7 +32,7 @@ class MultiplicativeKalmanFilter {
*/ */
ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_, ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel, const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters); AcsParameters *acsParameters);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
@ -53,7 +53,7 @@ class MultiplicativeKalmanFilter {
const bool validGYRs_, const double *magneticField_, const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS, const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters); AcsParameters *acsParameters);
enum MekfStatus : uint8_t { enum MekfStatus : uint8_t {
@ -99,8 +99,8 @@ class MultiplicativeKalmanFilter {
double biasGYR[3]; /*Between measured and estimated sat Rate*/ double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/ /*Parameter INIT*/
/*Functions*/ /*Functions*/
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]); double satRotRate[3]);
}; };

4
mission/controller/acs/Navigation.cpp
View File

@ -16,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MekfData *mekfData, AcsParameters *acsParameters) { acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters) {
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
@ -41,7 +41,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
} }
} }
void Navigation::resetMekf(acsctrl::MekfData *mekfData) { void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
mekfStatus = multiplicativeKalmanFilter.reset(mekfData); mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
} }

4
mission/controller/acs/Navigation.h
View File

@ -17,9 +17,9 @@ class Navigation {
ReturnValue_t useMekf(ACS::SensorValues *sensorValues, ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::AttitudeEstimationData *mekfData,
AcsParameters *acsParameters); AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData); void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2); ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);

33
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -10,36 +10,17 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(const bool magFieldValid,
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool mekfValid, const bool strValid,
const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const bool fusedRateValid,
const uint8_t gyrEnabled, const uint8_t dampingEnabled) { const uint8_t mekfEnabled,
const uint8_t strRateEnabled) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) { } else if (mekfEnabled and mekfValid) {
return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF; return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF;
} else if (sunDirValid) { } else if (strValid and strRateEnabled and fusedRateValid) {
if (gyrEnabled and satRotRateValid) { return acs::ControlModeStrategy::PTGCTRL_WITHOUT_MEKF;
return acs::ControlModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not sunDirValid) {
if (dampingEnabled) {
if (gyrEnabled and satRotRateValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not dampingEnabled and satRotRateValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else { } else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }

8
mission/controller/acs/control/PtgCtrl.h
View File

@ -22,10 +22,10 @@ class PtgCtrl {
PtgCtrl(AcsParameters *acsParameters_); PtgCtrl(AcsParameters *acsParameters_);
virtual ~PtgCtrl(); virtual ~PtgCtrl();
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool strValid, const bool fusedRateValid,
const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled); const uint8_t strRateEnabled);
/* @brief: Calculates the needed torque for the pointing control mechanism /* @brief: Calculates the needed torque for the pointing control mechanism
*/ */

30
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -20,6 +20,7 @@ enum SetIds : uint32_t {
CTRL_VAL_DATA, CTRL_VAL_DATA,
ACTUATOR_CMD_DATA, ACTUATOR_CMD_DATA,
FUSED_ROTATION_RATE_DATA, FUSED_ROTATION_RATE_DATA,
FUSED_ROTATION_RATE_SOURCES_DATA,
TLE_SET, TLE_SET,
}; };
@ -111,6 +112,13 @@ enum PoolIds : lp_id_t {
ROT_RATE_ORTHOGONAL, ROT_RATE_ORTHOGONAL,
ROT_RATE_PARALLEL, ROT_RATE_PARALLEL,
ROT_RATE_TOTAL, ROT_RATE_TOTAL,
ROT_RATE_SOURCE,
// Fused Rotation Rate Sources
ROT_RATE_ORTHOGONAL_SUSMGM,
ROT_RATE_PARALLEL_SUSMGM,
ROT_RATE_TOTAL_SUSMGM,
ROT_RATE_TOTAL_QUEST,
ROT_RATE_TOTAL_STR,
// TLE // TLE
TLE_LINE_1, TLE_LINE_1,
TLE_LINE_2, TLE_LINE_2,
@ -126,7 +134,8 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4; static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3; static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 4;
static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
static constexpr uint8_t TLE_SET_ENTRIES = 2; static constexpr uint8_t TLE_SET_ENTRIES = 2;
/** /**
@ -297,6 +306,25 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this); lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this); lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this); lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
private:
};
class FusedRotRateSourcesData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
public:
FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {}
lp_vec_t<double, 3> rotRateOrthogonalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateParallelSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalQuest =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_QUEST, this);
lp_vec_t<double, 3> rotRateTotalStr = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_STR, this);
private: private:
}; };