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

Merge remote-tracking branch 'origin/main' into cfdp-source-handler
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2023-08-16 11:38:16 +02:00
parent 952e1c16e5 8a1e6bc5d2
commit c47995a0eb
25 changed files with 260 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
mission/controller/AcsController.cpp
View File

@ -204,8 +204,7 @@ void AcsController::performSafe() {
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(),
acsParameters.safeModeControllerParameters.useMekf,
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) {
@ -223,7 +222,8 @@ void AcsController::performSafe() {
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value,
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;

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

@ -113,6 +113,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x13:
parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight);
break;
case 0x14:
parameterWrapper->set(mgmHandlingParameters.useMgm4);
break;
default:
return INVALID_IDENTIFIER_ID;
}

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

@ -80,6 +80,7 @@ class AcsParameters : public HasParametersIF {
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
float mgmVectorFilterWeight = 0.85;
float mgmDerivativeFilterWeight = 0.85;
uint8_t useMgm4 = false;
} mgmHandlingParameters;
struct SusHandlingParameters {

2
mission/controller/acs/SensorProcessing.cpp
View File

@ -101,7 +101,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
}
if (mgm4valid) {
if (mgm4valid and mgmParameters->useMgm4) {
float mgm4ValueUT[3];
VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3); // nT to uT
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,

29
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -9,10 +9,12 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid,
const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid,
const uint8_t mekfEnabled,
const uint8_t gyrEnabled,
const uint8_t dampingEnabled) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
@ -20,7 +22,7 @@ acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
} else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateSplitValid) {
} else if (not gyrEnabled and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
@ -95,9 +97,10 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
const double *rotRateOrthogonalB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle) {
void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
const double *rotRateParallelB, const double *rotRateOrthogonalB,
const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -105,8 +108,14 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
errorAngle = acos(dotSun);
std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
if (VectorOperations<double>::norm(rotRateParallelB, 3) != 0 and
VectorOperations<double>::norm(rotRateOrthogonalB, 3) != 0) {
std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
} else {
splitRotationalRate(rotRateTotalB, sunDirB);
}
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_orthoSusMgm);
calculateAngleErrorTorque(sunDirB, sunDirRefB,

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

@ -14,7 +14,6 @@ class SafeCtrl {
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateSplitValid,
const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled);
@ -25,9 +24,10 @@ class SafeCtrl {
void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle);
void safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB,
double *magMomB, double &errorAngle);
void safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
const double *rotRateParallelB, const double *rotRateOrthogonalB,
const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle);
void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle);

65
mission/system/EiveSystem.cpp
View File

@ -133,6 +133,10 @@ void EiveSystem::handleEventMessages() {
case pdec::INVALID_TC_FRAME: {
if (event.getParameter1() == pdec::FRAME_DIRTY_RETVAL) {
frameDirtyErrorCounter++;
// Check whether threshold was reached after 10 seconds.
if (frameDirtyErrorCounter == 1) {
frameDirtyCheckCd.resetTimer();
}
}
break;
}
@ -296,42 +300,39 @@ ReturnValue_t EiveSystem::sendFullRebootCommand() {
}
void EiveSystem::pdecRecoveryLogic() {
if (pdecResetWasAttempted and pdecResetWasAttemptedCd.hasTimedOut()) {
pdecResetWasAttempted = false;
// PDEC reset has happened too often in the last time. Perform reboot to same image.
if (pdecResetCounter >= PDEC_RESET_MAX_COUNT_BEFORE_REBOOT) {
if (waitingForPdecReboot) {
return;
}
triggerEvent(core::PDEC_REBOOT);
// Some delay to ensure that the event is stored in the persistent TM store as well.
TaskFactory::delayTask(500);
// Send reboot command.
ReturnValue_t result = sendSelfRebootCommand();
if (result != returnvalue::OK) {
sif::error << "Sending a reboot command has failed" << std::endl;
// If the previous operation failed, it should be re-attempted the next task cycle.
pdecResetCounterResetCd.resetTimer();
return;
}
waitingForPdecReboot = true;
return;
}
if (frameDirtyCheckCd.hasTimedOut()) {
if (pdecResetCounterResetCd.hasTimedOut()) {
pdecResetCounter = 0;
}
if (frameDirtyCheckCd.hasTimedOut() and frameDirtyErrorCounter > 0) {
if (frameDirtyErrorCounter >= FRAME_DIRTY_COM_REBOOT_LIMIT) {
// If a PTME reset was already attempted and there is still an issue receiving TC frames,
// reboot the system.
if (pdecResetWasAttempted) {
if (waitingForPdecReboot) {
return;
}
triggerEvent(core::PDEC_REBOOT);
// Some delay to ensure that the event is stored in the persistent TM store as well.
TaskFactory::delayTask(500);
// Send reboot command.
ReturnValue_t result = sendSelfRebootCommand();
if (result != returnvalue::OK) {
sif::error << "Sending a reboot command has failed" << std::endl;
// If the previous operation failed, it should be re-attempted the next task cycle.
pdecResetWasAttemptedCd.resetTimer();
return;
}
waitingForPdecReboot = true;
return;
} else {
// Try one full PDEC reset.
CommandMessage msg;
store_address_t dummy{};
ActionMessage::setCommand(&msg, pdec::RESET_PDEC_WITH_REINIITALIZATION, dummy);
commandQueue->sendMessage(pdecHandlerQueueId, &msg);
pdecResetWasAttemptedCd.resetTimer();
pdecResetWasAttempted = true;
}
// Try one full PDEC reset.
CommandMessage msg;
store_address_t dummy{};
ActionMessage::setCommand(&msg, pdec::RESET_PDEC_WITH_REINIITALIZATION, dummy);
commandQueue->sendMessage(pdecHandlerQueueId, &msg);
pdecResetCounterResetCd.resetTimer();
pdecResetCounter++;
}
frameDirtyErrorCounter = 0;
frameDirtyCheckCd.resetTimer();
}
}

5
mission/system/EiveSystem.h
View File

@ -10,6 +10,7 @@
class EiveSystem : public Subsystem, public HasActionsIF {
public:
static constexpr uint8_t FRAME_DIRTY_COM_REBOOT_LIMIT = 4;
static constexpr uint32_t PDEC_RESET_MAX_COUNT_BEFORE_REBOOT = 10;
static constexpr ActionId_t EXECUTE_I2C_REBOOT = 10;
@ -39,11 +40,11 @@ class EiveSystem : public Subsystem, public HasActionsIF {
Countdown frameDirtyCheckCd = Countdown(10000);
// If the PDEC reset was already attempted in the last 2 minutes, there is a high chance that
// only a full reboot will fix the issue.
Countdown pdecResetWasAttemptedCd = Countdown(120000);
bool pdecResetWasAttempted = false;
Countdown pdecResetCounterResetCd = Countdown(120000);
bool waitingForI2cReboot = false;
bool waitingForPdecReboot = false;
uint32_t pdecResetCounter = 0;
ActionHelper actionHelper;
PowerSwitchIF* powerSwitcher = nullptr;
std::atomic_uint16_t& i2cErrors;