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

this should work
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-11-24 11:30:27 +01:00
parent 647d5fda7c
commit c7ec9726c4
7 changed files with 94 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
mission/acs/defs.h
View File

@ -53,6 +53,13 @@ enum GpsSource : uint8_t {
SPG4, SPG4,
}; };
enum RotRateSource : uint8_t {
ALL_INVALID,
SUSMGM,
QUEST,
STR,
};
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);

49
mission/controller/AcsController.cpp
View File

@ -169,7 +169,7 @@ void AcsController::performAttitudeControl() {
attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
fusedRotationEstimation.estimateFusedRotationRate( fusedRotationEstimation.estimateFusedRotationRate(
&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateData); &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &attitudeEstimationData, &acsParameters); &susDataProcessed, &attitudeEstimationData, &acsParameters);
@ -377,22 +377,39 @@ void AcsController::performDetumble() {
void AcsController::performPointingCtrl() { void AcsController::performPointingCtrl() {
bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid()); sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
acs::ControlModeStrategy ptgCtrlStrat = uint8_t useMekf = false;
ptgCtrl.pointingCtrlStrategy(mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, switch (mode) {
strValid, fusedRotRateData.rotRateTotal.isValid(), mekfEnabled, case acs::PTG_IDLE:
acsParameters.strParameters.useStrForRotRate); 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) { if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
// ptgCtrlLostCounter++; ptgCtrlLostCounter++;
// if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
// triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
// ptgCtrlLostCounter = 0; ptgCtrlLostCounter = 0;
// } }
// commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
// cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime);
// acsParameters.rwHandlingParameters.rampTime); }
// }
uint8_t enableAntiStiction = true; uint8_t enableAntiStiction = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

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

@ -431,6 +431,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x9: case 0x9:
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break; break;
case 0xA:
parameterWrapper->set(idleModeControllerParameters.useMekf);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -468,36 +471,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(targetModeControllerParameters.refDirection); parameterWrapper->set(targetModeControllerParameters.useMekf);
break; break;
case 0xB: case 0xB:
parameterWrapper->setVector(targetModeControllerParameters.refRotRate); parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break; break;
case 0xC: case 0xC:
parameterWrapper->setVector(targetModeControllerParameters.quatRef); parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break; break;
case 0xE: case 0xE:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt); parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
break; break;
case 0xF: case 0xF:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt); parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break; break;
case 0x10: case 0x10:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt); parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break; break;
case 0x11: case 0x11:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
break; break;
case 0x12: case 0x12:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break; break;
case 0x13: case 0x13:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break; break;
case 0x14: case 0x14:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x15:
parameterWrapper->set(targetModeControllerParameters.blindRotRate); parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break; break;
default: default:
@ -537,18 +543,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
break; break;
case 0xB: case 0xB:
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break; break;
case 0xC: case 0xC:
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break; break;
case 0xD: case 0xD:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break; break;
case 0xE: case 0xE:
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break; break;
default: default:
@ -588,15 +597,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(nadirModeControllerParameters.refDirection); parameterWrapper->set(nadirModeControllerParameters.useMekf);
break; break;
case 0xB: case 0xB:
parameterWrapper->setVector(nadirModeControllerParameters.quatRef); parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break; break;
case 0xC: case 0xC:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break; break;
case 0xD: case 0xD:
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
break;
case 0xE:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
break; break;
default: default:
@ -636,12 +648,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break; break;
case 0xA: case 0xA:
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); parameterWrapper->set(inertialModeControllerParameters.useMekf);
break; break;
case 0xB: case 0xB:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break; break;
case 0xC: case 0xC:
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break;
case 0xD:
parameterWrapper->setVector(inertialModeControllerParameters.quatRef); parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
break; break;
default: default:

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

@ -863,6 +863,7 @@ class AcsParameters : public HasParametersIF {
double deSatGainFactor = 1000; double deSatGainFactor = 1000;
uint8_t desatOn = true; uint8_t desatOn = true;
uint8_t enableAntiStiction = true; uint8_t enableAntiStiction = true;
uint8_t useMekf = false;
} pointingLawParameters; } pointingLawParameters;
struct IdleModeControllerParameters : PointingLawParameters { struct IdleModeControllerParameters : PointingLawParameters {

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

@ -26,6 +26,8 @@ void FusedRotationEstimation::estimateFusedRotationRate(
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true); fusedRotRateData->rotRateOrthogonal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::RotRateSource::STR;
fusedRotRateData->rotRateSource.setValid(true);
} }
} else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
acsParameters->onBoardParams.fusedRateFromQuest) { acsParameters->onBoardParams.fusedRateFromQuest) {
@ -38,6 +40,8 @@ void FusedRotationEstimation::estimateFusedRotationRate(
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true); fusedRotRateData->rotRateOrthogonal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::RotRateSource::QUEST;
fusedRotRateData->rotRateSource.setValid(true);
} }
} else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
@ -51,12 +55,15 @@ void FusedRotationEstimation::estimateFusedRotationRate(
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true); fusedRotRateData->rotRateOrthogonal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::RotRateSource::SUSMGM;
fusedRotRateData->rotRateSource.setValid(true);
} else { } else {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateSource.value = acs::RotRateSource::ALL_INVALID;
fusedRotRateData->setValidity(false, true); fusedRotRateData->setValidity(false, true);
} }
} }

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

@ -10,17 +10,17 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(const bool magFieldValid, acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
const bool mekfValid, const bool strValid, const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid,
const bool fusedRateValid, const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) {
const uint8_t mekfEnabled,
const uint8_t strRateEnabled) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) { } else if (mekfEnabled and mekfValid) {
return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF; return acs::ControlModeStrategy::PTGCTRL_MEKF;
} else if (strValid and strRateEnabled and fusedRateValid) { } else if (strValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_WITHOUT_MEKF; return acs::ControlModeStrategy::PTGCTRL_STR;
} else if (questValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_QUEST;
} else { } else {
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }

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

@ -23,9 +23,10 @@ class PtgCtrl {
virtual ~PtgCtrl(); virtual ~PtgCtrl();
acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool strValid, const bool fusedRateValid, const bool strValid, const bool questValid,
const uint8_t mekfEnabled, const bool fusedRateValid,
const uint8_t strRateEnabled); const uint8_t rotRateSource,
const uint8_t mekfEnabled);
/* @brief: Calculates the needed torque for the pointing control mechanism /* @brief: Calculates the needed torque for the pointing control mechanism
*/ */