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,
};
enum RotRateSource : uint8_t {
ALL_INVALID,
SUSMGM,
QUEST,
STR,
};
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
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);
fusedRotationEstimation.estimateFusedRotationRate(
&susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
&attitudeEstimationData, timeDelta, &fusedRotRateData);
&attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &attitudeEstimationData, &acsParameters);
@ -377,22 +377,39 @@ 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(mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
strValid, fusedRotRateData.rotRateTotal.isValid(), mekfEnabled,
acsParameters.strParameters.useStrForRotRate);
uint8_t useMekf = false;
switch (mode) {
case acs::PTG_IDLE:
useMekf = acsParameters.idleModeControllerParameters.useMekf;
break;
case acs::PTG_TARGET:
useMekf = acsParameters.targetModeControllerParameters.useMekf;
break;
case acs::PTG_TARGET_GS:
useMekf = acsParameters.gsTargetModeControllerParameters.useMekf;
break;
case acs::PTG_NADIR:
useMekf = acsParameters.nadirModeControllerParameters.useMekf;
break;
case acs::PTG_INERTIAL:
useMekf = acsParameters.inertialModeControllerParameters.useMekf;
break;
}
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
fusedRotRateData.rotRateSource.isValid(), useMekf);
// if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
// ptgCtrlLostCounter++;
// if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
// triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
// ptgCtrlLostCounter = 0;
// }
// commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration,
// cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
// acsParameters.rwHandlingParameters.rampTime);
// }
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}};

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

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

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

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

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

@ -26,6 +26,8 @@ void FusedRotationEstimation::estimateFusedRotationRate(
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::RotRateSource::STR;
fusedRotRateData->rotRateSource.setValid(true);
}
} else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
acsParameters->onBoardParams.fusedRateFromQuest) {
@ -38,6 +40,8 @@ void FusedRotationEstimation::estimateFusedRotationRate(
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::RotRateSource::QUEST;
fusedRotRateData->rotRateSource.setValid(true);
}
} else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
@ -51,12 +55,15 @@ void FusedRotationEstimation::estimateFusedRotationRate(
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true);
fusedRotRateData->rotRateSource.value = acs::RotRateSource::SUSMGM;
fusedRotRateData->rotRateSource.setValid(true);
} else {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateSource.value = acs::RotRateSource::ALL_INVALID;
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() {}
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(const bool magFieldValid,
const bool mekfValid, const bool strValid,
const bool fusedRateValid,
const uint8_t mekfEnabled,
const uint8_t strRateEnabled) {
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid,
const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) {
if (not magFieldValid) {
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF;
} else if (strValid and strRateEnabled and fusedRateValid) {
return acs::ControlModeStrategy::PTGCTRL_WITHOUT_MEKF;
return acs::ControlModeStrategy::PTGCTRL_MEKF;
} else if (strValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_STR;
} else if (questValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) {
return acs::ControlModeStrategy::PTGCTRL_QUEST;
} else {
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();
acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool strValid, const bool fusedRateValid,
const uint8_t mekfEnabled,
const uint8_t strRateEnabled);
const bool strValid, const bool questValid,
const bool fusedRateValid,
const uint8_t rotRateSource,
const uint8_t mekfEnabled);
/* @brief: Calculates the needed torque for the pointing control mechanism
*/