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); triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
uint8_t safeCtrlStrat = detumble.detumbleStrategy(mgmDataProcessed.mgmVecTot.isValid(), uint8_t safeCtrlStrat = detumble.detumbleStrategy(
gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid()); mgmDataProcessed.mgmVecTotDerivative.isValid(),
acsParameters.detumbleParameter.useFullDetumbleLaw);
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
switch (safeCtrlStrat) { switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL): case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainD); magMomMtq, acsParameters.detumbleParameter.gainFull);
break; break;
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainD); magMomMtq, acsParameters.detumbleParameter.gainBdot);
break; break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0); 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); parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
break; break;
case 0x3: 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; break;
default: default:
return INVALID_IDENTIFIER_ID; 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 sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
double sunTargetDir[3] = {0, 0, 1}; double sunTargetDir[3] = {0, 0, 1};
uint8_t useMekf = true; uint8_t useMekf = false;
uint8_t dampingDuringEclipse = true; uint8_t dampingDuringEclipse = true;
} safeModeControllerParameters; } safeModeControllerParameters;
@ -931,8 +931,10 @@ class AcsParameters : public HasParametersIF {
struct DetumbleParameter { struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2 * M_PI / 180; double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.4 * M_PI / 180; double omegaDetumbleEnd = 1 * M_PI / 180;
double gainD = pow(10.0, -3.3); double gainBdot = pow(10.0, -3.3);
double gainFull = pow(10.0, -2.3);
uint8_t useFullDetumbleLaw = false;
} detumbleParameter; } detumbleParameter;
}; };

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

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

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

@ -12,7 +12,7 @@ class Detumble {
virtual ~Detumble(); virtual ~Detumble();
uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, 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, void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
double gain); double gain);