diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 060e0aac..1d1502f2 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -321,29 +321,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x8): // SafeModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(safeModeControllerParameters.k_rate_mekf); + parameterWrapper->set(safeModeControllerParameters.k_ortho_mekf); break; case 0x1: parameterWrapper->set(safeModeControllerParameters.k_align_mekf); break; case 0x2: - parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf); + parameterWrapper->set(safeModeControllerParameters.k_parallel_mekf); break; case 0x3: - parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf); + parameterWrapper->set(safeModeControllerParameters.k_ortho_no_mekf); break; case 0x4: - parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); + parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf); break; case 0x5: - parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); + parameterWrapper->set(safeModeControllerParameters.k_parallel_no_mekf); break; case 0x6: - parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); + parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); break; case 0x7: + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); + break; + case 0x8: + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); + break; + case 0x9: parameterWrapper->setVector(safeModeControllerParameters.satRateRef); break; + case 0xA: + parameterWrapper->set(safeModeControllerParameters.cosineStartSpin); + break; + case 0xB: + parameterWrapper->set(safeModeControllerParameters.sineCalculateOmegaSun); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 5b185ea4..3e956ae3 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -811,11 +811,13 @@ class AcsParameters : public HasParametersIF { } rwMatrices; struct SafeModeControllerParameters { - double k_rate_mekf = 0.00059437; + double k_ortho_mekf = 0.00059437; double k_align_mekf = 0.000056875; + double k_parallel_mekf = 0; - double k_rate_no_mekf = 0.00059437; + double k_ortho_no_mekf = 0.00059437; double k_align_no_mekf = 0.000056875; + double k_parallel_no_mekf = 0; double sunMagAngleMin = 5 * M_PI / 180; @@ -823,6 +825,9 @@ class AcsParameters : public HasParametersIF { double sunTargetDir[3] = {0, 0, 1}; double satRateRef[3] = {0, 0, 0}; + + float cosineStartSpin = 0.9; + float sineCalculateOmegaSun = 0.24; } safeModeControllerParameters; struct PointingLawParameters {