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

detumble mode changes
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-04-14 11:38:05 +02:00
parent 53cccc3c13
commit 1e521f0575
5 changed files with 21 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
mission/controller/AcsController.cpp
View File

@ -259,18 +259,19 @@ void AcsController::performDetumble() {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false;
}
uint8_t safeCtrlStrat = detumble.detumbleStrategy(mgmDataProcessed.mgmVecTot.isValid(),
gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid());
uint8_t safeCtrlStrat = detumble.detumbleStrategy(
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(),
acsParameters.detumbleParameter.useFullDetumbleLaw);
double magMomMtq[3] = {0, 0, 0};
switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainD);
magMomMtq, acsParameters.detumbleParameter.gainFull);
break;
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainD);
magMomMtq, acsParameters.detumbleParameter.gainBdot);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0);

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

@ -711,7 +711,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
break;
case 0x3:
parameterWrapper->set(detumbleParameter.gainD);
parameterWrapper->set(detumbleParameter.gainBdot);
break;
case 0x4:
parameterWrapper->set(detumbleParameter.gainFull);
break;
case 0x5:
parameterWrapper->set(detumbleParameter.useFullDetumbleLaw);
break;
default:
return INVALID_IDENTIFIER_ID;

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

@ -821,7 +821,7 @@ class AcsParameters : public HasParametersIF {
double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
double sunTargetDir[3] = {0, 0, 1};
uint8_t useMekf = true;
uint8_t useMekf = false;
uint8_t dampingDuringEclipse = true;
} safeModeControllerParameters;
@ -931,8 +931,10 @@ class AcsParameters : public HasParametersIF {
struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.4 * M_PI / 180;
double gainD = pow(10.0, -3.3);
double omegaDetumbleEnd = 1 * M_PI / 180;
double gainBdot = pow(10.0, -3.3);
double gainFull = pow(10.0, -2.3);
uint8_t useFullDetumbleLaw = false;
} detumbleParameter;
};

4
mission/controller/acs/control/Detumble.cpp
View File

@ -8,10 +8,10 @@ Detumble::Detumble() {}
Detumble::~Detumble() {}
uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const bool magFieldRateValid) {
const bool magFieldRateValid, const bool useFullDetumbleLaw) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (satRotRateValid) {
} else if (satRotRateValid and useFullDetumbleLaw) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
} else if (magFieldRateValid) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;

2
mission/controller/acs/control/Detumble.h
View File

@ -12,7 +12,7 @@ class Detumble {
virtual ~Detumble();
uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const bool magFieldRateValid);
const bool magFieldRateValid, const bool useFullDetumbleLaw);
void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
double gain);