From 4a67f9ffe5a094e4e75d4ca70765b149c37e3f3a Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 23 Nov 2023 16:56:36 +0100 Subject: [PATCH] this is a mess --- mission/acs/defs.h | 5 +- mission/controller/AcsController.cpp | 112 +++++++++++------- mission/controller/AcsController.h | 16 ++- mission/controller/acs/AcsParameters.cpp | 6 + mission/controller/acs/AcsParameters.h | 3 +- .../acs/FusedRotationEstimation.cpp | 100 +++++++++++----- .../controller/acs/FusedRotationEstimation.h | 25 ++-- .../acs/MultiplicativeKalmanFilter.cpp | 10 +- .../acs/MultiplicativeKalmanFilter.h | 10 +- mission/controller/acs/Navigation.cpp | 4 +- mission/controller/acs/Navigation.h | 4 +- mission/controller/acs/control/PtgCtrl.cpp | 33 ++---- mission/controller/acs/control/PtgCtrl.h | 8 +- .../AcsCtrlDefinitions.h | 30 ++++- 14 files changed, 234 insertions(+), 132 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 2f82ca9f..d9ce3c6c 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -41,8 +41,9 @@ enum ControlModeStrategy : uint8_t { SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_DETERIORATED = 21, // Added in vNext - PTGCTRL_ACTIVE_MEKF = 100, - PTGCTRL_WITHOUT_MEKF = 101, + PTGCTRL_MEKF = 100, + PTGCTRL_STR = 101, + PTGCTRL_QUEST = 102, }; enum GpsSource : uint8_t { diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 47c5d77e..3990c3f2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -7,6 +7,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), + attitudeEstimation(&acsParameters), fusedRotationEstimation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), @@ -19,10 +20,11 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) gyrDataRaw(this), gyrDataProcessed(this), gpsDataProcessed(this), - mekfData(this), + attitudeEstimationData(this), ctrlValData(this), actuatorCmdData(this), fusedRotRateData(this), + fusedRotRateSourcesData(this), tleData(this) {} ReturnValue_t AcsController::initialize() { @@ -56,7 +58,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t return HasActionsIF::EXECUTION_FINISHED; } case RESET_MEKF: { - navigation.resetMekf(&mekfData); + navigation.resetMekf(&attitudeEstimationData); return HasActionsIF::EXECUTION_FINISHED; } case RESTORE_MEKF_NONFINITE_RECOVERY: { @@ -164,20 +166,23 @@ void AcsController::performAttitudeControl() { sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, - &gyrDataProcessed, &fusedRotRateData); + attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); + fusedRotationEstimation.estimateFusedRotationRate( + &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, + &attitudeEstimationData, timeDelta, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData, &acsParameters); + &susDataProcessed, &attitudeEstimationData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO, static_cast(mekfData.mekfStatus.value)); + 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(&mekfData); + navigation.resetMekf(&attitudeEstimationData); mekfLost = true; } } else if (mekfInvalidFlag) { @@ -220,9 +225,10 @@ void AcsController::performSafe() { acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { case (acs::ControlModeStrategy::SAFECTRL_MEKF): - safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, - susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, - magMomMtq, errAng); + safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, + attitudeEstimationData.satRotRateMekf.value, + susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value, + sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; @@ -275,8 +281,8 @@ void AcsController::performSafe() { // detumble check and switch if (acsParameters.safeModeControllerParameters.useMekf) { - if (mekfData.satRotRateMekf.isValid() and - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + if (attitudeEstimationData.satRotRateMekf.isValid() and + VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } @@ -336,8 +342,8 @@ void AcsController::performDetumble() { acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); if (acsParameters.safeModeControllerParameters.useMekf) { - if (mekfData.satRotRateMekf.isValid() and - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + if (attitudeEstimationData.satRotRateMekf.isValid() and + VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } @@ -369,20 +375,24 @@ void AcsController::performDetumble() { } void AcsController::performPointingCtrl() { + bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and + sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid()); acs::ControlModeStrategy ptgCtrlStrat = - ptgCtrl.pointingCtrlStrategy(magFieldValid, mekfValid, satRotRateValid, sunDirValid, - fusedRateTotalValid, mekfEnabled, gyrEnabled, dampingEnabled); + ptgCtrl.pointingCtrlStrategy(mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, + strValid, fusedRotRateData.rotRateTotal.isValid(), mekfEnabled, + acsParameters.strParameters.useStrForRotRate); - if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { - ptgCtrlLostCounter++; - if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { - triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); - ptgCtrlLostCounter = 0; - } - commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], - cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - acsParameters.rwHandlingParameters.rampTime); - } + // if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { + // ptgCtrlLostCounter++; + // if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { + // triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); + // ptgCtrlLostCounter = 0; + // } + // commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, + // cmdSpeedRws[0], + // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + // acsParameters.rwHandlingParameters.rampTime); + // } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -410,8 +420,9 @@ void AcsController::performPointingCtrl() { case acs::PTG_IDLE: guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, @@ -422,7 +433,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; @@ -432,8 +443,9 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, @@ -446,7 +458,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; @@ -455,8 +467,9 @@ void AcsController::performPointingCtrl() { case acs::PTG_TARGET_GS: guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, @@ -467,7 +480,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; @@ -477,8 +490,9 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgNadirThreeAxes( timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, @@ -491,7 +505,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; @@ -500,8 +514,9 @@ void AcsController::performPointingCtrl() { case acs::PTG_INERTIAL: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, sizeof(targetQuat)); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, + guidance.comparePtg(attitudeEstimationData.quatMekf.value, + attitudeEstimationData.satRotRateMekf.value, targetQuat, targetSatRotRate, + acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, @@ -514,7 +529,7 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, + mgmDataProcessed.mgmVecTot.isValid(), attitudeEstimationData.satRotRateMekf.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; @@ -732,11 +747,12 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0}); - // MEKF + // Attitude Estimation localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus); - poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0}); + localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest); + poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0}); // Ctrl Values localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat); localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); @@ -753,7 +769,15 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); + // Fused Rot Rate Sources + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr); + poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0}); // TLE Data localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1); localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2); @@ -777,7 +801,7 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { case acsctrl::GPS_PROCESSED_DATA: return &gpsDataProcessed; case acsctrl::MEKF_DATA: - return &mekfData; + return &attitudeEstimationData; case acsctrl::CTRL_VAL_DATA: return &ctrlValData; case acsctrl::ACTUATOR_CMD_DATA: diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index f67595ee..6d30cba6 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes AcsParameters acsParameters; SensorProcessing sensorProcessing; + AttitudeEstimation attitudeEstimation; FusedRotationEstimation fusedRotationEstimation; Navigation navigation; ActuatorCmd actuatorCmd; @@ -218,11 +220,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; @@ -243,6 +246,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); // TLE Dataset acsctrl::TleData tleData; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 551f7cd3..ab5e301e 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -29,6 +29,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x2: parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse); break; + case 0x3: + parameterWrapper->set(onBoardParams.fusedRateFromStr); + break; + case 0x4: + parameterWrapper->set(onBoardParams.fusedRateFromQuest); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index cbebb52a..7c1b2de6 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -20,6 +20,8 @@ class AcsParameters : public HasParametersIF { double sampleTime = 0.4; // [s] uint16_t ptgCtrlLostTimer = 750; uint8_t fusedRateSafeDuringEclipse = true; + uint8_t fusedRateFromStr = false; + uint8_t fusedRateFromQuest = false; } onBoardParams; struct InertiaEIVE { @@ -910,7 +912,6 @@ class AcsParameters : public HasParametersIF { struct StrParameters { double exclusionAngle = 20 * M_PI / 180; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame - uint8_t useStrForRotRate = true; } strParameters; struct GpsParameters { diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index cf91b951..ee245519 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -7,32 +7,73 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) void FusedRotationEstimation::estimateFusedRotationRate( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, - const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { - if (acsParameters->strParameters.useStrForRotRate and - (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and + acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, + 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())) { double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - if (VectorOperations::norm(quatOld, 4) != 0 and timeDelta != 0) { - estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData); + if (VectorOperations::norm(quatOldStr, 4) != 0 and timeDelta != 0) { + estimateFusedRotationRateQuat(quatNew, timeDelta, true, fusedRotRateSourcesData); } 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 { - std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld)); - estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, - fusedRotRateData); + std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); + { + 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::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( - double *quatNew, const double timeDelta, acsctrl::FusedRotRateData *fusedRotRateData) { +void FusedRotationEstimation::estimateFusedRotationRateQuat( + double *quatNew, const double timeDelta, const bool str, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { double quatOldInv[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::normalize(quatDelta); @@ -40,25 +81,28 @@ void FusedRotationEstimation::estimateFusedRotationRateStr( double angle = QuaternionOperations::getAngle(quatDelta); if (angle == 0.0) { { - PoolReadGuard pg(fusedRotRateData); - 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, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (str) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } else { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, + 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } } } VectorOperations::normalize(quatDelta, rotVec, 3); VectorOperations::mulScalar(rotVec, angle / timeDelta, rotVec, 3); { - PoolReadGuard pg(fusedRotRateData); - 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, rotVec, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + PoolReadGuard pg(fusedRotRateSourcesData); + if (str) { + std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalStr.setValid(true); + } else { + std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double)); + fusedRotRateSourcesData->rotRateTotalQuest.setValid(true); + } } } diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index 490fe126..2c4c1588 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -15,28 +15,33 @@ class FusedRotationEstimation { void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, - ACS::SensorValues *sensorValues, const double timeDelta, + ACS::SensorValues *sensorValues, + acsctrl::AttitudeEstimationData *attitudeEstimationData, + const double timeDelta, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, 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: private: static constexpr double ZERO_VEC3[3] = {0, 0, 0}; static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; 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}; + void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, 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_ */ 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..8c3b1dd9 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -16,7 +16,7 @@ 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 +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); } 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/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 2818d4e7..cf722f35 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -10,36 +10,17 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_ PtgCtrl::~PtgCtrl() {} -acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( - 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) { +acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(const bool magFieldValid, + const bool mekfValid, const bool strValid, + const bool fusedRateValid, + const uint8_t mekfEnabled, + const uint8_t strRateEnabled) { if (not magFieldValid) { return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF; - } else if (sunDirValid) { - if (gyrEnabled and satRotRateValid) { - 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 if (strValid and strRateEnabled and fusedRateValid) { + return acs::ControlModeStrategy::PTGCTRL_WITHOUT_MEKF; } else { return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 2872cd5e..b484f3c9 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -22,10 +22,10 @@ class PtgCtrl { PtgCtrl(AcsParameters *acsParameters_); virtual ~PtgCtrl(); - acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( - 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); + acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool strValid, const bool fusedRateValid, + const uint8_t mekfEnabled, + const uint8_t strRateEnabled); /* @brief: Calculates the needed torque for the pointing control mechanism */ diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 26edf269..83b4bb87 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, }; @@ -111,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, // TLE TLE_LINE_1, 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 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; static constexpr uint8_t TLE_SET_ENTRIES = 2; /** @@ -297,6 +306,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: };