From 8f61d14a828fb897f0db06d21861b06978438d9b Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 14 Oct 2022 11:11:23 +0200 Subject: [PATCH] implemented getParameter for AcsParameters --- mission/controller/acs/AcsParameters.cpp | 536 ++++++++++++++++++++++- 1 file changed, 530 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 0b516c13..ca1057bc 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -3,15 +3,539 @@ #include #include -#include - -AcsParameters::AcsParameters() {} +AcsParameters::AcsParameters(){}; //(uint8_t parameterModuleId) : + // parameterModuleId(parameterModuleId) {} AcsParameters::~AcsParameters() {} -/* -ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, + +/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex) { - return returnvalue::OK; + if (domainId == parameterModuleId) { + switch (parameterId >> 8) { + case 0x0: // direct members + switch (parameterId & 0xFF) { + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x1: // OnBoardParams + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(onBoardParams.sampleTime); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x2: // InertiaEIVE + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(inertiaEIVE.inertiaMatrix); + break; + case 0x1: + parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x3: // MgmHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); + break; + case 0x5: + parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); + break; + case 0x6: + parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); + break; + case 0x7: + parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); + break; + case 0x8: + parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); + break; + case 0x9: + parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); + break; + case 0xA: + parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); + break; + case 0xB: + parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); + break; + case 0xC: + parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); + break; + case 0xD: + parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); + break; + case 0xE: + parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x4: // SusHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); + break; + case 0x5: + parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); + break; + case 0x6: + parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); + break; + case 0x7: + parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); + break; + case 0x8: + parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); + break; + case 0x9: + parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); + break; + case 0xA: + parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); + break; + case 0xB: + parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); + break; + case 0xC: + parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); + break; + case 0xD: + parameterWrapper->set(susHandlingParameters.sus0coeffBeta); + break; + case 0xE: + parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); + break; + case 0xF: + parameterWrapper->set(susHandlingParameters.sus1coeffBeta); + break; + case 0x10: + parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); + break; + case 0x11: + parameterWrapper->set(susHandlingParameters.sus2coeffBeta); + break; + case 0x12: + parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); + break; + case 0x13: + parameterWrapper->set(susHandlingParameters.sus3coeffBeta); + break; + case 0x14: + parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); + break; + case 0x15: + parameterWrapper->set(susHandlingParameters.sus4coeffBeta); + break; + case 0x16: + parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); + break; + case 0x17: + parameterWrapper->set(susHandlingParameters.sus5coeffBeta); + break; + case 0x18: + parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); + break; + case 0x19: + parameterWrapper->set(susHandlingParameters.sus6coeffBeta); + break; + case 0x1A: + parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); + break; + case 0x1B: + parameterWrapper->set(susHandlingParameters.sus7coeffBeta); + break; + case 0x1C: + parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); + break; + case 0x1D: + parameterWrapper->set(susHandlingParameters.sus8coeffBeta); + break; + case 0x1E: + parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); + break; + case 0x1F: + parameterWrapper->set(susHandlingParameters.sus9coeffBeta); + break; + case 0x20: + parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); + break; + case 0x21: + parameterWrapper->set(susHandlingParameters.sus10coeffBeta); + break; + case 0x22: + parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); + break; + case 0x23: + parameterWrapper->set(susHandlingParameters.sus11coeffBeta); + break; + case 0x24: + parameterWrapper->set(susHandlingParameters.filterAlpha); + break; + case 0x25: + parameterWrapper->set(susHandlingParameters.sunThresh); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x5): // GyrHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x6): // RwHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(rwHandlingParameters.inertiaWheel); + break; + case 0x5: + parameterWrapper->set(rwHandlingParameters.maxTrq); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x7): // RwMatrices + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(rwMatrices.alignmentMatrix); + break; + case 0x1: + parameterWrapper->set(rwMatrices.pseudoInverse); + break; + case 0x2: + parameterWrapper->set(rwMatrices.without0); + break; + case 0x3: + parameterWrapper->set(rwMatrices.without1); + break; + case 0x4: + parameterWrapper->set(rwMatrices.without2); + break; + case 0x5: + parameterWrapper->set(rwMatrices.without3); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x8): // SafeModeControllerParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(safeModeControllerParameters.k_rate_mekf); + break; + case 0x1: + parameterWrapper->set(safeModeControllerParameters.k_align_mekf); + break; + case 0x2: + parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf); + break; + case 0x3: + parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf); + break; + case 0x4: + parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); + break; + case 0x5: + parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + break; + case 0x6: + parameterWrapper->set(safeModeControllerParameters.satRateRef); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x9): // DetumbleCtrlParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(detumbleCtrlParameters.gainD); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xA): // PointingModeControllerParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(targetModeControllerParameters.updtFlag); + break; + case 0x1: + parameterWrapper->set(targetModeControllerParameters.A_rw); + break; + case 0x2: + parameterWrapper->set(targetModeControllerParameters.refDirection); + break; + case 0x3: + parameterWrapper->set(targetModeControllerParameters.refRotRate); + break; + case 0x4: + parameterWrapper->set(targetModeControllerParameters.quatRef); + break; + case 0x5: + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + break; + case 0x6: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + break; + case 0x7: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.blindRotRate); + break; + case 0x9: + parameterWrapper->set(targetModeControllerParameters.zeta); + break; + case 0xA: + parameterWrapper->set(targetModeControllerParameters.zetaLow); + break; + case 0xB: + parameterWrapper->set(targetModeControllerParameters.om); + break; + case 0xC: + parameterWrapper->set(targetModeControllerParameters.omLow); + break; + case 0xD: + parameterWrapper->set(targetModeControllerParameters.omMax); + break; + case 0xE: + parameterWrapper->set(targetModeControllerParameters.qiMin); + break; + case 0xF: + parameterWrapper->set(targetModeControllerParameters.gainNullspace); + break; + case 0x10: + parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + break; + case 0x11: + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + break; + case 0x12: + parameterWrapper->set(targetModeControllerParameters.desatOn); + break; + case 0x13: + parameterWrapper->set(targetModeControllerParameters.omegaEarth); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xB): // StrParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(strParameters.exclusionAngle); + break; + case 0x1: + parameterWrapper->set(strParameters.boresightAxis); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xC): // GpsParameters + switch (parameterId & 0xFF) { + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xD): // GroundStationParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(groundStationParameters.latitudeGs); + break; + case 0x1: + parameterWrapper->set(groundStationParameters.longitudeGs); + break; + case 0x2: + parameterWrapper->set(groundStationParameters.altitudeGs); + break; + case 0x3: + parameterWrapper->set(groundStationParameters.earthRadiusEquat); + break; + case 0x4: + parameterWrapper->set(groundStationParameters.earthRadiusPolar); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xE): // SunModelParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(sunModelParameters.useSunModel); + break; + case 0x1: + parameterWrapper->set(sunModelParameters.domega); + break; + case 0x2: + parameterWrapper->set(sunModelParameters.omega_0); + break; + case 0x3: + parameterWrapper->set(sunModelParameters.m_0); + break; + case 0x4: + parameterWrapper->set(sunModelParameters.dm); + break; + case 0x5: + parameterWrapper->set(sunModelParameters.e); + break; + case 0x6: + parameterWrapper->set(sunModelParameters.e1); + break; + case 0x7: + parameterWrapper->set(sunModelParameters.p1); + break; + case 0x8: + parameterWrapper->set(sunModelParameters.p2); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xF): // KalmanFilterParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter); + break; + case 0x1: + parameterWrapper->set(kalmanFilterParameters.requestResetFlag); + break; + case 0x2: + parameterWrapper->set( + kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps); + break; + case 0x3: + parameterWrapper->set(kalmanFilterParameters.processNoiseOmega); + break; + case 0x4: + parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion); + break; + case 0x5: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); + break; + case 0x6: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); + break; + case 0x7: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); + break; + case 0x8: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); + break; + case 0x9: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); + break; + case 0xA: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x10): // MagnetorquesParameter + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); + break; + case 0x4: + parameterWrapper->set(magnetorquesParameter.inverseAlignment); + break; + case 0x5: + parameterWrapper->set(magnetorquesParameter.DipolMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x11): // DetumbleParameter + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(detumbleParameter.detumblecounter); + break; + case 0x1: + parameterWrapper->set(detumbleParameter.omegaDetumbleStart); + break; + case 0x2: + parameterWrapper->set(detumbleParameter.omegaDetumbleEnd); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + default: + return INVALID_IDENTIFIER_ID; + } + return returnvalue::OK; + } else { + return INVALID_DOMAIN_ID; + } }*/