diff --git a/CHANGELOG.md b/CHANGELOG.md index 53beac03..34a7850c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,7 +14,7 @@ will consitute of a breaking change warranting a new major release: - The TMTC interface changes in any shape of form. - The behaviour of the OBSW changes in a major shape or form relevant for operations -# [v7.5.0] 2023-12-XX +# [v7.5.0] 2023-12-06 ## Changed @@ -26,6 +26,13 @@ will consitute of a breaking change warranting a new major release: - Added action cmd to read the currently stored TLE. - Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate the time difference. + +## Added + +- Higher ACS modes can now be entered without a running `MEKF`. Higher modes will collect their + quaternion and rotational rate depending on the available sources. +- `QUEST` attitude estimation was added to the `AcsController`. +- The fused rotational rate can now be estimated from `QUEST` and the `STR`. # [v7.4.1] 2023-12-06 @@ -123,6 +130,16 @@ will consitute of a breaking change warranting a new major release: during which the SUS was not working as well as the maximum amount of invalid messages. - Updated battery internal resistance to new value +## Changed + +- `Power Controller` now uses monotonic clock for calculating time difference +- `ACS Controller` now uses monotonic clock for calculating time difference and the normal clock + for model calculations. The `timeDelta` is now calculated in the controller instead of + everywhere where it is needed. +- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing + on to the relevant mode functions. It handles all telemetry relevant functions, which were + always called, regardless of the mode. + # [v7.1.0] 2023-10-11 - Bumped `eive-tmtc` to v5.8.0. diff --git a/fsfw b/fsfw index 7187f2b5..7105e199 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 7187f2b5cdfe163bf7ed1a8fab48900d69f4c8bf +Subproject commit 7105e199c650303ac1a48e75aebc44182630931e diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 91030839..3a62b51f 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -22,10 +22,10 @@ enum AcsMode : Mode_t { enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; -enum SafeModeStrategy : uint8_t { - SAFECTRL_OFF = 0, - SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, - SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, +enum ControlModeStrategy : uint8_t { + CTRL_OFF = 0, + CTRL_NO_MAG_FIELD_FOR_CONTROL = 1, + CTRL_NO_SENSORS_FOR_CONTROL = 2, // OBSW version <= v6.1.0 LEGACY_SAFECTRL_ACTIVE_MEKF = 10, LEGACY_SAFECTRL_WITHOUT_MEKF = 11, @@ -40,14 +40,28 @@ enum SafeModeStrategy : uint8_t { SAFECTRL_ECLIPSE_IDELING = 19, SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_DETERIORATED = 21, + // Added in vNext + PTGCTRL_MEKF = 100, + PTGCTRL_STR = 101, + PTGCTRL_QUEST = 102, }; - -enum GpsSource : uint8_t { +namespace gps { +enum Source : uint8_t { NONE, GPS, GPS_EXTRAPOLATED, SPG4, }; +} + +namespace rotrate { +enum Source : uint8_t { + NONE, + SUSMGM, + QUEST, + STR, +}; +} static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. @@ -64,9 +78,9 @@ static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO); static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO); //! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values. static constexpr Event MEKF_AUTOMATIC_RESET = MAKE_EVENT(5, severity::INFO); -//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution during any pointing ACS mode for a -//! prolonged time. -static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH); +//! [EXPORT] : [COMMENT] For a prolonged time, no attitude information was available for the +//! Pointing Controller. Falling back to Safe Mode. +static constexpr Event PTG_CTRL_NO_ATTITUDE_INFORMATION = MAKE_EVENT(6, severity::HIGH); //! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has //! failed. //! P1: Missing information about magnetic field, P2: Missing information about rotational rate diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 45877421..6adb4a0b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -4,6 +4,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), sdcMan(sdcMan), + attitudeEstimation(&acsParameters), fusedRotationEstimation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), @@ -16,10 +17,11 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun gyrDataRaw(this), gyrDataProcessed(this), gpsDataProcessed(this), - mekfData(this), + attitudeEstimationData(this), ctrlValData(this), actuatorCmdData(this), - fusedRotRateData(this) {} + fusedRotRateData(this), + fusedRotRateSourcesData(this) {} ReturnValue_t AcsController::initialize() { ReturnValue_t result = parameterHelper.initialize(); @@ -52,7 +54,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: { @@ -169,28 +171,32 @@ 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, &fusedRotRateSourcesData, &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(attitudeEstimationData.mekfStatus.value)); + mekfInvalidFlag = true; + } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { + triggerEvent(acs::MEKF_AUTOMATIC_RESET); + navigation.resetMekf(&attitudeEstimationData); + mekfLost = true; + } + } else if (mekfInvalidFlag) { + triggerEvent(acs::MEKF_RECOVERY); + mekfInvalidFlag = false; + } switch (mode) { case acs::SAFE: - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and - result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { - if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); - mekfInvalidFlag = true; - } - if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { - triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); - mekfLost = true; - } - } else if (mekfInvalidFlag) { - triggerEvent(acs::MEKF_RECOVERY); - mekfInvalidFlag = false; - } switch (submode) { case SUBMODE_NONE: performSafe(); @@ -205,35 +211,6 @@ void AcsController::performAttitudeControl() { case acs::PTG_TARGET_GS: case acs::PTG_NADIR: case acs::PTG_INERTIAL: - if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and - result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { - mekfInvalidCounter++; - if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); - mekfInvalidFlag = true; - } - if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) { - triggerEvent(acs::MEKF_AUTOMATIC_RESET); - navigation.resetMekf(&mekfData); - mekfLost = true; - } - if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) { - // Trigger this so STR FDIR can set the device faulty. - EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, - 0); - mekfInvalidCounter = 0; - } - commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, - cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - acsParameters.rwHandlingParameters.rampTime); - return; - } else { - if (mekfInvalidFlag) { - triggerEvent(acs::MEKF_RECOVERY); - mekfInvalidFlag = false; - } - mekfInvalidCounter = 0; - } performPointingCtrl(); break; } @@ -245,27 +222,28 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( + acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_MEKF): - safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, - susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, - magMomMtq, errAng); + case (acs::ControlModeStrategy::SAFECTRL_MEKF): + safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, + attitudeEstimationData.satRotRateMekf.value, + susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value, + sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_GYR): + case (acs::ControlModeStrategy::SAFECTRL_GYR): safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): + case (acs::ControlModeStrategy::SAFECTRL_SUSMGM): safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, fusedRotRateData.rotRateParallel.value, fusedRotRateData.rotRateOrthogonal.value, @@ -273,29 +251,29 @@ void AcsController::performSafe() { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING): errAng = NAN; safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -308,8 +286,8 @@ void AcsController::performSafe() { // detumble check and switch if (acsParameters.safeModeControllerParameters.useMekf) { - if (mekfData.satRotRateMekf.isValid() and - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + if (attitudeEstimationData.satRotRateMekf.isValid() and + VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } @@ -340,24 +318,24 @@ void AcsController::performSafe() { } void AcsController::performDetumble() { - acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( + acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); double magMomMtq[3] = {0, 0, 0}; switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL): + case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL): detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainFull); break; - case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): + case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainBdot); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -369,8 +347,8 @@ void AcsController::performDetumble() { acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); if (acsParameters.safeModeControllerParameters.useMekf) { - if (mekfData.satRotRateMekf.isValid() and - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + if (attitudeEstimationData.satRotRateMekf.isValid() and + VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } @@ -402,6 +380,68 @@ 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()); + uint8_t useMekf = false; + switch (mode) { + case acs::PTG_IDLE: + useMekf = acsParameters.idleModeControllerParameters.useMekf; + break; + case acs::PTG_TARGET: + useMekf = acsParameters.targetModeControllerParameters.useMekf; + break; + case acs::PTG_TARGET_GS: + useMekf = acsParameters.gsTargetModeControllerParameters.useMekf; + break; + case acs::PTG_NADIR: + useMekf = acsParameters.nadirModeControllerParameters.useMekf; + break; + case acs::PTG_INERTIAL: + useMekf = acsParameters.inertialModeControllerParameters.useMekf; + break; + } + acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy( + mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid, + attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(), + fusedRotRateData.rotRateSource.isValid(), useMekf); + + 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); + return; + } else { + ptgCtrlLostCounter = 0; + } + + double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0}; + switch (ptgCtrlStrat) { + case acs::ControlModeStrategy::PTGCTRL_MEKF: + std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI)); + std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB)); + break; + case acs::ControlModeStrategy::PTGCTRL_STR: + quatBI[0] = sensorValues.strSet.caliQx.value; + quatBI[1] = sensorValues.strSet.caliQy.value; + quatBI[2] = sensorValues.strSet.caliQz.value; + quatBI[3] = sensorValues.strSet.caliQw.value; + std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); + break; + case acs::ControlModeStrategy::PTGCTRL_QUEST: + std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI)); + std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); + break; + default: + sif::error << "AcsController: Invalid pointing mode strategy for performDetumble" + << std::endl; + break; + } + uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); @@ -428,8 +468,8 @@ 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(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, @@ -440,9 +480,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; @@ -450,8 +490,8 @@ 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(quatBI, rotRateB, targetQuat, targetSatRotRate, + acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, @@ -464,17 +504,17 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; 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(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, @@ -485,9 +525,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; @@ -495,8 +535,8 @@ 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(quatBI, rotRateB, targetQuat, targetSatRotRate, + acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, @@ -509,17 +549,17 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; 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(quatBI, rotRateB, targetQuat, targetSatRotRate, + acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, @@ -532,9 +572,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: @@ -673,7 +713,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng; std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); - ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF; + ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF; ctrlValData.setValidity(true, true); } } @@ -750,11 +790,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); @@ -771,7 +812,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}); return returnvalue::OK; } @@ -792,13 +841,15 @@ 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: return &actuatorCmdData; case acsctrl::FUSED_ROTATION_RATE_DATA: return &fusedRotRateData; + case acsctrl::FUSED_ROTATION_RATE_SOURCES_DATA: + return &fusedRotRateSourcesData; default: return nullptr; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 5b3c6340..2e1fe13e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -67,6 +68,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes AcsParameters acsParameters; SensorProcessing sensorProcessing; + AttitudeEstimation attitudeEstimation; FusedRotationEstimation fusedRotationEstimation; Navigation navigation; ActuatorCmd actuatorCmd; @@ -82,7 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint8_t detumbleCounter = 0; uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = false; - uint16_t mekfInvalidCounter = 0; + uint16_t ptgCtrlLostCounter = 0; bool safeCtrlFailureFlag = false; uint8_t safeCtrlFailureCounter = 0; uint8_t resetMekfCount = 0; @@ -239,11 +241,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry gpsVelocity = PoolEntry(3); PoolEntry gpsSource = PoolEntry(); - // MEKF - acsctrl::MekfData mekfData; + // Attitude Estimation + acsctrl::AttitudeEstimationData attitudeEstimationData; PoolEntry quatMekf = PoolEntry(4); PoolEntry satRotRateMekf = PoolEntry(3); PoolEntry mekfStatus = PoolEntry(); + PoolEntry quatQuest = PoolEntry(4); // Ctrl Values acsctrl::CtrlValData ctrlValData; @@ -264,6 +267,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry rotRateOrthogonal = PoolEntry(3); PoolEntry rotRateParallel = PoolEntry(3); PoolEntry rotRateTotal = PoolEntry(3); + PoolEntry rotRateSource = PoolEntry(); + + // Fused Rot Rate Sources + acsctrl::FusedRotRateSourcesData fusedRotRateSourcesData; + PoolEntry rotRateOrthogonalSusMgm = PoolEntry(3); + PoolEntry rotRateParallelSusMgm = PoolEntry(3); + PoolEntry rotRateTotalSusMgm = PoolEntry(3); + PoolEntry rotRateTotalQuest = PoolEntry(3); + PoolEntry rotRateTotalStr = PoolEntry(3); // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index 719ef507..8fb774dc 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -216,6 +216,7 @@ void PowerController::calculateStateOfCharge() { pwrCtrlCoreHk.coulombCounterCharge.setValid(false); } } + return; } // commit to dataset diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 7b867a46..a6ce6c5d 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -24,11 +24,20 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(onBoardParams.sampleTime); break; case 0x1: - parameterWrapper->set(onBoardParams.mekfViolationTimer); + parameterWrapper->set(onBoardParams.ptgCtrlLostTimer); break; case 0x2: parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse); break; + case 0x3: + parameterWrapper->set(onBoardParams.fusedRateFromStr); + break; + case 0x4: + parameterWrapper->set(onBoardParams.fusedRateFromQuest); + break; + case 0x5: + parameterWrapper->set(onBoardParams.questFilterWeight); + break; default: return INVALID_IDENTIFIER_ID; } @@ -425,6 +434,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x9: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; + case 0xA: + parameterWrapper->set(idleModeControllerParameters.useMekf); + break; default: return INVALID_IDENTIFIER_ID; } @@ -462,36 +474,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->set(targetModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xE: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; case 0xF: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; case 0x10: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; case 0x11: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; case 0x12: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; case 0x13: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; case 0x14: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x15: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -531,18 +546,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); + parameterWrapper->set(gsTargetModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xD: - parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xE: + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + break; + case 0xF: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -582,15 +600,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + parameterWrapper->set(nadirModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(nadirModeControllerParameters.quatRef); + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xD: + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; + case 0xE: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: @@ -630,12 +651,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + parameterWrapper->set(inertialModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; case 0xC: + parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); + break; + case 0xD: parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 130d9a91..80269d55 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -18,8 +18,11 @@ class AcsParameters : public HasParametersIF { struct OnBoardParams { double sampleTime = 0.4; // [s] - uint16_t mekfViolationTimer = 750; + uint16_t ptgCtrlLostTimer = 750; uint8_t fusedRateSafeDuringEclipse = true; + uint8_t fusedRateFromStr = false; + uint8_t fusedRateFromQuest = false; + double questFilterWeight = 0.0; } onBoardParams; struct InertiaEIVE { @@ -861,6 +864,7 @@ class AcsParameters : public HasParametersIF { double deSatGainFactor = 1000; uint8_t desatOn = true; uint8_t enableAntiStiction = true; + uint8_t useMekf = false; } pointingLawParameters; struct IdleModeControllerParameters : PointingLawParameters { diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp new file mode 100644 index 00000000..e287b22f --- /dev/null +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -0,0 +1,111 @@ +#include "AttitudeEstimation.h" + +AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) { + acsParameters = acsParameters_; +} + +AttitudeEstimation::~AttitudeEstimation() {} + +void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, + acsctrl::MgmDataProcessed *mgmData, + acsctrl::AttitudeEstimationData *attitudeEstimation) { + if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and + mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid())) { + { + PoolReadGuard pg{attitudeEstimation}; + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double)); + attitudeEstimation->quatQuest.setValid(false); + } + } + return; + } + + // Normalize Data + double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0}, + normSusI[3] = {0, 0, 0}; + VectorOperations::normalize(susData->susVecTot.value, normMgmB, 3); + VectorOperations::normalize(susData->sunIjkModel.value, normMgmI, 3); + VectorOperations::normalize(mgmData->mgmVecTot.value, normSusB, 3); + VectorOperations::normalize(mgmData->magIgrfModel.value, normSusI, 3); + + // Create Helper Vectors + double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0}, + helperSum[3] = {0, 0, 0}; + VectorOperations::cross(normSusB, normMgmB, normHelperB); + VectorOperations::cross(normSusI, normMgmI, normHelperI); + VectorOperations::normalize(normHelperB, normHelperB, 3); + VectorOperations::normalize(normHelperI, normHelperI, 3); + VectorOperations::cross(normHelperB, normHelperI, helperCross); + VectorOperations::add(normHelperB, normHelperI, helperSum, 3); + + // Sensor Weights + double kSus = 0, kMgm = 0; + kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2); + kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2); + + // Weighted Vectors + double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0}, + kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0}; + VectorOperations::mulScalar(normSusB, kSus, weightedSusB, 3); + VectorOperations::mulScalar(normMgmB, kMgm, weightedMgmB, 3); + VectorOperations::cross(weightedSusB, normSusI, kSusVec); + VectorOperations::cross(weightedMgmB, normMgmI, kMgmVec); + VectorOperations::add(kSusVec, kMgmVec, kSumVec, 3); + + // Some weird Angles + double alpha = (1 + VectorOperations::dot(normHelperB, normHelperI)) * + (VectorOperations::dot(weightedSusB, normSusI) + + VectorOperations::dot(weightedMgmB, normMgmI)) + + VectorOperations::dot(helperCross, kSumVec); + double beta = VectorOperations::dot(helperSum, kSumVec); + double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2)); + + // I don't even know what this is supposed to be + double constPlus = + 1. / (2 * std::sqrt(gamma * (gamma + alpha) * + (1 + VectorOperations::dot(normHelperB, normHelperI)))); + double constMinus = + 1. / (2 * std::sqrt(gamma * (gamma - alpha) * + (1 + VectorOperations::dot(normHelperB, normHelperI)))); + + // Calculate Quaternion + double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0}, + qRotVecPt1[3] = {0, 0, 0}; + if (alpha >= 0) { + // Scalar Part + qBI[3] = (gamma + alpha) * (1 + VectorOperations::dot(normHelperB, normHelperI)); + // Rotational Vector Part + VectorOperations::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3); + VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); + VectorOperations::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3); + VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); + std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); + + VectorOperations::mulScalar(qBI, constPlus, qBI, 3); + QuaternionOperations::normalize(qBI, qBI); + } else { + // Scalar Part + qBI[3] = (beta) * (1 + VectorOperations::dot(normHelperB, normHelperI)); + // Rotational Vector Part + VectorOperations::mulScalar(helperCross, beta, qRotVecPt0, 3); + VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); + VectorOperations::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3); + VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); + std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); + + VectorOperations::mulScalar(qBI, constMinus, qBI, 3); + QuaternionOperations::normalize(qBI, qBI); + } + // Low Pass + if (VectorOperations::norm(qOld, 4) != 0.0) { + QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI); + } + { + PoolReadGuard pg{attitudeEstimation}; + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double)); + attitudeEstimation->quatQuest.setValid(true); + } + } +} diff --git a/mission/controller/acs/AttitudeEstimation.h b/mission/controller/acs/AttitudeEstimation.h new file mode 100644 index 00000000..8f307cc4 --- /dev/null +++ b/mission/controller/acs/AttitudeEstimation.h @@ -0,0 +1,31 @@ +#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ +#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ + +#include +#include +#include +#include +#include + +#include +#include + +class AttitudeEstimation { + public: + AttitudeEstimation(AcsParameters *acsParameters_); + virtual ~AttitudeEstimation(); + ; + + void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData, + acsctrl::AttitudeEstimationData *attitudeEstimation); + + protected: + private: + AcsParameters *acsParameters; + + double qOld[4] = {0, 0, 0, 0}; + + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; +}; + +#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */ diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index a8b0e9a6..4af05d8b 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -2,6 +2,7 @@ target_sources( ${LIB_EIVE_MISSION} PRIVATE AcsParameters.cpp ActuatorCmd.cpp + AttitudeEstimation.cpp FusedRotationEstimation.cpp Guidance.cpp Igrf13Model.cpp diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 4f1dad45..b4d0f0c4 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -4,19 +4,220 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) acsParameters = acsParameters_; } -void FusedRotationEstimation::estimateFusedRotationRateSafe( +void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, + acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, + acsctrl::FusedRotRateData *fusedRotRateData) { + estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData); + estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData); + estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed, + fusedRotRateSourcesData); + + if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and + acsParameters->onBoardParams.fusedRateFromStr) { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateTotal.value, + fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); + fusedRotRateData->rotRateTotal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; + fusedRotRateData->rotRateSource.setValid(true); + } + } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and + acsParameters->onBoardParams.fusedRateFromQuest) { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateTotal.value, + fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); + fusedRotRateData->rotRateTotal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST; + fusedRotRateData->rotRateSource.setValid(true); + } + } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid( + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid()); + std::memcpy(fusedRotRateData->rotRateParallel.value, + fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid( + fusedRotRateSourcesData->rotRateParallelSusMgm.isValid()); + std::memcpy(fusedRotRateData->rotRateTotal.value, + fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateTotal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM; + fusedRotRateData->rotRateSource.setValid(true); + } else { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->setValidity(false, true); + fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE; + fusedRotRateData->rotRateSource.setValid(true); + } + } +} + +void FusedRotationEstimation::estimateFusedRotationRateStr( + ACS::SensorValues *sensorValues, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { + if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and + sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(false); + } + } + std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); + return; + } + + double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, + sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; + if (VectorOperations::norm(quatOldStr, 4) != 0 and timeDelta != 0) { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOldStr, quatOldInv); + QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta); + if (VectorOperations::norm(quatDelta, 4) != 0.0) { + QuaternionOperations::normalize(quatDelta); + } + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (VectorOperations::norm(quatDelta, 3) == 0.0) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } + } + std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); + return; + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } + } + std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(false); + } + } + std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); + return; +} + +void FusedRotationEstimation::estimateFusedRotationRateQuest( + acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { + if (not attitudeEstimationData->quatQuest.isValid()) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); + } + } + std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); + } + + if (VectorOperations::norm(quatOldQuest, 4) != 0 and timeDelta != 0) { + double quatOldInv[4] = {0, 0, 0, 0}; + double quatDelta[4] = {0, 0, 0, 0}; + + QuaternionOperations::inverse(quatOldQuest, quatOldInv); + QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta); + if (VectorOperations::norm(quatDelta, 4) != 0.0) { + QuaternionOperations::normalize(quatDelta); + } + + double rotVec[3] = {0, 0, 0}; + double angle = QuaternionOperations::getAngle(quatDelta); + if (VectorOperations::norm(quatDelta, 3) == 0.0) { + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } + } + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + return; + } + VectorOperations::normalize(quatDelta, rotVec, 3); + VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } + } + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + return; + } + { + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); + } + } + std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); + return; +} + +void FusedRotationEstimation::estimateFusedRotationRateSusMgm( + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and - not fusedRotRateData->rotRateTotal.isValid()) or + not fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) or (not susDataProcessed->susVecTotDerivative.isValid() and not mgmDataProcessed->mgmVecTotDerivative.isValid())) { { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); - fusedRotRateData->setValidity(false, true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false); + } } // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { @@ -25,7 +226,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( return; } if (not susDataProcessed->susVecTot.isValid()) { - estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData); // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); @@ -49,7 +250,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( VectorOperations::mulScalar(susDataProcessed->susVecTot.value, omegaParallel, fusedRotRateParallel, 3); } else { - estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData); // store for calculation of angular acceleration if (gyrDataProcessed->gyrVecTot.isValid()) { std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); @@ -71,12 +272,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( VectorOperations::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal); { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal, - 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); - fusedRotRateData->setValidity(true, true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, fusedRotRateOrthogonal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(true); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, fusedRotRateParallel, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(true); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true); + } } // store for calculation of angular acceleration @@ -86,31 +293,44 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( } void FusedRotationEstimation::estimateFusedRotationRateEclipse( - acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or not gyrDataProcessed->gyrVecTot.isValid() or - VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { + VectorOperations::norm(fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3) == 0) { { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); - fusedRotRateData->setValidity(false, true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false); + } } return; } double angAccelB[3] = {0, 0, 0}; VectorOperations::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3); double fusedRotRateTotal[3] = {0, 0, 0}; - VectorOperations::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal, - 3); + VectorOperations::add(fusedRotRateSourcesData->rotRateTotalSusMgm.value, angAccelB, + fusedRotRateTotal, 3); { - PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); - fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false); + std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true); + } } } diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index fa4fda1c..2fc2ab8f 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -2,28 +2,46 @@ #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ #include +#include #include #include +#include #include class FusedRotationEstimation { public: FusedRotationEstimation(AcsParameters *acsParameters_); - void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::GyrDataProcessed *gyrDataProcessed, - acsctrl::FusedRotRateData *fusedRotRateData); + void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + ACS::SensorValues *sensorValues, + acsctrl::AttitudeEstimationData *attitudeEstimationData, + const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, + acsctrl::FusedRotRateData *fusedRotRateData); protected: private: - static constexpr double ZERO_VEC[3] = {0, 0, 0}; + static constexpr double ZERO_VEC3[3] = {0, 0, 0}; + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; AcsParameters *acsParameters; + double quatOldQuest[4] = {0, 0, 0, 0}; + double quatOldStr[4] = {0, 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, - acsctrl::FusedRotRateData *fusedRotRateData); + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); + 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_ */ diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index bf834d12..0be636ad 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -495,24 +495,24 @@ void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double time ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { - bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); - bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()); - bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()); - bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid()); + bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); + bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); + bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); + bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); - if (rw1valid && rw2valid && rw3valid && rw4valid) { + if (rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; - } else if (!rw1valid && rw2valid && rw3valid && rw4valid) { + } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && !rw2valid && rw3valid && rw4valid) { + } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && rw2valid && !rw3valid && rw4valid) { + } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && rw2valid && rw3valid && !rw4valid) { + } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 0f297b21..52acc7ee 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -19,7 +19,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} ReturnValue_t MultiplicativeKalmanFilter::init( const double *magneticField_, const bool validMagField_, const double *sunDir_, 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"? // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { @@ -191,7 +191,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, const bool validGYRs_, const double *magneticField_, const bool validMagField_, 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) { // Check for GYR Measurements int MDF = 0; // Matrix Dimension Factor @@ -1090,7 +1090,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( 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 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}}; @@ -1100,7 +1100,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { return MEKF_UNINITIALIZED; } -void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData, +void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus) { { 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]) { { PoolReadGuard pg(mekfData); diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 5a878c4b..bd722115 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -21,7 +21,7 @@ class MultiplicativeKalmanFilter { 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 * quaternion through the QUEST algorithm @@ -32,7 +32,7 @@ class MultiplicativeKalmanFilter { */ ReturnValue_t init(const double *magneticField_, const bool validMagField_, 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); /* @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 validMagField_, const double *sunDir_, 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); enum MekfStatus : uint8_t { @@ -99,8 +99,8 @@ class MultiplicativeKalmanFilter { double biasGYR[3]; /*Between measured and estimated sat Rate*/ /*Parameter INIT*/ /*Functions*/ - void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); - void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], + void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus); + void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, double quat[4], double satRotRate[3]); }; diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 4726586b..624f4cea 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -16,7 +16,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::MekfData *mekfData, AcsParameters *acsParameters) { + acsctrl::AttitudeEstimationData *mekfData, + AcsParameters *acsParameters) { double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; @@ -41,7 +42,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, } } -void Navigation::resetMekf(acsctrl::MekfData *mekfData) { +void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) { mekfStatus = multiplicativeKalmanFilter.reset(mekfData); } @@ -54,7 +55,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat { PoolReadGuard pg(gpsDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - gpsDataProcessed->source = acs::GpsSource::SPG4; + gpsDataProcessed->source = acs::gps::Source::SPG4; gpsDataProcessed->source.setValid(true); std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); gpsDataProcessed->gpsPosition.setValid(true); @@ -66,7 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat { PoolReadGuard pg(gpsDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - gpsDataProcessed->source = acs::GpsSource::NONE; + gpsDataProcessed->source = acs::gps::Source::NONE; gpsDataProcessed->source.setValid(true); std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); gpsDataProcessed->gpsPosition.setValid(false); diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 2785ff2e..4f58b197 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -17,9 +17,9 @@ class Navigation { ReturnValue_t useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters); - void resetMekf(acsctrl::MekfData *mekfData); + void resetMekf(acsctrl::AttitudeEstimationData *mekfData); ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed); ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 23388e5f..43582f18 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,7 +1,5 @@ #include "SensorProcessing.h" -#include - SensorProcessing::SensorProcessing() {} SensorProcessing::~SensorProcessing() {} @@ -17,7 +15,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; bool gpsValid = false; - if (gpsDataProcessed->source.value != acs::GpsSource::NONE) { + if (gpsDataProcessed->source.value != acs::gps::Source::NONE) { // There seems to be a bug here, which causes the model vector to drift until infinity, if the // model class is not initialized new every time. Works for now, but should be investigated. Igrf13Model igrf13; @@ -527,9 +525,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // init variables double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; - uint8_t gpsSource = acs::GpsSource::NONE; + uint8_t gpsSource = acs::gps::Source::NONE; // We do not trust the GPS and therefore it shall die here if SPG4 is running - if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) { + if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) { MathOperations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude, gdLongitude, altitude); double factor = 1 - pow(ECCENTRICITY_WGS84, 2); @@ -572,7 +570,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong validSavedPosSatE = true; - gpsSource = acs::GpsSource::GPS; + gpsSource = acs::gps::Source::GPS; } { PoolReadGuard pg(gpsDataProcessed); diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 8f422ec1..754843f8 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,18 +7,18 @@ Detumble::Detumble() {} Detumble::~Detumble() {} -acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, +acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, const bool magFieldRateValid, const bool useFullDetumbleLaw) { if (not magFieldValid) { - return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { - return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL; + return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL; } else if (magFieldRateValid) { - return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; + return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 9fca77e6..476766c3 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -11,7 +11,7 @@ class Detumble { Detumble(); virtual ~Detumble(); - acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + acs::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, const bool magFieldRateValid, const bool useFullDetumbleLaw); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 2f5847cc..eb510c53 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -10,6 +10,22 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_ PtgCtrl::~PtgCtrl() {} +acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid, + const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) { + if (not magFieldValid) { + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; + } else if (mekfEnabled and mekfValid) { + return acs::ControlModeStrategy::PTGCTRL_MEKF; + } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { + return acs::ControlModeStrategy::PTGCTRL_STR; + } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { + return acs::ControlModeStrategy::PTGCTRL_QUEST; + } else { + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; + } +} + void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 5f731e6b..a07849ec 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -2,6 +2,7 @@ #define PTGCTRL_H_ #include +#include #include #include #include @@ -9,7 +10,7 @@ class PtgCtrl { /* * @brief: This class handles the pointing control mechanism. Calculation of an commanded - * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * torque for the reaction wheels, and magnetic Field strength for magnetorquer for desaturation * * @note: A description of the used algorithms can be found in * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 @@ -21,6 +22,12 @@ class PtgCtrl { PtgCtrl(AcsParameters *acsParameters_); virtual ~PtgCtrl(); + acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool strValid, const bool questValid, + const bool fusedRateValid, + const uint8_t rotRateSource, + const uint8_t mekfEnabled); + /* @brief: Calculates the needed torque for the pointing control mechanism */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, @@ -36,7 +43,7 @@ class PtgCtrl { const int32_t speedRw3, double *mgtDpDes); /* @brief: Commands the stiction torque in case wheel speed is to low - * torqueCommand modified torque after antistiction + * torqueCommand modified torque after anti-stiction */ void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index de0cd197..b1699aef 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,40 +9,40 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, +acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { - return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::SafeModeStrategy::SAFECTRL_MEKF; + return acs::ControlModeStrategy::SAFECTRL_MEKF; } else if (sunDirValid) { if (gyrEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_GYR; + return acs::ControlModeStrategy::SAFECTRL_GYR; } else if (not gyrEnabled and fusedRateTotalValid) { - return acs::SafeModeStrategy::SAFECTRL_SUSMGM; + return acs::ControlModeStrategy::SAFECTRL_SUSMGM; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else if (not sunDirValid) { if (dampingEnabled) { if (gyrEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else if (not dampingEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index d35d5d04..1c38d81d 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -12,7 +12,7 @@ class SafeCtrl { SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + acs::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled); diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index ac348cf9..83a74552 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -20,6 +20,7 @@ enum SetIds : uint32_t { CTRL_VAL_DATA, ACTUATOR_CMD_DATA, FUSED_ROTATION_RATE_DATA, + FUSED_ROTATION_RATE_SOURCES_DATA, TLE_SET, }; @@ -96,6 +97,7 @@ enum PoolIds : lp_id_t { SAT_ROT_RATE_MEKF, QUAT_MEKF, MEKF_STATUS, + QUAT_QUEST, // Ctrl Values SAFE_STRAT, TGT_QUAT, @@ -110,6 +112,13 @@ enum PoolIds : lp_id_t { ROT_RATE_ORTHOGONAL, ROT_RATE_PARALLEL, 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, }; static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6; @@ -119,10 +128,11 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6; -static constexpr uint8_t MEKF_SET_ENTRIES = 3; +static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; 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; /** * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. @@ -246,13 +256,14 @@ class GpsDataProcessed : public StaticLocalDataSet { private: }; -class MekfData : public StaticLocalDataSet { +class AttitudeEstimationData : public StaticLocalDataSet { public: - MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {} + AttitudeEstimationData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {} lp_vec_t quatMekf = lp_vec_t(sid.objectId, QUAT_MEKF, this); lp_vec_t satRotRateMekf = lp_vec_t(sid.objectId, SAT_ROT_RATE_MEKF, this); lp_var_t mekfStatus = lp_var_t(sid.objectId, MEKF_STATUS, this); + lp_vec_t quatQuest = lp_vec_t(sid.objectId, QUAT_MEKF, this); private: }; @@ -291,6 +302,25 @@ class FusedRotRateData : public StaticLocalDataSet { lp_vec_t(sid.objectId, ROT_RATE_ORTHOGONAL, this); lp_vec_t rotRateParallel = lp_vec_t(sid.objectId, ROT_RATE_PARALLEL, this); lp_vec_t rotRateTotal = lp_vec_t(sid.objectId, ROT_RATE_TOTAL, this); + lp_var_t rotRateSource = lp_var_t(sid.objectId, ROT_RATE_SOURCE, this); + + private: +}; + +class FusedRotRateSourcesData : public StaticLocalDataSet { + public: + FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner) + : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {} + + lp_vec_t rotRateOrthogonalSusMgm = + lp_vec_t(sid.objectId, ROT_RATE_ORTHOGONAL_SUSMGM, this); + lp_vec_t rotRateParallelSusMgm = + lp_vec_t(sid.objectId, ROT_RATE_PARALLEL_SUSMGM, this); + lp_vec_t rotRateTotalSusMgm = + lp_vec_t(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this); + lp_vec_t rotRateTotalQuest = + lp_vec_t(sid.objectId, ROT_RATE_TOTAL_QUEST, this); + lp_vec_t rotRateTotalStr = lp_vec_t(sid.objectId, ROT_RATE_TOTAL_STR, this); private: }; diff --git a/mission/system/acs/StrFdir.cpp b/mission/system/acs/StrFdir.cpp index 97a4162a..83bd27fa 100644 --- a/mission/system/acs/StrFdir.cpp +++ b/mission/system/acs/StrFdir.cpp @@ -6,7 +6,7 @@ StrFdir::StrFdir(object_id_t strObject) : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {} ReturnValue_t StrFdir::eventReceived(EventMessage* event) { - if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { + if (event->getEvent() == acs::PTG_CTRL_NO_ATTITUDE_INFORMATION) { setFaulty(event->getEvent()); return returnvalue::OK; }