From 22558a2f3929692503119e4f6f654441642bc262 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 14:02:35 +0100 Subject: [PATCH 01/38] fixed some param types --- mission/controller/acs/AcsParameters.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 11de10a3..f44f4a61 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -783,9 +783,9 @@ class AcsParameters : public HasParametersIF { struct RwHandlingParameters { double inertiaWheel = 0.000028198; - double maxTrq = 0.0032; // 3.2 [mNm] - int32_t stictionSpeed = 100; // RPM - int32_t stictionReleaseSpeed = 120; // RPM + double maxTrq = 0.0032; // 3.2 [mNm] + int32_t stictionSpeed = 100; // RPM + int32_t stictionReleaseSpeed = 120; // RPM double stictionTorque = 0.0006; uint16_t rampTime = 10; @@ -843,7 +843,7 @@ class AcsParameters : public HasParametersIF { double refDirection[3] = {-1, 0, 0}; // Antenna double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude @@ -859,7 +859,7 @@ class AcsParameters : public HasParametersIF { struct GsTargetModeControllerParameters : PointingLawParameters { double refDirection[3] = {-1, 0, 0}; // Antenna - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude @@ -871,7 +871,7 @@ class AcsParameters : public HasParametersIF { double refDirection[3] = {-1, 0, 0}; // Antenna double quatRef[4] = {0, 0, 0, 1}; double refRotRate[3] = {0, 0, 0}; - int8_t timeElapsedMax = 10; // rot rate calculations + uint8_t timeElapsedMax = 10; // rot rate calculations } nadirModeControllerParameters; struct InertialModeControllerParameters : PointingLawParameters { From 17a1a5a655a8ded27f382dbbc23fc65de632042f Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 14:29:23 +0100 Subject: [PATCH 02/38] fixed paramerterWrapper setter to match type of parameter --- mission/controller/acs/AcsParameters.cpp | 202 +++++++++++------------ 1 file changed, 101 insertions(+), 101 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 2e5fbb1b..351601c3 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -30,19 +30,19 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x2: // InertiaEIVE switch (parameterId) { case 0x0: - parameterWrapper->set(inertiaEIVE.inertiaMatrix); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrix); break; case 0x1: - parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed); break; case 0x2: - parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed); break; case 0x3: - parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1); break; case 0x4: - parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3); + parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3); break; default: return INVALID_IDENTIFIER_ID; @@ -51,58 +51,58 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x3: // MgmHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm0orientationMatrix); break; case 0x1: - parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm1orientationMatrix); break; case 0x2: - parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm2orientationMatrix); break; case 0x3: - parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm3orientationMatrix); break; case 0x4: - parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm4orientationMatrix); break; case 0x5: - parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm0hardIronOffset); break; case 0x6: - parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm1hardIronOffset); break; case 0x7: - parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm2hardIronOffset); break; case 0x8: - parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm3hardIronOffset); break; case 0x9: - parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); + parameterWrapper->setVector(mgmHandlingParameters.mgm4hardIronOffset); break; case 0xA: - parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm0softIronInverse); break; case 0xB: - parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm1softIronInverse); break; case 0xC: - parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm2softIronInverse); break; case 0xD: - parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm3softIronInverse); break; case 0xE: - parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); + parameterWrapper->setMatrix(mgmHandlingParameters.mgm4softIronInverse); break; case 0xF: - parameterWrapper->set(mgmHandlingParameters.mgm02variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm02variance); break; case 0x10: - parameterWrapper->set(mgmHandlingParameters.mgm13variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm13variance); break; case 0x11: - parameterWrapper->set(mgmHandlingParameters.mgm4variance); + parameterWrapper->setVector(mgmHandlingParameters.mgm4variance); break; default: return INVALID_IDENTIFIER_ID; @@ -111,112 +111,112 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x4: // SusHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus0orientationMatrix); break; case 0x1: - parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus1orientationMatrix); break; case 0x2: - parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus2orientationMatrix); break; case 0x3: - parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus3orientationMatrix); break; case 0x4: - parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus4orientationMatrix); break; case 0x5: - parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus5orientationMatrix); break; case 0x6: - parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus6orientationMatrix); break; case 0x7: - parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus7orientationMatrix); break; case 0x8: - parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus8orientationMatrix); break; case 0x9: - parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus9orientationMatrix); break; case 0xA: - parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus10orientationMatrix); break; case 0xB: - parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); + parameterWrapper->setMatrix(susHandlingParameters.sus11orientationMatrix); break; case 0xC: - parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus0coeffAlpha); break; case 0xD: - parameterWrapper->set(susHandlingParameters.sus0coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus0coeffBeta); break; case 0xE: - parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus1coeffAlpha); break; case 0xF: - parameterWrapper->set(susHandlingParameters.sus1coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus1coeffBeta); break; case 0x10: - parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus2coeffAlpha); break; case 0x11: - parameterWrapper->set(susHandlingParameters.sus2coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus2coeffBeta); break; case 0x12: - parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus3coeffAlpha); break; case 0x13: - parameterWrapper->set(susHandlingParameters.sus3coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus3coeffBeta); break; case 0x14: - parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus4coeffAlpha); break; case 0x15: - parameterWrapper->set(susHandlingParameters.sus4coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus4coeffBeta); break; case 0x16: - parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus5coeffAlpha); break; case 0x17: - parameterWrapper->set(susHandlingParameters.sus5coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus5coeffBeta); break; case 0x18: - parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus6coeffAlpha); break; case 0x19: - parameterWrapper->set(susHandlingParameters.sus6coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus6coeffBeta); break; case 0x1A: - parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus7coeffAlpha); break; case 0x1B: - parameterWrapper->set(susHandlingParameters.sus7coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus7coeffBeta); break; case 0x1C: - parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus8coeffAlpha); break; case 0x1D: - parameterWrapper->set(susHandlingParameters.sus8coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus8coeffBeta); break; case 0x1E: - parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus9coeffAlpha); break; case 0x1F: - parameterWrapper->set(susHandlingParameters.sus9coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus9coeffBeta); break; case 0x20: - parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus10coeffAlpha); break; case 0x21: - parameterWrapper->set(susHandlingParameters.sus10coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus10coeffBeta); break; case 0x22: - parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); + parameterWrapper->setMatrix(susHandlingParameters.sus11coeffAlpha); break; case 0x23: - parameterWrapper->set(susHandlingParameters.sus11coeffBeta); + parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta); break; default: return INVALID_IDENTIFIER_ID; @@ -225,34 +225,34 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x5): // GyrHandlingParameters switch (parameterId) { case 0x0: - parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr0orientationMatrix); break; case 0x1: - parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr1orientationMatrix); break; case 0x2: - parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr2orientationMatrix); break; case 0x3: - parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); + parameterWrapper->setMatrix(gyrHandlingParameters.gyr3orientationMatrix); break; case 0x4: - parameterWrapper->set(gyrHandlingParameters.gyr0bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr0bias); break; case 0x5: - parameterWrapper->set(gyrHandlingParameters.gyr1bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr1bias); break; case 0x6: - parameterWrapper->set(gyrHandlingParameters.gyr2bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr2bias); break; case 0x7: - parameterWrapper->set(gyrHandlingParameters.gyr3bias); + parameterWrapper->setVector(gyrHandlingParameters.gyr3bias); break; case 0x8: - parameterWrapper->set(gyrHandlingParameters.gyr02variance); + parameterWrapper->setVector(gyrHandlingParameters.gyr02variance); break; case 0x9: - parameterWrapper->set(gyrHandlingParameters.gyr13variance); + parameterWrapper->setVector(gyrHandlingParameters.gyr13variance); break; case 0xA: parameterWrapper->set(gyrHandlingParameters.preferAdis); @@ -288,25 +288,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x7): // RwMatrices switch (parameterId) { case 0x0: - parameterWrapper->set(rwMatrices.alignmentMatrix); + parameterWrapper->setMatrix(rwMatrices.alignmentMatrix); break; case 0x1: - parameterWrapper->set(rwMatrices.pseudoInverse); + parameterWrapper->setMatrix(rwMatrices.pseudoInverse); break; case 0x2: - parameterWrapper->set(rwMatrices.without1); + parameterWrapper->setMatrix(rwMatrices.without1); break; case 0x3: - parameterWrapper->set(rwMatrices.without2); + parameterWrapper->setMatrix(rwMatrices.without2); break; case 0x4: - parameterWrapper->set(rwMatrices.without3); + parameterWrapper->setMatrix(rwMatrices.without3); break; case 0x5: - parameterWrapper->set(rwMatrices.without4); + parameterWrapper->setMatrix(rwMatrices.without4); break; case 0x6: - parameterWrapper->set(rwMatrices.nullspace); + parameterWrapper->setVector(rwMatrices.nullspace); break; default: return INVALID_IDENTIFIER_ID; @@ -330,13 +330,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); break; case 0x5: - parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop); + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); break; case 0x6: - parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); break; case 0x7: - parameterWrapper->set(safeModeControllerParameters.satRateRef); + parameterWrapper->setVector(safeModeControllerParameters.satRateRef); break; default: return INVALID_IDENTIFIER_ID; @@ -360,7 +360,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); @@ -394,7 +394,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); @@ -406,13 +406,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(targetModeControllerParameters.refDirection); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xB: - parameterWrapper->set(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xC: parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); @@ -460,7 +460,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); @@ -472,13 +472,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(targetModeControllerParameters.refDirection); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xB: - parameterWrapper->set(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xC: parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); @@ -514,7 +514,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); @@ -526,10 +526,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(nadirModeControllerParameters.refDirection); + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->set(nadirModeControllerParameters.quatRef); + parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xC: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); @@ -556,7 +556,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); break; case 0x6: parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); @@ -568,13 +568,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->set(inertialModeControllerParameters.tgtQuat); + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; case 0xA: - parameterWrapper->set(inertialModeControllerParameters.refRotRate); + parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; case 0xC: - parameterWrapper->set(inertialModeControllerParameters.quatRef); + parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: return INVALID_IDENTIFIER_ID; @@ -586,7 +586,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(strParameters.exclusionAngle); break; case 0x1: - parameterWrapper->set(strParameters.boresightAxis); + parameterWrapper->setVector(strParameters.boresightAxis); break; default: return INVALID_IDENTIFIER_ID; @@ -658,19 +658,19 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x12): // MagnetorquesParameter switch (parameterId) { case 0x0: - parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); + parameterWrapper->setMatrix(magnetorquesParameter.mtq0orientationMatrix); break; case 0x1: - parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); + parameterWrapper->setMatrix(magnetorquesParameter.mtq1orientationMatrix); break; case 0x2: - parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); + parameterWrapper->setMatrix(magnetorquesParameter.mtq2orientationMatrix); break; case 0x3: - parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); + parameterWrapper->setMatrix(magnetorquesParameter.alignmentMatrixMtq); break; case 0x4: - parameterWrapper->set(magnetorquesParameter.inverseAlignment); + parameterWrapper->setMatrix(magnetorquesParameter.inverseAlignment); break; case 0x5: parameterWrapper->set(magnetorquesParameter.DipolMax); From afb9303dcb5e54d90338f7d244a77c7dee71599d Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 16:35:32 +0100 Subject: [PATCH 03/38] naming fixes --- mission/controller/acs/AcsParameters.cpp | 14 +++++++------- mission/controller/acs/AcsParameters.h | 6 +++--- mission/controller/acs/control/Detumble.cpp | 4 ++-- mission/controller/acs/control/Detumble.h | 2 +- 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 351601c3..459d5ee4 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -658,25 +658,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x12): // MagnetorquesParameter switch (parameterId) { case 0x0: - parameterWrapper->setMatrix(magnetorquesParameter.mtq0orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq0orientationMatrix); break; case 0x1: - parameterWrapper->setMatrix(magnetorquesParameter.mtq1orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq1orientationMatrix); break; case 0x2: - parameterWrapper->setMatrix(magnetorquesParameter.mtq2orientationMatrix); + parameterWrapper->setMatrix(magnetorquerParameter.mtq2orientationMatrix); break; case 0x3: - parameterWrapper->setMatrix(magnetorquesParameter.alignmentMatrixMtq); + parameterWrapper->setMatrix(magnetorquerParameter.alignmentMatrixMtq); break; case 0x4: - parameterWrapper->setMatrix(magnetorquesParameter.inverseAlignment); + parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); break; case 0x5: - parameterWrapper->set(magnetorquesParameter.DipolMax); + parameterWrapper->set(magnetorquerParameter.dipolMax); break; case 0x6: - parameterWrapper->set(magnetorquesParameter.torqueDuration); + parameterWrapper->set(magnetorquerParameter.torqueDuration); break; default: return INVALID_IDENTIFIER_ID; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f44f4a61..ea867c65 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -912,16 +912,16 @@ class AcsParameters : public HasParametersIF { double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability } kalmanFilterParameters; - struct MagnetorquesParameter { + struct MagnetorquerParameter { double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; - double DipolMax = 0.2; // [Am^2] + double dipolMax = 0.2; // [Am^2] uint16_t torqueDuration = 300; // [ms] - } magnetorquesParameter; + } magnetorquerParameter; struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 705bf599..222a0ec2 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -23,7 +23,7 @@ Detumble::~Detumble() {} void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { detumbleParameter = &(acsParameters_->detumbleParameter); - magnetorquesParameter = &(acsParameters_->magnetorquesParameter); + magnetorquesParameter = &(acsParameters_->magnetorquerParameter); } ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, @@ -43,7 +43,7 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal return DETUMBLE_NO_SENSORDATA; } - double dipolMax = magnetorquesParameter->DipolMax; + double dipolMax = magnetorquesParameter->dipolMax; for (int i = 0; i < 3; i++) { magMom[i] = -dipolMax * sign(magRate[i]); } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 65e5ec28..dee82271 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -35,7 +35,7 @@ class Detumble { private: AcsParameters::DetumbleParameter *detumbleParameter; - AcsParameters::MagnetorquesParameter *magnetorquesParameter; + AcsParameters::MagnetorquerParameter *magnetorquesParameter; }; #endif /*ACS_CONTROL_DETUMBLE_H_*/ From fd4be08796cc1b1e94f2b00d8b623416c905098a Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 16:37:09 +0100 Subject: [PATCH 04/38] removed instance of AcsParameters as part of actuatorCmd --- mission/controller/acs/ActuatorCmd.cpp | 25 ++++++++++++++----------- mission/controller/acs/ActuatorCmd.h | 12 +++++++----- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 8cda84d6..7ea6d567 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -10,13 +10,14 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } +ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} -void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { +void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, + AcsParameters::RwHandlingParameters *rwHandlingParameters) { // Scaling the commanded torque to a maximum value - double maxTrq = acsParameters.rwHandlingParameters.maxTrq; + double maxTrq = rwHandlingParameters->maxTrq; double maxValue = 0; for (int i = 0; i < 4; i++) { // size of torque, always 4 ? @@ -33,17 +34,17 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, - const double *rwTorque, int32_t *rwCmdSpeed) { + const double *rwTorque, int32_t *rwCmdSpeed, + AcsParameters *acsParameters) { using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; double deltaSpeed[4] = {0, 0, 0, 0}; - double commandTime = acsParameters.onBoardParams.sampleTime, - inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; double radToRpm = 60 / (2 * PI); // factor for conversion to RPM // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = commandTime / inertiaWheel * radToRpm; + double factor = acsParameters->onBoardParams.sampleTime / + acsParameters->rwHandlingParameters.inertiaWheel * radToRpm; int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); for (int i = 0; i < 4; i++) { @@ -53,13 +54,15 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) { +void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, + AcsParameters::MagnetorquerParameter *magnetorquerParameter) { + sif::debug << magnetorquerParameter->dipolMax << std::endl; // Convert to actuator frame double dipolMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, - dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); + MatrixOperations::multiply(*magnetorquerParameter->inverseAlignment, dipolMoment, + dipolMomentActuatorDouble, 3, 3, 1); // Scaling along largest element if dipol exceeds maximum - double maxDipol = acsParameters.magnetorquesParameter.DipolMax; + double maxDipol = magnetorquerParameter->dipolMax; double maxValue = 0; for (int i = 0; i < 3; i++) { if (abs(dipolMomentActuator[i]) > maxDipol) { diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 969bd782..ca0deb5b 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -8,7 +8,7 @@ class ActuatorCmd { public: - ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? + ActuatorCmd(); // Input mode ? virtual ~ActuatorCmd(); /* @@ -17,7 +17,8 @@ class ActuatorCmd { * @param: rwTrq given torque for reaction wheels * rwTrqScaled possible scaled torque */ - void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); + void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, + AcsParameters::RwHandlingParameters *rwHandlingParameters); /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction @@ -29,7 +30,8 @@ class ActuatorCmd { * reaction wheel */ void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, - const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed); + const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, + AcsParameters *acsParameters); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -37,11 +39,11 @@ class ActuatorCmd { * @param: dipolMoment given dipol moment in spacecraft frame * dipolMomentActuator resulting dipol moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator); + void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, + AcsParameters::MagnetorquerParameter *magnetorquerParameter); protected: private: - AcsParameters acsParameters; }; #endif /* ACTUATORCMD_H_ */ From 92f5a8bf896ee840eeee202fb8e75b7f393ecf19 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 16:37:46 +0100 Subject: [PATCH 05/38] removed AcsParameters as part of Navigation --- .../acs/MultiplicativeKalmanFilter.cpp | 96 +++++++++---------- .../acs/MultiplicativeKalmanFilter.h | 22 ++--- mission/controller/acs/Navigation.cpp | 11 +-- mission/controller/acs/Navigation.h | 6 +- 4 files changed, 64 insertions(+), 71 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index a700c6a6..77a3ef00 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -12,33 +12,22 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -/*Initialisation of values for parameters in constructor*/ -MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) - : initialQuaternion{0, 0, 0, 1}, - initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} { - loadAcsParameters(acsParameters_); -} +MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {} MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} -void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); -} - ReturnValue_t MultiplicativeKalmanFilter::init( const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"? + const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters) { // valids for "model measurements"? // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { - validInit = true; // QUEST ALGO ----------------------------------------------------------------------- double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; - sigmaSun = kalmanFilterParameters->sensorNoiseSS; - sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - sigmaGyro = kalmanFilterParameters->sensorNoiseGYR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; + sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -192,21 +181,18 @@ ReturnValue_t MultiplicativeKalmanFilter::init( return MEKF_INITIALIZED; } else { // no initialisation possible, no valid measurements - validInit = false; updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); return MEKF_UNINITIALIZED; } } // --------------- MEKF DISCRETE TIME STEP ------------------------------- -ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, const bool validSTR_, - const double *rateGYRs_, const bool validGYRs_, - const double *magneticField_, - const bool validMagField_, const double *sunDir_, - const bool validSS, const double *sunDirJ, - const bool validSSModel, const double *magFieldJ, - const bool validMagModel, double sampleTime, - acsctrl::MekfData *mekfData) { +ReturnValue_t MultiplicativeKalmanFilter::mekfEst( + const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_, + const bool validGYRs_, const double *magneticField_, const bool validMagField_, + const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters) { // Check for GYR Measurements int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { @@ -248,9 +234,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // If we are here, MEKF will perform double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; - sigmaSun = kalmanFilterParameters->sensorNoiseSS; - sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - sigmaStr = kalmanFilterParameters->sensorNoiseSTR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; + sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -912,8 +898,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c biasGYR[2] = updatedGyroBias[2]; /* ----------- PROPAGATION ----------*/ - double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; - double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; + double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR; + double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR; double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; @@ -931,27 +917,31 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - if (normRotEst * sampleTime < M_PI / 10) { - double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3); + if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) { + double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime + + 1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3); MatrixOperations::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3); - double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2)); + double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2)); MatrixOperations::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3); std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double)); - double fact22 = pow(sigmaU, 2) * sampleTime; + double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); } else { - double fact22 = pow(sigmaU, 2) * sampleTime; + double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3]; - double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + double fact12_0 = + (normRotEst * acsParameters->onBoardParams.sampleTime - + sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3)); MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3); - double fact12_1 = 1. / 2. * pow(sampleTime, 2); + double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2); MatrixOperations::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3); double fact12_2 = - (1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / + (1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) + + cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) / pow(normRotEst, 4); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3); MatrixOperations::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3); @@ -961,13 +951,15 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c MatrixOperations::transpose(*covQ12, *covQ12trans, 3); double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3]; - double fact11_0 = pow(sigmaV, 2) * sampleTime; + double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime; MatrixOperations::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3); - double fact11_1 = 1. / 3. * pow(sampleTime, 3); + double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3); MatrixOperations::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3); - double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) - - 1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) / - pow(normRotEst, 5); + double fact11_2 = + (2 * normRotEst * acsParameters->onBoardParams.sampleTime - + 2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) - + 1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) / + pow(normRotEst, 5); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3); MatrixOperations::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3); MatrixOperations::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3); @@ -1017,9 +1009,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3]; - double fact11_1 = sin(normRotEst * sampleTime) / normRotEst; + double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst; MatrixOperations::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3); - double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); + double fact11_2 = + (1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3); MatrixOperations::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3); MatrixOperations::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3); @@ -1028,8 +1021,11 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3]; double fact12_0 = fact11_2; MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3); - double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); + MatrixOperations::multiplyScalar(*identityMatrix3, + acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3); + double fact12_2 = + (normRotEst * acsParameters->onBoardParams.sampleTime - + sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3)); MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3); MatrixOperations::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3); MatrixOperations::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3); @@ -1056,8 +1052,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c // Propagated Quaternion double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double rotCos = cos(0.5 * normRotEst * sampleTime); - double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst; + double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime); + double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst; VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 47e1f807..3e9bfa49 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -17,9 +17,8 @@ class MultiplicativeKalmanFilter { */ public: /* @brief: Constructor - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ - MultiplicativeKalmanFilter(AcsParameters *acsParameters_); + MultiplicativeKalmanFilter(); virtual ~MultiplicativeKalmanFilter(); ReturnValue_t reset(acsctrl::MekfData *mekfData); @@ -33,8 +32,8 @@ class MultiplicativeKalmanFilter { */ ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, - const double *magFieldJ, const bool validMagModel, - acsctrl::MekfData *mekfData); + const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -54,7 +53,8 @@ class MultiplicativeKalmanFilter { const bool validGYRs_, const double *magneticField_, const bool validMagField_, const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, - const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); + const bool validMagModel, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); enum MekfStatus : uint8_t { UNINITIALIZED = 0, @@ -79,23 +79,21 @@ class MultiplicativeKalmanFilter { private: /*Parameters*/ - AcsParameters::InertiaEIVE *inertiaEIVE; - AcsParameters::KalmanFilterParameters *kalmanFilterParameters; double quaternion_STR_SB[4]; - bool validInit; /*States*/ - double initialQuaternion[4]; /*after reset?QUEST*/ - double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/ + double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/ + double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ - uint8_t sensorsAvail; + uint8_t sensorsAvail = 0; /*Outputs*/ double quatBJ[4]; /* Output Quaternion */ double biasGYR[3]; /*Between measured and estimated sat Rate*/ /*Parameter INIT*/ /*Functions*/ - void loadAcsParameters(AcsParameters *acsParameters_); void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus); void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4], double satRotRate[3]); diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 03446609..9e8b3719 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -8,9 +8,7 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) { - acsParameters = *acsParameters_; -} +Navigation::Navigation() {} Navigation::~Navigation() {} @@ -18,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::MekfData *mekfData) { + acsctrl::MekfData *mekfData, AcsParameters *acsParameters) { double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; bool quatIBValid = sensorValues->strSet.caliQx.isValid() && @@ -30,7 +28,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), - mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData); + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData, + acsParameters); return mekfStatus; } else { mekfStatus = multiplicativeKalmanFilter.mekfEst( @@ -39,7 +38,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, - mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); + mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters); return mekfStatus; } } diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index cf9e81e3..b567fbdd 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -9,19 +9,19 @@ class Navigation { public: - Navigation(AcsParameters *acsParameters_); + Navigation(); virtual ~Navigation(); ReturnValue_t useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData); + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, + AcsParameters *acsParameters); void resetMekf(acsctrl::MekfData *mekfData); protected: private: MultiplicativeKalmanFilter multiplicativeKalmanFilter; - AcsParameters acsParameters; ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED; }; From 12ac8f7c708a1fbdb20f093bce9bfeccb249548d Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 16:38:59 +0100 Subject: [PATCH 06/38] removed AcsParameters as part of SensorProcessing --- mission/controller/acs/SensorProcessing.cpp | 5 +++-- mission/controller/acs/SensorProcessing.h | 3 +-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index e268d786..466d9ed5 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -14,7 +14,7 @@ using namespace Math; -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {} +SensorProcessing::SensorProcessing() {} SensorProcessing::~SensorProcessing() {} @@ -26,7 +26,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed) { - // ---------------- IGRF- 13 Implementation here ------------------------------------------------ + // ---------------- IGRF- 13 Implementation here + // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; if (gpsValid) { // Should be existing class object which will be called and modified here. diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index cdd29d8b..d845c2f3 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -15,7 +15,7 @@ class SensorProcessing { public: void reset(); - SensorProcessing(AcsParameters *acsParameters_); + SensorProcessing(); virtual ~SensorProcessing(); void process(timeval now, ACS::SensorValues *sensorValues, @@ -77,7 +77,6 @@ class SensorProcessing { bool validSavedPosSatE = false; SusConverter susConverter; - AcsParameters acsParameters; }; #endif /*SENSORPROCESSING_H_*/ From 7bf72e3c18ee97c07acc5a0c64ad684c7c3b5999 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 16:43:31 +0100 Subject: [PATCH 07/38] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b078c4d7..9a4d88e0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,9 @@ will consitute of a breaking change warranting a new major release: - Linux GPS handler now checks the individual `*_SET` flags when analysing the `gpsd` struct. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/400 +- Several `AcsController` components had their own implementation of `AcsParameters`. This resulted + in those parameters not being updated, while the actual ones were updated. All instances of + `AcsParameters` not belonging to `AcsController` are removed now. # [v1.32.0] From 309358f44708981a14f6ad03849dadd20e14f520 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 17:05:09 +0100 Subject: [PATCH 08/38] fixed updating wrong parameter set --- CHANGELOG.md | 5 +++- mission/controller/acs/AcsParameters.cpp | 34 ++++++++++-------------- 2 files changed, 18 insertions(+), 21 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9a4d88e0..d96c36ab 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,7 +22,10 @@ will consitute of a breaking change warranting a new major release: PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/400 - Several `AcsController` components had their own implementation of `AcsParameters`. This resulted in those parameters not being updated, while the actual ones were updated. All instances of - `AcsParameters` not belonging to `AcsController` are removed now. + `AcsParameters` not belonging to `AcsController` are eiter removed or replaced by pointer + instances. +- Instead of updating the `gsTargetModeControllerParameters`, the `targetModeControllerParameters` + were updated. # [v1.32.0] diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 459d5ee4..a19571eb 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -445,52 +445,46 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0xB): // GsTargetModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(targetModeControllerParameters.zeta); + parameterWrapper->set(gsTargetModeControllerParameters.zeta); break; case 0x1: - parameterWrapper->set(targetModeControllerParameters.om); + parameterWrapper->set(gsTargetModeControllerParameters.om); break; case 0x2: - parameterWrapper->set(targetModeControllerParameters.omMax); + parameterWrapper->set(gsTargetModeControllerParameters.omMax); break; case 0x3: - parameterWrapper->set(targetModeControllerParameters.qiMin); + parameterWrapper->set(gsTargetModeControllerParameters.qiMin); break; case 0x4: - parameterWrapper->set(targetModeControllerParameters.gainNullspace); + parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0x9: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xC: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); - break; - case 0xE: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); - break; - case 0xF: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: return INVALID_IDENTIFIER_ID; From 5c30adb9bb9f4f8fecc1d6aa27de39ce404b5100 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 17:08:48 +0100 Subject: [PATCH 09/38] changed AcsParameters instance in guidance to const pointer --- mission/controller/AcsController.cpp | 33 ++++++------- mission/controller/acs/ActuatorCmd.cpp | 1 - mission/controller/acs/Guidance.cpp | 67 +++++++++++++------------- mission/controller/acs/Guidance.h | 3 +- 4 files changed, 51 insertions(+), 53 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f3269285..b056f093 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -6,9 +6,6 @@ AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), - sensorProcessing(&acsParameters), - navigation(&acsParameters), - actuatorCmd(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), detumble(&acsParameters), @@ -142,7 +139,7 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -173,7 +170,7 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && @@ -207,7 +204,7 @@ void AcsController::performDetumble() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -221,7 +218,7 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -254,7 +251,7 @@ void AcsController::performPointingCtrl() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, - &susDataProcessed, &mekfData); + &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { @@ -303,7 +300,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -327,7 +324,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -348,7 +345,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -372,7 +369,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -395,7 +392,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -409,11 +406,11 @@ void AcsController::performPointingCtrl() { ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); } - actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); + actuatorCmd.cmdSpeedToRws( + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, + cmdSpeedRws, &acsParameters); + actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, &acsParameters.magnetorquerParameter); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 7ea6d567..7173151d 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -56,7 +56,6 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, AcsParameters::MagnetorquerParameter *magnetorquerParameter) { - sif::debug << magnetorquerParameter->dipolMax << std::endl; // Convert to actuator frame double dipolMomentActuatorDouble[3] = {0, 0, 0}; MatrixOperations::multiply(*magnetorquerParameter->inverseAlignment, dipolMoment, diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 09f0be20..9e9c04f9 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -12,7 +12,7 @@ #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" -Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {} +Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::~Guidance() {} @@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetE); + acsParameters->targetModeControllerParameters.latitudeTgt, + acsParameters->targetModeControllerParameters.longitudeTgt, + acsParameters->targetModeControllerParameters.altitudeTgt, targetE); // target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; @@ -57,9 +57,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; + refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2]; double noramlizedTargetDirB[3] = {0, 0, 0}; VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations::normalize(refDir, refDir, 3); @@ -96,15 +96,15 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve //------------------------------------------------------------------------------------- // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- - if (acsParameters.targetModeControllerParameters.avoidBlindStr) { + if (acsParameters->targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1); - double exclAngle = acsParameters.strParameters.exclusionAngle, - blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, - blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; + double exclAngle = acsParameters->strParameters.exclusionAngle, + blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart, + blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop; double sightAngleSun = - VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); + VectorOperations::dot(acsParameters->strParameters.boresightAxis, sunDirB); if (!(strBlindAvoidFlag)) { double critSightAngle = blindStart * exclAngle; @@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve } } else { if (sightAngleSun < blindEnd * exclAngle) { - double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; + double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate; double blindRefRate[3] = {0, 0, 0}; if (sunDirB[1] < 0) { blindRefRate[0] = normBlindRefRate; @@ -144,9 +144,9 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, - acsParameters.targetModeControllerParameters.longitudeTgt, - acsParameters.targetModeControllerParameters.altitudeTgt, targetE); + acsParameters->targetModeControllerParameters.latitudeTgt, + acsParameters->targetModeControllerParameters.longitudeTgt, + acsParameters->targetModeControllerParameters.altitudeTgt, targetE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetE, posSatE, targetDirE, 3); @@ -198,12 +198,13 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmIX, targetQuat); - int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double targetQuat[4], double targetSatRotRate[3]) { + sif::debug << acsParameters->gsTargetModeControllerParameters.altitudeTgt << std::endl; //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- @@ -211,9 +212,9 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] double groundStationE[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.gsTargetModeControllerParameters.latitudeTgt, - acsParameters.gsTargetModeControllerParameters.longitudeTgt, - acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE); + acsParameters->gsTargetModeControllerParameters.latitudeTgt, + acsParameters->gsTargetModeControllerParameters.longitudeTgt, + acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(groundStationE, posSatE, targetDirE, 3); @@ -262,7 +263,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); - int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } @@ -332,9 +333,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0]; - refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2]; + refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2]; double noramlizedTargetDirB[3] = {0, 0, 0}; VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations::normalize(refDir, refDir, 3); @@ -406,7 +407,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl {xAxis[2], yAxis[2], zAxis[2]}}; QuaternionOperations::fromDcm(dcmTgt, targetQuat); - int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; + int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax; targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate); } @@ -516,19 +517,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid()); if (rw1valid && rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; } else if (!rw1valid && rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && !rw2valid && rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && rw2valid && !rw3valid && rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); return returnvalue::OK; } else if (rw1valid && rw2valid && rw3valid && !rw4valid) { - std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double)); + std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { // @note: This one takes the normal pseudoInverse of all four raction wheels valid. @@ -542,13 +543,13 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, + std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, + std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop, 3 * sizeof(double)); } - std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, + std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef, 3 * sizeof(double)); } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index da9d429b..c7f465f8 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -55,7 +55,8 @@ class Guidance { ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); private: - AcsParameters acsParameters; + const AcsParameters *acsParameters; + bool strBlindAvoidFlag = false; timeval timeSavedQuaternion; double savedQuaternion[4] = {0, 0, 0, 0}; From 26369039dada40cba14c824a606f8de523a3a159 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Feb 2023 17:36:33 +0100 Subject: [PATCH 10/38] better solution for actuatorCmd --- mission/controller/AcsController.cpp | 30 ++++++++++++++++++-------- mission/controller/acs/ActuatorCmd.cpp | 27 +++++++++-------------- mission/controller/acs/ActuatorCmd.h | 14 ++++++------ 3 files changed, 37 insertions(+), 34 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b056f093..d6d2fab6 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -170,7 +170,9 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && @@ -218,7 +220,9 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, &acsParameters.magnetorquerParameter); + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -300,7 +304,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -324,7 +329,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -345,7 +351,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -369,7 +376,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -392,7 +400,8 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, &acsParameters.rwHandlingParameters); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, + acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -409,8 +418,11 @@ void AcsController::performPointingCtrl() { actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, - cmdSpeedRws, &acsParameters); - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, &acsParameters.magnetorquerParameter); + cmdSpeedRws, acsParameters.onBoardParams.sampleTime, + acsParameters.rwHandlingParameters.inertiaWheel); + actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, + *acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipolMax); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 7173151d..cbe6b504 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -14,11 +14,7 @@ ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} -void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, - AcsParameters::RwHandlingParameters *rwHandlingParameters) { - // Scaling the commanded torque to a maximum value - double maxTrq = rwHandlingParameters->maxTrq; - +void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque) { double maxValue = 0; for (int i = 0; i < 4; i++) { // size of torque, always 4 ? if (abs(rwTrq[i]) > maxValue) { @@ -26,16 +22,15 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, } } - if (maxValue > maxTrq) { - double scalingFactor = maxTrq / maxValue; + if (maxValue > maxTorque) { + double scalingFactor = maxTorque / maxValue; VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); } } -void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, - const int32_t speedRw2, const int32_t speedRw3, - const double *rwTorque, int32_t *rwCmdSpeed, - AcsParameters *acsParameters) { +void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, + int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, + double sampleTime, double inertiaWheel) { using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel @@ -43,8 +38,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, double deltaSpeed[4] = {0, 0, 0, 0}; double radToRpm = 60 / (2 * PI); // factor for conversion to RPM // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = acsParameters->onBoardParams.sampleTime / - acsParameters->rwHandlingParameters.inertiaWheel * radToRpm; + double factor = sampleTime / inertiaWheel * radToRpm; int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); for (int i = 0; i < 4; i++) { @@ -55,13 +49,12 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, } void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - AcsParameters::MagnetorquerParameter *magnetorquerParameter) { + const double *inverseAlignment, double maxDipol) { // Convert to actuator frame double dipolMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(*magnetorquerParameter->inverseAlignment, dipolMoment, - dipolMomentActuatorDouble, 3, 3, 1); + MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, + 1); // Scaling along largest element if dipol exceeds maximum - double maxDipol = magnetorquerParameter->dipolMax; double maxValue = 0; for (int i = 0; i < 3; i++) { if (abs(dipolMomentActuator[i]) > maxDipol) { diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index ca0deb5b..def3c1b6 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,14 +1,13 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "AcsParameters.h" #include "MultiplicativeKalmanFilter.h" #include "SensorProcessing.h" #include "SensorValues.h" class ActuatorCmd { public: - ActuatorCmd(); // Input mode ? + ActuatorCmd(); virtual ~ActuatorCmd(); /* @@ -17,8 +16,7 @@ class ActuatorCmd { * @param: rwTrq given torque for reaction wheels * rwTrqScaled possible scaled torque */ - void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, - AcsParameters::RwHandlingParameters *rwHandlingParameters); + void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque); /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction @@ -29,9 +27,9 @@ class ActuatorCmd { * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, - const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, - AcsParameters *acsParameters); + void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, + const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, + double inertiaWheel); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -40,7 +38,7 @@ class ActuatorCmd { * dipolMomentActuator resulting dipol moment in actuator reference frame */ void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - AcsParameters::MagnetorquerParameter *magnetorquerParameter); + const double *inverseAlignment, double maxDipol); protected: private: From d86b9d55da217901badce30b199268ee9c14281b Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 28 Feb 2023 09:18:44 +0100 Subject: [PATCH 11/38] cleaned up SafeCtrl --- mission/controller/acs/control/SafeCtrl.cpp | 35 +++++++-------------- mission/controller/acs/control/SafeCtrl.h | 7 +---- 2 files changed, 12 insertions(+), 30 deletions(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index aa04cbb6..734c6a0c 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -1,10 +1,3 @@ -/* - * SafeCtrl.cpp - * - * Created on: 19 Apr 2022 - * Author: Robin Marquardt - */ - #include "SafeCtrl.h" #include @@ -15,19 +8,10 @@ #include "../util/MathOperations.h" -SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { - loadAcsParameters(acsParameters_); - MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, - 3); -} +SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } SafeCtrl::~SafeCtrl() {} -void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); - inertiaEIVE = &(acsParameters_->inertiaEIVE); -} - ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, @@ -39,8 +23,8 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, } double kRate = 0, kAlign = 0; - kRate = safeModeControllerParameters->k_rate_mekf; - kAlign = safeModeControllerParameters->k_align_mekf; + kRate = acsParameters->safeModeControllerParameters.k_rate_mekf; + kAlign = acsParameters->safeModeControllerParameters.k_align_mekf; // Calc sunDirB ,magFieldB with mekf output and model double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -73,6 +57,9 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); // Adding factor of inertia for axes + MatrixOperations::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10, + *gainMatrixInertia, 3, + 3); // why only for mekf one and not for no mekf MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); // MagMom B (orthogonal torque) @@ -129,7 +116,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl /* Only valid if angle between sun direction and magnetic field direction * is sufficiently large */ double angleSunMag = acos(cosAngleSunMag); - if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { + if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) { return; } @@ -139,8 +126,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl // Torque Align calculation double kRateNoMekf = 0, kAlignNoMekf = 0; - kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; - kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf; + kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf; double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); double crossSusSunRef[3] = {0, 0, 0}; @@ -159,8 +146,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl // Final torque double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, - 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate, + torqueB, 3, 3, 1); // Magnetic moment double magMomB[3] = {0, 0, 0}; diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 1784f9ca..f14bc464 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -17,8 +17,6 @@ class SafeCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE; static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, bool rateMekfValid, double *sunDirRef, @@ -30,12 +28,9 @@ class SafeCtrl { bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle, double *outputMagMomB, bool *outputValid); - void idleSunPointing(); // with reaction wheels - protected: private: - AcsParameters::SafeModeControllerParameters *safeModeControllerParameters; - AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters *acsParameters; double gainMatrixInertia[3][3]; double magFieldBState[3]; From 04a561b9a0a342cf88e9ece24b39c90b5a54989a Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 28 Feb 2023 10:12:25 +0100 Subject: [PATCH 12/38] cleaned up detumble and fixed gyr detumble law to actual bdot law --- mission/controller/AcsController.cpp | 4 +-- mission/controller/acs/control/Detumble.cpp | 28 +++++++++------------ mission/controller/acs/control/Detumble.h | 20 +++++---------- 3 files changed, 20 insertions(+), 32 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d6d2fab6..260b793a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -8,7 +8,6 @@ AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), guidance(&acsParameters), safeCtrl(&acsParameters), - detumble(&acsParameters), ptgCtrl(&acsParameters), parameterHelper(this), mgmDataRaw(this), @@ -219,7 +218,8 @@ void AcsController::performDetumble() { double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); + mgmDataProcessed.mgmVecTot.isValid(), magMomMtq, + acsParameters.detumbleParameter.gainD); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, *acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipolMax); diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 222a0ec2..28cbefbf 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -17,33 +17,27 @@ #include "../util/MathOperations.h" -Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } +Detumble::Detumble() {} Detumble::~Detumble() {} -void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { - detumbleParameter = &(acsParameters_->detumbleParameter); - magnetorquesParameter = &(acsParameters_->magnetorquerParameter); -} - ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, - const double *magField, const bool magFieldValid, double *magMom) { + const double *magField, const bool magFieldValid, double *magMom, + double gain) { if (!magRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleParameter->gainD; double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); VectorOperations::mulScalar(magRate, factor, magMom, 3); return returnvalue::OK; } -ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, - double *magMom) { +ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, + double dipolMax) { if (!magRateValid) { return DETUMBLE_NO_SENSORDATA; } - double dipolMax = magnetorquesParameter->dipolMax; for (int i = 0; i < 3; i++) { magMom[i] = -dipolMax * sign(magRate[i]); } @@ -51,14 +45,16 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal return returnvalue::OK; } -ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid, +ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, const bool *magFieldValid, - double *magMom) { + double *magMom, double gain) { if (!satRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleParameter->gainD; - double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); - VectorOperations::mulScalar(satRate, factor, magMom, 3); + double factor = gain / pow(VectorOperations::norm(magField, 3), 2); + double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0}; + VectorOperations::normalize(magField, magFieldNormed, 3); + VectorOperations::cross(satRate, magFieldNormed, crossProduct); + VectorOperations::mulScalar(crossProduct, factor, magMom, 3); return returnvalue::OK; } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index dee82271..2bf3607d 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -12,30 +12,22 @@ class Detumble { public: - Detumble(AcsParameters *acsParameters_); + Detumble(); virtual ~Detumble(); static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE; static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters for this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField, - const bool magFieldValid, double *magMom); + const bool magFieldValid, double *magMom, double gain); - ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); + ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, + double dipolMax); - ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); - - ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField, - const bool *magFieldValid, double *magMom); + ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, + const bool *magFieldValid, double *magMom, double gain); private: - AcsParameters::DetumbleParameter *detumbleParameter; - AcsParameters::MagnetorquerParameter *magnetorquesParameter; }; #endif /*ACS_CONTROL_DETUMBLE_H_*/ From 84643020da498b4499d0858b33fbb8ce6415e68f Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 28 Feb 2023 10:43:17 +0100 Subject: [PATCH 13/38] fixed gs target mode using wrong parameter set --- mission/controller/AcsController.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 260b793a..27f8eaf4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -344,10 +344,10 @@ void AcsController::performPointingCtrl() { susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, + ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); From 737d5ecb53b1fd02449cf5c0bc7bb85c4318c322 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 28 Feb 2023 10:47:37 +0100 Subject: [PATCH 14/38] cleaned up ptgCtrl --- mission/controller/acs/Guidance.cpp | 1 - mission/controller/acs/control/PtgCtrl.cpp | 52 +++++++++------------- mission/controller/acs/control/PtgCtrl.h | 11 +---- 3 files changed, 23 insertions(+), 41 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 9e9c04f9..cdfc6d11 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -204,7 +204,6 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double targetQuat[4], double targetSatRotRate[3]) { - sif::debug << acsParameters->gsTargetModeControllerParameters.altitudeTgt << std::endl; //------------------------------------------------------------------------------------- // Calculation of target quaternion for ground station pointing //------------------------------------------------------------------------------------- diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 74a32a4a..c7c8d2f0 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -1,10 +1,3 @@ -/* - * PtgCtrl.cpp - * - * Created on: 17 Jul 2022 - * Author: Robin Marquardt - */ - #include "PtgCtrl.h" #include @@ -16,16 +9,10 @@ #include "../util/MathOperations.h" -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::~PtgCtrl() {} -void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - rwHandlingParameters = &(acsParameters_->rwHandlingParameters); - rwMatrices = &(acsParameters_->rwMatrices); -} - void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { @@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixDiagonal[0][0] = gainVector[0]; gainMatrixDiagonal[1][1] = gainVector[1]; gainMatrixDiagonal[2][2] = gainVector[2]; - MatrixOperations::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), - *gainMatrix, 3, 3, 3); + MatrixOperations::multiply( + *gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3); // Inverse of gainMatrix double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -72,8 +59,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, - 3, 3); + MatrixOperations::multiply( + *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3); MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); //------------------------------------------------------------------------------------------------ @@ -98,7 +85,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters // Torque for rate error double torqueRate[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate, + torqueRate, 3, 3, 1); VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); @@ -123,11 +111,13 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP // calculating momentum of satellite and momentum of reaction wheels double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; - VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); - MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, - 1); + VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, + momentumRwU, 4); + MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU, + momentumRw, 3, 4, 1); double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate, + momentumSat, 3, 3, 1); VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; @@ -152,11 +142,12 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); double diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, + VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, wheelMomentum, 4); double gainNs = pointingLawParameters->gainNullspace; double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, + MathOperations::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, + acsParameters->rwMatrices.nullspace, *nullSpaceMatrix, 4); MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); @@ -174,21 +165,22 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueComm for (uint8_t i = 0; i < 4; i++) { if (rwAvailable[i]) { if (torqueMemory[i] != 0) { - if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { + if ((omegaRw[i] * torqueMemory[i]) > + acsParameters->rwHandlingParameters.stictionReleaseSpeed) { torqueMemory[i] = 0; } else { - torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque; } } else { - if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && - (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { + if ((omegaRw[i] < acsParameters->rwHandlingParameters.stictionSpeed) && + (omegaRw[i] > -acsParameters->rwHandlingParameters.stictionSpeed)) { if (omegaRw[i] < omegaMemory[i]) { torqueMemory[i] = -1; } else { torqueMemory[i] = 1; } - torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque; } } } else { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fb27fc6d..87185612 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -33,13 +33,7 @@ class PtgCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG; static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters for this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); - /* @brief: Calculates the needed torque for the pointing control mechanism - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); @@ -54,15 +48,12 @@ class PtgCtrl { const int32_t *speedRw3, double *rwTrqNs); /* @brief: Commands the stiction torque in case wheel speed is to low - * @param: sensorValues class containing all RW related values * torqueCommand modified torque after antistiction */ void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand); private: - AcsParameters::RwHandlingParameters *rwHandlingParameters; - AcsParameters::InertiaEIVE *inertiaEIVE; - AcsParameters::RwMatrices *rwMatrices; + const AcsParameters *acsParameters; double torqueMemory[4] = {0, 0, 0, 0}; double omegaMemory[4] = {0, 0, 0, 0}; From d59daa24857a517e4ff0e72a4828db2b558f6943 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 28 Feb 2023 15:00:05 +0100 Subject: [PATCH 15/38] we might need this pointing law stuff ... --- mission/controller/AcsController.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 27f8eaf4..5b3eae4a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -299,6 +299,8 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, + *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace( &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), From 7b0657bb55a43389c735a228fa76c0af2e66c126 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 28 Feb 2023 15:01:07 +0100 Subject: [PATCH 16/38] fixed usage of wrong parameters --- mission/controller/AcsController.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 5b3eae4a..feb8fb6e 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -356,11 +356,11 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, + &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; case acs::PTG_NADIR: From 2c6ea8dc9f641c0bcd707826d9e6debe460fc1b0 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 28 Feb 2023 15:46:52 +0100 Subject: [PATCH 17/38] fixed wrong if condition --- mission/controller/acs/Guidance.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index cdfc6d11..ed14cc48 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -540,8 +540,8 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, } void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { - if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or - not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore + if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) and + not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { From ba0e07b5c66d47ece241d6d4b61f80244cd671ab Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 10:04:04 +0100 Subject: [PATCH 18/38] merge aftermath --- mission/controller/acs/control/SafeCtrl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index d56330bb..392e32ba 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -122,8 +122,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); // Torque Align calculation - double kRateNoMekf = acsParameters->safeModeControllerParameters->k_rate_no_mekf; - double kAlignNoMekf = acsParameters->safeModeControllerParameters->k_align_no_mekf; + double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf; + double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf; double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); double crossSusSunRef[3] = {0, 0, 0}; From faf4937e7648976385cdfc450d24f20bdf5314d7 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 10:11:12 +0100 Subject: [PATCH 19/38] fixed detumbleCtrl magField and magRate units --- mission/controller/acs/control/Detumble.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 5e69fdcc..893d6a4b 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -19,8 +19,13 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, if (!magRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); - VectorOperations::mulScalar(magRate, factor, magMom, 3); + // convert uT to T + double magFieldT[3], magRateT[3]; + VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); + VectorOperations::mulScalar(magRate, 1e-6, magRateT, 3); + // control law + double factor = -gain / pow(VectorOperations::norm(magFieldT, 3), 2); + VectorOperations::mulScalar(magRateT, factor, magMom, 3); return returnvalue::OK; } @@ -43,9 +48,13 @@ ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateVa if (!satRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } + // convert uT to T + double magFieldT[3]; + VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); + // control law double factor = gain / pow(VectorOperations::norm(magField, 3), 2); double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0}; - VectorOperations::normalize(magField, magFieldNormed, 3); + VectorOperations::normalize(magFieldT, magFieldNormed, 3); VectorOperations::cross(satRate, magFieldNormed, crossProduct); VectorOperations::mulScalar(crossProduct, factor, magMom, 3); return returnvalue::OK; From bd295d5b520cbafbc8d3a88ae6ec7ec4dd1e2c0c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 10:12:52 +0100 Subject: [PATCH 20/38] detumbleCounter doesnt hard reset anymore --- mission/controller/AcsController.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 3bf89d77..45ad4d91 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -185,8 +185,8 @@ void AcsController::performSafe() { VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; - } else { - detumbleCounter = 0; + } else if (detumbleCounter > 0) { + detumbleCounter -= 1; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; @@ -235,8 +235,8 @@ void AcsController::performDetumble() { VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; - } else { - detumbleCounter = 0; + } else if (detumbleCounter > 0) { + detumbleCounter -= 1; } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; From d8a07312f2199e7330c5e9cb88eac16b264d60e4 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 10:36:00 +0100 Subject: [PATCH 21/38] changelog --- CHANGELOG.md | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2e3c5dc4..4ef271de 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,25 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- `AcsParameters` setter were previously all for scalar parameters. Now vector and matrix + parameters use their respective setters. +- Several `AcsController` components had their own implementation of `AcsParameters`. This resulted + in those parameters not being updated, while the actual ones were updated. All instances of + `AcsParameters` not belonging to `AcsController` are eiter removed or replaced by pointer + instances. +- Instead of updating the `gsTargetModeControllerParameters`, the `targetModeControllerParameters` + were updated. +- Fixed Idle Mode Controller never calling `ptgLaw` and therefore never calculating control + values. +- Fixed wrong check on wether file used for persistant boolean flag on successful still existed. + +## Changed + +- The `detumbleCounter` now does not get hard reset anymore, if the critical rate does not get + violated anymore. Instead it is incrementally reset. + # [v1.35.1] 2023-03-04 ## Fixed @@ -100,12 +119,6 @@ eive-tmtc: v2.16.2 - Linux GPS handler now checks the individual `*_SET` flags when analysing the `gpsd` struct. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/400 -- Several `AcsController` components had their own implementation of `AcsParameters`. This resulted - in those parameters not being updated, while the actual ones were updated. All instances of - `AcsParameters` not belonging to `AcsController` are eiter removed or replaced by pointer - instances. -- Instead of updating the `gsTargetModeControllerParameters`, the `targetModeControllerParameters` - were updated. # [v1.32.0] 2023-02-24 From fd436dbe8b3b32ae7f1d0d82fe0493a9b96b23fb Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 11:10:47 +0100 Subject: [PATCH 22/38] added fdir for unrealistic gps altitudes --- mission/controller/AcsController.cpp | 1 + mission/controller/AcsController.h | 1 + mission/controller/acs/AcsParameters.cpp | 9 +++++++++ mission/controller/acs/AcsParameters.h | 7 +++++-- mission/controller/acs/SensorProcessing.cpp | 15 ++++++++++++--- mission/controller/acs/SensorValues.h | 11 +++++------ .../controllerdefinitions/AcsCtrlDefinitions.h | 2 ++ 7 files changed, 35 insertions(+), 11 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 45ad4d91..c3149c38 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -588,6 +588,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD // GPS Processed localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index f59a4605..ae61e2a2 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -189,6 +189,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes acsctrl::GpsDataProcessed gpsDataProcessed; PoolEntry gcLatitude = PoolEntry(); PoolEntry gdLongitude = PoolEntry(); + PoolEntry altitude = PoolEntry(); PoolEntry gpsPosition = PoolEntry(3); PoolEntry gpsVelocity = PoolEntry(3); diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index a19571eb..0d3785cc 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -591,6 +591,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x0: parameterWrapper->set(gpsParameters.timeDiffVelocityMax); break; + case 0x1: + parameterWrapper->set(gpsParameters.minimumFdirAltitude); + break; + case 0x2: + parameterWrapper->set(gpsParameters.maximumFdirAltitude); + break; + case 0x3: + parameterWrapper->set(gpsParameters.fdirAltitude); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index ea867c65..4d6aa14a 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -772,7 +772,7 @@ class AcsParameters : public HasParametersIF { double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594}; double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845}; - /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is + /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is * assumed to be equal for the same class of sensors */ float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms @@ -886,7 +886,10 @@ class AcsParameters : public HasParametersIF { } strParameters; struct GpsParameters { - double timeDiffVelocityMax = 30; //[s] + double timeDiffVelocityMax = 30; // [s] + double minimumFdirAltitude = 475 * 1e3; // [m] + double maximumFdirAltitude = 575 * 1e3; // [m] + double fdirAltitude = 525 * 1e3; // [m] } gpsParameters; struct SunModelParameters { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 466d9ed5..b3423367 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -550,8 +550,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong const bool validGps, const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { - // name to convert not process - double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; + double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, + gpsVelocityE[3] = {0, 0, 0}; if (validGps) { // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic gdLongitude = gpsLongitude * PI / 180.; @@ -560,9 +560,17 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong double factor = 1 - pow(eccentricityWgs84, 2); gcLatitude = atan(factor * tan(latitudeRad)); + // Altitude FDIR + if (gpsAltitude > gpsParameters->maximumFdirAltitude || + gpsAltitude < gpsParameters->maximumFdirAltitude) { + altitude = gpsParameters->fdirAltitude; + } else { + altitude = gpsAltitude; + } + // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE); + MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); if (validSavedPosSatE && (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); @@ -581,6 +589,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong if (pg.getReadResult() == returnvalue::OK) { gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gcLatitude.value = gcLatitude; + gpsDataProcessed->altitude.value = altitude; std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double)); gpsDataProcessed->setValidity(validGps, true); diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 79c3dfe1..25946d0b 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,17 +1,16 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#include +#include +#include +#include +#include #include #include #include #include -#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" -#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" -#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" -#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" -#include "mission/devices/devicedefinitions/GPSDefinitions.h" - namespace ACS { class SensorValues { diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c3509a04..deb78152 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -86,6 +86,7 @@ enum PoolIds : lp_id_t { // GPS Processed GC_LATITUDE, GD_LONGITUDE, + ALTITUDE, GPS_POSITION, GPS_VELOCITY, // MEKF @@ -228,6 +229,7 @@ class GpsDataProcessed : public StaticLocalDataSet { lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this); + lp_var_t altitude = lp_var_t(sid.objectId, ALTITUDE, this); lp_vec_t gpsPosition = lp_vec_t(sid.objectId, GPS_POSITION, this); lp_vec_t gpsVelocity = lp_vec_t(sid.objectId, GPS_VELOCITY, this); From 82966eed319c34370bf45de3e947ec4b9af97a8f Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 11:13:00 +0100 Subject: [PATCH 23/38] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4ef271de..cc73f13b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Added +- `SensorProcessing` now includes an FDIR for GPS altitude. If the measured GPS altitude is out + of bounds of the range defined in the `AcsParameters`, the altitude defaults to an altitude + set in the `AcsParameters`. + ## Fixed - `AcsParameters` setter were previously all for scalar parameters. Now vector and matrix From 903c0c82580b73e53d753b2c68b7d79d9fcc792c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 11:25:06 +0100 Subject: [PATCH 24/38] fixed gpsDataProcessed dataSet size --- mission/controller/controllerdefinitions/AcsCtrlDefinitions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index deb78152..f82e75f1 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -110,7 +110,7 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; -static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4; +static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; From fd405e61a4ee9d762e382fee709d64db17e73176 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 13:53:46 +0100 Subject: [PATCH 25/38] fixed scaling of mtq dipole commands --- mission/controller/acs/ActuatorCmd.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index cbe6b504..37212113 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -55,14 +55,11 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentAct MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); // Scaling along largest element if dipol exceeds maximum - double maxValue = 0; - for (int i = 0; i < 3; i++) { - if (abs(dipolMomentActuator[i]) > maxDipol) { - maxValue = abs(dipolMomentActuator[i]); - } - } - if (maxValue > maxDipol) { - double scalingFactor = maxDipol / maxValue; + uint8_t maxIdx = 0; + VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); + double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); + if (maxAbsValue > maxDipol) { + double scalingFactor = maxDipol / maxAbsValue; VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, dipolMomentActuatorDouble, 3); } From 1cc39bb6d7729ad0d75a5ca2aeed32f9cd4c454d Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 13:57:45 +0100 Subject: [PATCH 26/38] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index cc73f13b..f9275219 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -34,6 +34,8 @@ will consitute of a breaking change warranting a new major release: - Fixed Idle Mode Controller never calling `ptgLaw` and therefore never calculating control values. - Fixed wrong check on wether file used for persistant boolean flag on successful still existed. +- Scaling of MTQ Cmds now scales the current values to command with the current values and not + the values of the last step, which would result in undefined behaviour. ## Changed From 72ab4b1a24c794cf334a98dacd4a79066ad69424 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 14:07:37 +0100 Subject: [PATCH 27/38] fixed naming collision with file for solar deployment --- mission/controller/AcsController.cpp | 2 +- mission/controller/acs/Guidance.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c3149c38..439fcc68 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -196,7 +196,7 @@ void AcsController::performSafe() { updateCtrlValData(errAng); updateActuatorCmdData(cmdDipolMtqs); - // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/, + // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.rwHandlingParameters.rampTime); } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index c7f465f8..f3369092 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -62,8 +62,8 @@ class Guidance { double savedQuaternion[4] = {0, 0, 0, 0}; double omegaRefSaved[3] = {0, 0, 0}; - static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment"; - static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment"; + static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm"; + static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm"; }; #endif /* ACS_GUIDANCE_H_ */ From 5b0db43e0fab8c628decc2d8ca34351d0a05df99 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Mar 2023 14:15:33 +0100 Subject: [PATCH 28/38] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index f9275219..0f103ae4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -36,6 +36,8 @@ will consitute of a breaking change warranting a new major release: - Fixed wrong check on wether file used for persistant boolean flag on successful still existed. - Scaling of MTQ Cmds now scales the current values to command with the current values and not the values of the last step, which would result in undefined behaviour. +- Solved naming collision between file used for solar array deployment and confirmation for + ACS for solar array deployment. ## Changed From f3c3e8a4430d7becefaba9872b2d0eb3ddfc3036 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 09:17:19 +0100 Subject: [PATCH 29/38] corrected vector length to 1 --- mission/controller/acs/AcsParameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 4d6aa14a..8dc6e1ee 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -817,7 +817,7 @@ class AcsParameters : public HasParametersIF { double sunMagAngleMin = 5 * M_PI / 180; - double sunTargetDirLeop[3] = {0, .5, .5}; + double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)}; double sunTargetDir[3] = {0, 0, 1}; double satRateRef[3] = {0, 0, 0}; From 0dbd6b703d4aae29c8279aa96c1872e697c41d43 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 09:44:30 +0100 Subject: [PATCH 30/38] small fixes --- mission/controller/acs/ActuatorCmd.cpp | 2 +- mission/controller/acs/Guidance.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 37212113..588ee82a 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -16,7 +16,7 @@ ActuatorCmd::~ActuatorCmd() {} void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque) { double maxValue = 0; - for (int i = 0; i < 4; i++) { // size of torque, always 4 ? + for (int i = 0; i < 4; i++) { if (abs(rwTrq[i]) > maxValue) { maxValue = abs(rwTrq[i]); } diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 71cb227e..ca312876 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -544,7 +544,7 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { // ToDo: if file does not exist anymore - std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, + std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop, From e4efd422349e170323a7e8e46bb10e20d586a2cb Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 10:28:31 +0100 Subject: [PATCH 31/38] fixed idle mode param commands changing target mode params --- mission/controller/acs/AcsParameters.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 0d3785cc..f39ea601 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -345,31 +345,31 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x9): // IdleModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(targetModeControllerParameters.zeta); + parameterWrapper->set(idleModeControllerParameters.zeta); break; case 0x1: - parameterWrapper->set(targetModeControllerParameters.om); + parameterWrapper->set(idleModeControllerParameters.om); break; case 0x2: - parameterWrapper->set(targetModeControllerParameters.omMax); + parameterWrapper->set(idleModeControllerParameters.omMax); break; case 0x3: - parameterWrapper->set(targetModeControllerParameters.qiMin); + parameterWrapper->set(idleModeControllerParameters.qiMin); break; case 0x4: - parameterWrapper->set(targetModeControllerParameters.gainNullspace); + parameterWrapper->set(idleModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(idleModeControllerParameters.desatOn); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; default: From 75a80b55ec4f0fc763bd2159913292f5296f443b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 10:29:27 +0100 Subject: [PATCH 32/38] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 16c371e5..1dc9062e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,6 +32,8 @@ will consitute of a breaking change warranting a new major release: instances. - Instead of updating the `gsTargetModeControllerParameters`, the `targetModeControllerParameters` were updated. +- Instead of updating the `idleModeControllerParameters`, the `targetModeControllerParameters` + were updated. - Fixed Idle Mode Controller never calling `ptgLaw` and therefore never calculating control values. - Fixed wrong check on wether file used for persistant boolean flag on successful still existed. From 9a30ae5175715b93be7f436d073515d101369d0b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 10:30:01 +0100 Subject: [PATCH 33/38] who knows what that todo was good for --- mission/controller/acs/Guidance.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index ca312876..35a2f025 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -542,8 +542,7 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { std::error_code e; if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or - not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, - e)) { // ToDo: if file does not exist anymore + not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); } else { From cb8a49775d06460a76485023bdac687fface0cdb Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 11:37:04 +0100 Subject: [PATCH 34/38] added protection to not command rwSpeeds larger than the allowed speed range --- mission/controller/AcsController.cpp | 4 ++-- mission/controller/acs/AcsParameters.cpp | 9 ++++++--- mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/ActuatorCmd.cpp | 9 ++++++++- mission/controller/acs/ActuatorCmd.h | 2 +- 5 files changed, 18 insertions(+), 7 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f827950a..c7bbe0aa 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -416,7 +416,6 @@ void AcsController::performPointingCtrl() { enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; } - if (enableAntiStiction) { ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); } @@ -425,13 +424,14 @@ void AcsController::performPointingCtrl() { sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws, acsParameters.onBoardParams.sampleTime, + acsParameters.rwHandlingParameters.maxRwSpeed, acsParameters.rwHandlingParameters.inertiaWheel); actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, *acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipolMax); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); - updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs); + updateActuatorCmdData(torqueRwsScaled, cmdSpeedRws, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index f39ea601..4abe0d7a 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -270,15 +270,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(rwHandlingParameters.maxTrq); break; case 0x2: - parameterWrapper->set(rwHandlingParameters.stictionSpeed); + parameterWrapper->set(rwHandlingParameters.maxRwSpeed); break; case 0x3: - parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); + parameterWrapper->set(rwHandlingParameters.stictionSpeed); break; case 0x4: - parameterWrapper->set(rwHandlingParameters.stictionTorque); + parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); break; case 0x5: + parameterWrapper->set(rwHandlingParameters.stictionTorque); + break; + case 0x6: parameterWrapper->set(rwHandlingParameters.rampTime); break; default: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 8dc6e1ee..98cbd9ad 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -784,6 +784,7 @@ class AcsParameters : public HasParametersIF { struct RwHandlingParameters { double inertiaWheel = 0.000028198; double maxTrq = 0.0032; // 3.2 [mNm] + int32_t maxRwSpeed = 65000; // 0.1 RPM int32_t stictionSpeed = 100; // RPM int32_t stictionReleaseSpeed = 120; // RPM double stictionTorque = 0.0006; diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 588ee82a..592fc69d 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -30,7 +30,7 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, dou void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, - double sampleTime, double inertiaWheel) { + double sampleTime, int32_t maxRwSpeed, double inertiaWheel) { using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel @@ -45,6 +45,13 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee deltaSpeedInt[i] = std::round(deltaSpeed[i]); } VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); + for (uint8_t i = 0; i < 4; i++) { + if (rwCmdSpeed[i] > maxRwSpeed) { + rwCmdSpeed[i] = maxRwSpeed; + } else if (rwCmdSpeed[i] < -maxRwSpeed) { + rwCmdSpeed[i] = -maxRwSpeed; + } + } VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index def3c1b6..269e0191 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -29,7 +29,7 @@ class ActuatorCmd { */ void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, - double inertiaWheel); + int32_t maxRwSpeed, double inertiaWheel); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques From 555e0ce49da693b3c0eccfd4ee4fabf2a427c478 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 11:39:31 +0100 Subject: [PATCH 35/38] changelog --- CHANGELOG.md | 1 + mission/controller/acs/control/PtgCtrl.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1dc9062e..a6d45d74 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ will consitute of a breaking change warranting a new major release: - `SensorProcessing` now includes an FDIR for GPS altitude. If the measured GPS altitude is out of bounds of the range defined in the `AcsParameters`, the altitude defaults to an altitude set in the `AcsParameters`. +- `AcsController` will now never command a RW speed larger than the maximum allowed speed. ## Fixed diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index c7c8d2f0..01c047db 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -78,19 +78,19 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters pErrorSign[i] = pError[i]; } } - // Torque for quaternion error + // torque for quaternion error double torqueQuat[3] = {0, 0, 0}; MatrixOperations::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); - // Torque for rate error + // torque for rate error double torqueRate[3] = {0, 0, 0}; MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate, torqueRate, 3, 3, 1); VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); - // Final commanded Torque for every reaction wheel + // final commanded Torque for every reaction wheel double torque[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueQuat, torque, 3); MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); @@ -108,7 +108,7 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP return; } - // calculating momentum of satellite and momentum of reaction wheels + // calculating momentum of satellite and momentum of reaction wheels double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, @@ -119,11 +119,11 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate, momentumSat, 3, 3, 1); VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); - // calculating momentum error + // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3); - // resulting magnetic dipole command + // resulting magnetic dipole command double crossMomentumMagField[3] = {0, 0, 0}; VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; @@ -137,7 +137,7 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; - // Conversion to [rad/s] for further calculations + // conversion to [rad/s] for further calculations VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); double diffRwSpeed[4] = {0, 0, 0, 0}; From bce8df376a9d314734d1968f90e1aa748def3884 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 17:21:52 +0100 Subject: [PATCH 36/38] changed antistiction --- CHANGELOG.md | 1 + mission/controller/acs/AcsParameters.h | 6 ++-- mission/controller/acs/control/PtgCtrl.cpp | 39 ++++++++++------------ mission/controller/acs/control/PtgCtrl.h | 25 +++++--------- 4 files changed, 29 insertions(+), 42 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a6d45d74..3ee2279b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -47,6 +47,7 @@ will consitute of a breaking change warranting a new major release: - The `detumbleCounter` now does not get hard reset anymore, if the critical rate does not get violated anymore. Instead it is incrementally reset. +- The RW antistiction now only takes the RW speeds in account. # [v1.36.0] 2023-03-08 diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 98cbd9ad..822cfabc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -784,9 +784,9 @@ class AcsParameters : public HasParametersIF { struct RwHandlingParameters { double inertiaWheel = 0.000028198; double maxTrq = 0.0032; // 3.2 [mNm] - int32_t maxRwSpeed = 65000; // 0.1 RPM - int32_t stictionSpeed = 100; // RPM - int32_t stictionReleaseSpeed = 120; // RPM + int32_t maxRwSpeed = 65000; // 0.1 RPM + int32_t stictionSpeed = 1000; // 0.1 RPM + int32_t stictionReleaseSpeed = 1000; // 0.1 RPM double stictionTorque = 0.0006; uint16_t rampTime = 10; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 01c047db..23f51f9e 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -154,38 +154,33 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } -void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) { +void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { bool rwAvailable[4] = { (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()), (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()), (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()), (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())}; - int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value, - sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value}; + int32_t currRwSpeed[4] = { + sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value, + sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value}; for (uint8_t i = 0; i < 4; i++) { if (rwAvailable[i]) { - if (torqueMemory[i] != 0) { - if ((omegaRw[i] * torqueMemory[i]) > - acsParameters->rwHandlingParameters.stictionReleaseSpeed) { - torqueMemory[i] = 0; - } else { - torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque; - } - } else { - if ((omegaRw[i] < acsParameters->rwHandlingParameters.stictionSpeed) && - (omegaRw[i] > -acsParameters->rwHandlingParameters.stictionSpeed)) { - if (omegaRw[i] < omegaMemory[i]) { - torqueMemory[i] = -1; - } else { - torqueMemory[i] = 1; + if (rwCmdSpeeds[i] != 0) { + if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed && + rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) { + if (currRwSpeed[i] == 0) { + if (rwCmdSpeeds[i] > 0) { + rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; + } else if (rwCmdSpeeds[i] < 0) { + rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; + } + } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) { + rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; + } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { + rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; } - - torqueCommand[i] = torqueMemory[i] * acsParameters->rwHandlingParameters.stictionTorque; } } - } else { - torqueMemory[i] = 0; } - omegaMemory[i] = omegaRw[i]; } } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 87185612..f6c6c345 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,16 +1,3 @@ -/* - * PtgCtrl.h - * - * Created on: 17 Jul 2022 - * Author: Robin Marquardt - * - * @brief: This class handles the pointing control mechanism. Calculation of an commanded - * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation - * - * @note: A description of the used algorithms can be found in - * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 - */ - #ifndef PTGCTRL_H_ #define PTGCTRL_H_ @@ -23,6 +10,13 @@ #include "eive/resultClassIds.h" class PtgCtrl { + /* + * @brief: This class handles the pointing control mechanism. Calculation of an commanded + * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * + * @note: A description of the used algorithms can be found in + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 + */ public: /* @brief: Constructor * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters @@ -50,13 +44,10 @@ class PtgCtrl { /* @brief: Commands the stiction torque in case wheel speed is to low * torqueCommand modified torque after antistiction */ - void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand); + void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed); private: const AcsParameters *acsParameters; - - double torqueMemory[4] = {0, 0, 0, 0}; - double omegaMemory[4] = {0, 0, 0, 0}; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ From 342ff62837c148abbc8b80d34b4caa9342bd0bc8 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 17:31:12 +0100 Subject: [PATCH 37/38] fixed rw torque scaling --- CHANGELOG.md | 1 + mission/controller/AcsController.cpp | 31 +++++++++++--------------- mission/controller/acs/ActuatorCmd.cpp | 13 +++++------ mission/controller/acs/ActuatorCmd.h | 6 ++--- 4 files changed, 22 insertions(+), 29 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3ee2279b..5899b136 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -42,6 +42,7 @@ will consitute of a breaking change warranting a new major release: the values of the last step, which would result in undefined behaviour. - Solved naming collision between file used for solar array deployment and confirmation for ACS for solar array deployment. +- Fixed that scaling of RW torque would result in a zero vector unless the maximum value was exceeded. ## Changed diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c7bbe0aa..fa0c8898 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -291,13 +291,13 @@ void AcsController::performPointingCtrl() { } else { multipleRwUnavailableCounter = 0; } - double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; - double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - double mgtDpDes[3] = {0, 0, 0}; // Variables required for guidance double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, errorAngle = 0, errorSatRotRate[3] = {0, 0, 0}; + // Variables required for setting actuators + double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0}, + mgtDpDes[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); @@ -310,8 +310,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, - acsParameters.rwHandlingParameters.maxTrq); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -335,8 +334,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, - acsParameters.rwHandlingParameters.maxTrq); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -357,8 +355,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, - acsParameters.rwHandlingParameters.maxTrq); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -382,8 +379,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, - acsParameters.rwHandlingParameters.maxTrq); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -406,8 +402,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled, - acsParameters.rwHandlingParameters.maxTrq); + actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, @@ -416,22 +411,22 @@ void AcsController::performPointingCtrl() { enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; } - if (enableAntiStiction) { - ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled); - } actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, cmdSpeedRws, acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.maxRwSpeed, acsParameters.rwHandlingParameters.inertiaWheel); + if (enableAntiStiction) { + ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); + } actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, *acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipolMax); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); - updateActuatorCmdData(torqueRwsScaled, cmdSpeedRws, cmdDipolMtqs); + updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 592fc69d..f32ce241 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -14,17 +14,14 @@ ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} -void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque) { - double maxValue = 0; - for (int i = 0; i < 4; i++) { - if (abs(rwTrq[i]) > maxValue) { - maxValue = abs(rwTrq[i]); - } - } +void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) { + uint8_t maxIdx = 0; + VectorOperations::maxAbsValue(rwTrq, 4, &maxIdx); + double maxValue = rwTrq[maxIdx]; if (maxValue > maxTorque) { double scalingFactor = maxTorque / maxValue; - VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); + VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrq, 4); } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 269e0191..5d5d47f3 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -13,10 +13,10 @@ class ActuatorCmd { /* * @brief: scalingTorqueRws() scales the torque via maximum part in case this part is * higher then the maximum torque - * @param: rwTrq given torque for reaction wheels - * rwTrqScaled possible scaled torque + * @param: rwTrq given torque for reaction wheels which will be + * scaled if needed to be */ - void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled, double maxTorque); + void scalingTorqueRws(double *rwTrq, double maxTorque); /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction From b6d18ef4d2f6ae598dccb247f17c622d3ca289ed Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Mar 2023 17:41:18 +0100 Subject: [PATCH 38/38] changelog fix --- CHANGELOG.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 82e981ea..79939ff0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -49,9 +49,6 @@ will consitute of a breaking change warranting a new major release: - The `detumbleCounter` now does not get hard reset anymore, if the critical rate does not get violated anymore. Instead it is incrementally reset. - The RW antistiction now only takes the RW speeds in account. - -## Changed - - ACS CTRL transition to DETUBMLE is now done in CTRL internally. No system level handling necessary anymore. - More fixes and improvements for SD card handling. Extend SD card setup in core controller to