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

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

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

5
mission/acs/defs.h
View File

@ -41,8 +41,9 @@ enum ControlModeStrategy : uint8_t {
SAFECTRL_DETUMBLE_FULL = 20,
SAFECTRL_DETUMBLE_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 {

112
mission/controller/AcsController.cpp
View File

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

16
mission/controller/AcsController.h
View File

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

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

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

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

@ -20,6 +20,8 @@ class AcsParameters : public HasParametersIF {
double sampleTime = 0.4; // [s]
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 {

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

@ -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<double>::norm(quatOld, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateStr(quatNew, timeDelta, fusedRotRateData);
if (VectorOperations<double>::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<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateQuat(attitudeEstimationData->quatQuest.value, timeDelta, true,
fusedRotRateSourcesData);
} else {
{
PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
} else {
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
{
PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
// Fused Rotation Estimation of SUS&MGM
}
void FusedRotationEstimation::estimateFusedRotationRateStr(
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<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::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);
}
}
}

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

@ -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_ */

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

@ -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);

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

@ -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]);
};

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

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

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

@ -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);

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

@ -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;
}

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

@ -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
*/

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

@ -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<FUSED_ROT_RATE_SET_ENTRIES> {
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
private:
};
class FusedRotRateSourcesData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
public:
FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {}
lp_vec_t<double, 3> rotRateOrthogonalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateParallelSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalSusMgm =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this);
lp_vec_t<double, 3> rotRateTotalQuest =
lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_QUEST, this);
lp_vec_t<double, 3> rotRateTotalStr = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_STR, this);
private:
};