From ffc7a5576332b008015887291fb51eed72494eda Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 21 Oct 2022 14:23:31 +0200 Subject: [PATCH 001/119] added Gps Velocity Calculation --- mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/Guidance.cpp | 6 ++--- mission/controller/acs/OutputValues.h | 1 + mission/controller/acs/SensorProcessing.cpp | 28 +++++++++++++++++++-- mission/controller/acs/SensorProcessing.h | 10 ++++++-- 5 files changed, 39 insertions(+), 7 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 6c72d9e5..d5386909 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -833,6 +833,7 @@ class AcsParameters /*: public HasParametersIF*/ { } strParameters; struct GpsParameters { + double timeDiffVelocityMax = 30; //[s] } gpsParameters; struct GroundStationParameters { diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f6ebee5f..d8bee4f5 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -111,9 +111,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues // Calculation of reference rotation rate //------------------------------------------------------------------------------------- double velSatE[3] = {0, 0, 0}; - velSatE[0] = 0.0; // sensorValues->gps0Velocity[0]; - velSatE[1] = 0.0; // sensorValues->gps0Velocity[1]; - velSatE[2] = 0.0; // sensorValues->gps0Velocity[2]; + velSatE[0] = outputValues->gpsVelocity[0]; // sensorValues->gps0Velocity[0]; + velSatE[1] = outputValues->gpsVelocity[1]; // sensorValues->gps0Velocity[1]; + velSatE[2] = outputValues->gpsVelocity[2]; // sensorValues->gps0Velocity[2]; double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; // Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E MatrixOperations::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h index c9fc71f1..0a4fb04f 100644 --- a/mission/controller/acs/OutputValues.h +++ b/mission/controller/acs/OutputValues.h @@ -44,6 +44,7 @@ public: double gcLatitude; // geocentric latitude, radian double gdLongitude; // Radian longitude + double gpsVelocity[3]; //earth fixed frame }; } diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 998927f6..2b78a3e2 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -454,7 +454,9 @@ void SensorProcessing::processGyr( } void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, - const bool validGps, double *gcLatitude, double *gdLongitude) { + const double gps0altitude, const uint32_t gps0UnixSeconds, + const bool validGps, const AcsParameters::GpsParameters *gpsParameters, + double *gcLatitude, double *gdLongitude, double *gpsVelocityE) { // name to convert not process if (validGps) { // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic @@ -464,6 +466,25 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo double factor = 1 - pow(eccentricityWgs84, 2); *gcLatitude = atan(factor * tan(latitudeRad)); validGcLatitude = true; + + // Calculation of the satellite velocity in earth fixed frame + double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}; + // RADIANS OR DEGREE ? Function needs rad as input + MathOperations::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, posSatE); + if (validSavedPosSatE && (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { + VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); + double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE; + VectorOperations::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3); + } + savedPosSatE[0] = posSatE[0]; + savedPosSatE[1] = posSatE[1]; + savedPosSatE[2] = posSatE[2]; + + timeOfSavedPosSatE = gps0UnixSeconds; + validSavedPosSatE = true; + } + else { + validGcLatitude = false; } } @@ -472,7 +493,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, const AcsParameters *acsParameters) { sensorValues->update(); processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, - sensorValues->gpsSet.isValid(), &outputValues->gcLatitude, &outputValues->gdLongitude); + sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, + sensorValues->gpsSet.isValid(), &acsParameters->gpsParameters, + &outputValues->gcLatitude, &outputValues->gdLongitude, + &outputValues->gpsVelocity); outputValues->mgmUpdated = processMgm( sensorValues->mgm0Lis3Set.fieldStrengths.value, diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 50b78238..81b7e6da 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -65,8 +65,10 @@ class SensorProcessing { void processStr(); - void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, - double *gcLatitude, double *gdLongitude); + void processGps(const double gps0latitude, const double gps0longitude, + const double gps0altitude, const uint32_t gps0UnixSeconds, + const bool validGps, const AcsParameters::GpsParameters *gpsParameters, + double *gcLatitude, double *gdLongitude, double *gpsVelocityE); double savedMagFieldEst[3]; timeval timeOfSavedMagFieldEst; @@ -75,6 +77,10 @@ class SensorProcessing { bool validMagField; bool validGcLatitude; + double savedPosSatE[3]; + uint32_t timeOfSavedPosSatE; + bool validSavedPosSatE; + SusConverter susConverter; AcsParameters acsParameters; }; From e87221a8a3efb90e0b840f2fabcf81d622e121cb Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 21 Oct 2022 16:46:09 +0200 Subject: [PATCH 002/119] added gyro detumble --- mission/controller/acs/control/Detumble.cpp | 14 ++++++++++++++ mission/controller/acs/control/Detumble.h | 4 ++++ 2 files changed, 18 insertions(+) diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 67903c7c..f12d5962 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -63,3 +63,17 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateVa return returnvalue::OK; } + +ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid, + const double *magField, const bool *magFieldValid, + double *magMom) { + + if (!satRateValid || !magFieldValid) { + return DETUMBLE_NO_SENSORDATA; + } + double gain = detumbleCtrlParameters->gainD; + double factor = -gain / pow(VectorOperations::norm(magField,3),2); + VectorOperations::mulScalar(satRate, factor, magMom, 3); + return returnvalue::OK; + +} diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index b85aaf86..e49c88ac 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -38,6 +38,10 @@ public: 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); + private: AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters; AcsParameters::MagnetorquesParameter* magnetorquesParameter; From ba541300caf65b9abe29ffd7b12153f7698f7169 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 28 Oct 2022 18:18:28 +0200 Subject: [PATCH 003/119] updated AcsParams, added class function to get quaternion for sun pointing (guidance) --- mission/controller/acs/AcsParameters.h | 93 +++++++++++--------- mission/controller/acs/Guidance.cpp | 60 ++++++++++++- mission/controller/acs/Guidance.h | 4 + mission/controller/acs/SensorProcessing.cpp | 2 +- mission/controller/acs/control/Detumble.cpp | 6 +- mission/controller/acs/control/Detumble.h | 2 +- mission/controller/acs/control/PtgCtrl.cpp | 1 + mission/controller/acs/control/PtgCtrl.h | 2 +- mission/controller/acs/util/MathOperations.h | 24 +++-- 9 files changed, 133 insertions(+), 61 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index d5386909..b67a484d 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -22,12 +22,22 @@ class AcsParameters /*: public HasParametersIF*/ { const ParameterWrapper *newValues, uint16_t startAtIndex); */ struct OnBoardParams { - double sampleTime = 0.1; // [s] + double sampleTime = 0.4; // [s] } onBoardParams; struct InertiaEIVE { - double inertiaMatrix[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.5, 1.0}}; - double inertiaMatrixInverse[3][3]; + double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, + {-0.0001821456, 0.1701302, 0.0004748963}, + {-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021 + double inertiaMatrixNoPanel[3][3] = {{0.122485, -0.0001798426, -0.005008}, + {-0.0001798426, 0.162240, 0.000475596}, + {-0.005008, 0.000475596, 0.060136}}; //19.11.2021 + double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207}, + {-0.0001836122, 0.16619787, 0.0083537}, + {-0.00501207, 0.0083537, 0.07192588}}; //19.11.2021 + double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767}, + {-0.000178376, 0.166172, -0.007403}, + {-0.005009767, -0.007403, 0.07195314}}; //19.11.2021 } inertiaEIVE; struct MgmHandlingParameters { @@ -768,41 +778,50 @@ class AcsParameters /*: public HasParametersIF*/ { double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, {0.0000, -0.9205, 0.0000, 0.9205}, {0.3907, 0.3907, 0.3907, 0.3907}}; - double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, - {0.2136, -0.3317, 1.0123}, - {-0.8672, -0.1406, 0.1778}, - {0.6426, 0.4794, 1.3603}}; - double without0[4][3]; - double without1[4][3]; - double without2[4][3]; - double without3[4][3]; - double nullspace[4] = {-0.7358, 0.5469, -0.3637, -0.1649}; +// double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, +// {0.2136, -0.3317, 1.0123}, +// {-0.8672, -0.1406, 0.1778}, +// {0.6426, 0.4794, 1.3603}}; +// where does the first pseudo inverse come frome - matlab gives result below + double pseudoInverse[4][3] = {{0.5432, 0, 0.6398}, + {0, -0.5432, 0.6398}, + {-0.5432, 0, 0.6398}, + {0, 0.5432, 0.6398}}; + double without0[4][3] = {{0, 0, 0}, + {0.5432, -0.5432, 1.2797}, + {-1.0864, 0, 0}, + {0.5432, 0.5432, 1.2797}}; + double without1[4][3] = {{0.5432, -0.5432, 1.2797}, + {0, 0, 0}, + {-0.5432, -0.5432, 1.2797}, + {0, 1.0864, 0}}; + double without2[4][3] = {{1.0864, 0, 0}, + {-0.5432, -0.5432, 1.2797}, + {0, 0, 0}, + {-0.5432, 0.5432, 1.2797}}; + double without3[4][3] = {{0.5432, 0.5432, 1.2797}, + {0, -1.0864, 0}, + {-0.5432, 0.5432, 1.2797}, + {0, 0, 0}}; + double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; } rwMatrices; struct SafeModeControllerParameters { double k_rate_mekf = 0.00059437; double k_align_mekf = 0.000056875; - double k_rate_no_mekf; - double k_align_no_mekf; - double sunMagAngleMin; + double k_rate_no_mekf = 0.00059437;; + double k_align_no_mekf = 0.000056875;; + double sunMagAngleMin; // ??? - double sunTargetDir[3] = {1, 0, 0}; // Body frame - double satRateRef[3]; // Body frame + double sunTargetDir[3] = {0, 0, 1}; // Geometry Frame + double satRateRef[3] = {0, 0, 0}; // Geometry Frame } safeModeControllerParameters; - struct DetumbleCtrlParameters { - double gainD = pow(10.0, -3.3); - - } detumbleCtrlParameters; - // ToDo: mutiple structs for different pointing mode controllers? struct PointingModeControllerParameters { - double updtFlag; - double A_rw[3][12]; - - double refDirection[3] = {1, 0, 0}; + double refDirection[3] = {1, 0, 0}; //Antenna double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; bool avoidBlindStr = true; @@ -811,9 +830,7 @@ class AcsParameters /*: public HasParametersIF*/ { double blindRotRate = 1 * M_PI / 180; double zeta = 0.3; - double zetaLow; double om = 0.3; - double omLow; double omMax = 1 * M_PI / 180; double qiMin = 0.1; double gainNullspace = 0.01; @@ -828,8 +845,7 @@ class AcsParameters /*: public HasParametersIF*/ { struct StrParameters { double exclusionAngle = 20 * M_PI / 180; - // double strOrientationMatrix[3][3]; - double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // in body/geometry frame + double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame } strParameters; struct GpsParameters { @@ -840,13 +856,9 @@ class AcsParameters /*: public HasParametersIF*/ { double latitudeGs = 48.7495 * M_PI / 180.; // [rad] Latitude double longitudeGs = 9.10384 * M_PI / 180.; // [rad] Longitude double altitudeGs = 500; // [m] Altitude - double earthRadiusEquat = 6378137; // [m] - double earthRadiusPolar = 6356752.314; // [m] } groundStationParameters; // Stuttgart struct SunModelParameters { - enum UseSunModel { NO = 0, YES = 3 }; - uint8_t useSunModel; float domega = 36000.771; float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee float m_0 = 357.5256; // coefficients for mean anomaly @@ -859,12 +871,6 @@ class AcsParameters /*: public HasParametersIF*/ { } sunModelParameters; struct KalmanFilterParameters { - uint8_t activateKalmanFilter; - uint8_t requestResetFlag; - double maxToleratedTimeBetweenKalmanFilterExecutionSteps; - double processNoiseOmega[3]; - double processNoiseQuaternion[4]; - double sensorNoiseSTR = 0.1 * M_PI / 180; double sensorNoiseSS = 8 * M_PI / 180; double sensorNoiseMAG = 4 * M_PI / 180; @@ -885,9 +891,10 @@ class AcsParameters /*: public HasParametersIF*/ { } magnetorquesParameter; struct DetumbleParameter { - uint8_t detumblecounter; - double omegaDetumbleStart; - double omegaDetumbleEnd; + uint8_t detumblecounter = 75; + double omegaDetumbleStart = 2 * M_PI / 180; + double omegaDetumbleEnd = 0.2 * M_PI / 180; + double gainD = pow(10.0, -3.3); } detumbleParameter; }; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index d8bee4f5..f01deab6 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -45,8 +45,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value, - sensorValues->gpsSet.longitude.value, + double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; + double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); // Target direction in the ECEF frame @@ -190,6 +191,59 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues } } +void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, + double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion to sun + //------------------------------------------------------------------------------------- + double quatBJ[4] = {0, 0, 0, 0}; + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::toDcm(quatBJ, dcmBJ); + + double sunDirJ[3] = {0, 0, 0}, sunDir[3] = {0, 0, 0}; + if (outputValues->sunDirModelValid) { + sunDirJ[0] = outputValues->sunDirModel[0]; + sunDirJ[1] = outputValues->sunDirModel[1]; + sunDirJ[2] = outputValues->sunDirModel[2]; + MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDir, 3, 3, 1); + } + + else { + sunDir[0] = outputValues->sunDirEst[0]; + sunDir[1] = outputValues->sunDirEst[1]; + sunDir[2] = outputValues->sunDirEst[2]; + } + + double sunRef[3] = {0, 0, 0}; + sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0]; + sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1]; + sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2]; + + double sunCross[3] = {0, 0, 0}; + VectorOperations::cross(sunDir, sunRef, sunCross); + double normSunDir = VectorOperations::norm(sunDir, 3); + double normSunRef = VectorOperations::norm(sunRef, 3); + double dotSun = VectorOperations::dot(sunDir, sunRef); + + targetQuat[0] = sunCross[0]; + targetQuat[1] = sunCross[1]; + targetQuat[2] = sunCross[2]; + targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun); + + VectorOperations::normalize(targetQuat, targetQuat, 4); + + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; +} + void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3]) { double quatRef[4] = {0, 0, 0, 0}; @@ -310,7 +364,7 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double * // @note: This one takes the normal pseudoInverse of all four raction wheels valid. // Does not make sense, but is implemented that way in MATLAB ?! // Thought: It does not really play a role, because in case there are more then one - // reaction wheel the pointing control is destined to fail. + // reaction wheel invalid the pointing control is destined to fail. rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index bf53d767..7b05a251 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -26,6 +26,10 @@ public: void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate for sun pointing after ground station + void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, + double targetQuat[4], double refSatRate[3]); + // @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 2b78a3e2..1a8b02d9 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -496,7 +496,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, sensorValues->gpsSet.isValid(), &acsParameters->gpsParameters, &outputValues->gcLatitude, &outputValues->gdLongitude, - &outputValues->gpsVelocity); + outputValues->gpsVelocity); outputValues->mgmUpdated = processMgm( sensorValues->mgm0Lis3Set.fieldStrengths.value, diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index f12d5962..9f99de5b 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -27,7 +27,7 @@ Detumble::~Detumble(){ void Detumble::loadAcsParameters(AcsParameters *acsParameters_){ - detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); + detumbleParameter = &(acsParameters_->detumbleParameter); magnetorquesParameter = &(acsParameters_->magnetorquesParameter); } @@ -40,7 +40,7 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid, if (!magRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleCtrlParameters->gainD; + double gain = detumbleParameter->gainD; double factor = -gain / pow(VectorOperations::norm(magField,3),2); VectorOperations::mulScalar(magRate, factor, magMom, 3); return returnvalue::OK; @@ -71,7 +71,7 @@ ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateVa if (!satRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleCtrlParameters->gainD; + double gain = detumbleParameter->gainD; double factor = -gain / pow(VectorOperations::norm(magField,3),2); VectorOperations::mulScalar(satRate, factor, magMom, 3); return returnvalue::OK; diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index e49c88ac..34f7c312 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -43,7 +43,7 @@ public: double *magMom); private: - AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters; + AcsParameters::DetumbleParameter* detumbleParameter; AcsParameters::MagnetorquesParameter* magnetorquesParameter; }; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 6201a32a..211bc50e 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -109,6 +109,7 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do double torque[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueQuat, torque, 3); MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); + VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index be67187d..b537681e 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -34,7 +34,7 @@ class PtgCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::PTG; static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters für this class + /* @brief: Load AcsParameters for this class * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void loadAcsParameters(AcsParameters *acsParameters_); diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 6a534880..7a20e9e2 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -104,9 +104,16 @@ public: } static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){ - - double radiusPolar = 6378137; - double radiusEqua = 6356752.314; + /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, + * longitude and altitude + * @param: lat geodetic latitude [rad] + * longi longitude [rad] + * alt altitude [m] + * cartesianOutput Cartesian Coordinates in ECEF (3x1) + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff + * Landis Markley and John L. Crassidis*/ + double radiusPolar = 6356752.314; + double radiusEqua = 6378137; double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2)); double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2)); @@ -117,13 +124,12 @@ public: } - /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame - * @param: time Current time - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff - * Landis Markley and John L. Crassidis*/ static void dcmEJ(timeval time, T1 * outputDcmEJ){ - + /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame + * @param: time Current time + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff + * Landis Markley and John L. Crassidis*/ double JD2000Floor = 0; double JD2000 = convertUnixToJD2000(time); // Getting Julian Century from Day start : JD (Y,M,D,0,0,0) From 75ab11fc35d44c394dd1dc4d93555ace44338f66 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 4 Nov 2022 17:21:17 +0100 Subject: [PATCH 004/119] updated AcsParameters. change DCM_EJ calc to with precission and nutation --- mission/controller/AcsController.cpp | 39 +++++++++++++++++++- mission/controller/AcsController.h | 2 + mission/controller/acs/AcsParameters.h | 14 ++++--- mission/controller/acs/Guidance.cpp | 16 ++------ mission/controller/acs/control/PtgCtrl.cpp | 2 +- mission/controller/acs/control/PtgCtrl.h | 2 +- mission/controller/acs/util/MathOperations.h | 13 ++++++- 7 files changed, 66 insertions(+), 22 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 856a0034..9199e1d4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -45,6 +45,9 @@ void AcsController::performControlOperation() { case SUBMODE_PTG_GS: performPointingCtrl(); break; + case SUBMODE_PTG_SUN: + performPointingCtrlSun(); + break; } } break; @@ -124,7 +127,41 @@ void AcsController::performPointingCtrl() { double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; - ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + double rwTrqNs[4] = {0, 0, 0, 0}; + ptgCtrl.ptgNullspace( + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input + actuatorCmd.cmdSpeedToRws( + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws, + rwTrqNs, cmdSpeedRws); + double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + ptgCtrl.ptgDesaturation( + outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); +} + +void AcsController::performPointingCtrlSun() { + ACS::SensorValues sensorValues; + ACS::OutputValues outputValues; + + timeval now; // Übergabe ? + + sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &outputValues, &validMekf); + double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; + guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); + double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; + guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate); + double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; + ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); double rwTrqNs[4] = {0, 0, 0, 0}; ptgCtrl.ptgNullspace( &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c23adfc1..fda3f0ec 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -27,12 +27,14 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_DETUMBLE = 3; static const Submode_t SUBMODE_PTG_GS = 4; static const Submode_t SUBMODE_PTG_NADIR = 5; + static const Submode_t SUBMODE_PTG_SUN = 6; protected: void performSafe(); void performDetumble(); void performPointingCtrl(); + void performPointingCtrlSun(); private: AcsParameters acsParameters; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index b67a484d..fe8654f5 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -29,7 +29,11 @@ class AcsParameters /*: public HasParametersIF*/ { double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, {-0.0001821456, 0.1701302, 0.0004748963}, {-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021 - double inertiaMatrixNoPanel[3][3] = {{0.122485, -0.0001798426, -0.005008}, + // Possible inertia matrices + double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, + {-0.0001821456, 0.1701302, 0.0004748963}, + {-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021 + double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008}, {-0.0001798426, 0.162240, 0.000475596}, {-0.005008, 0.000475596, 0.060136}}; //19.11.2021 double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207}, @@ -37,7 +41,7 @@ class AcsParameters /*: public HasParametersIF*/ { {-0.00501207, 0.0083537, 0.07192588}}; //19.11.2021 double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767}, {-0.000178376, 0.166172, -0.007403}, - {-0.005009767, -0.007403, 0.07195314}}; //19.11.2021 + {-0.005009767, -0.007403, 0.07195314}}; } inertiaEIVE; struct MgmHandlingParameters { @@ -821,7 +825,7 @@ class AcsParameters /*: public HasParametersIF*/ { // ToDo: mutiple structs for different pointing mode controllers? struct PointingModeControllerParameters { - double refDirection[3] = {1, 0, 0}; //Antenna + double refDirection[3] = {-1, 0, 0}; //Antenna double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; bool avoidBlindStr = true; @@ -891,9 +895,9 @@ class AcsParameters /*: public HasParametersIF*/ { } magnetorquesParameter; struct DetumbleParameter { - uint8_t detumblecounter = 75; + uint8_t detumblecounter = 75; // 30 s double omegaDetumbleStart = 2 * M_PI / 180; - double omegaDetumbleEnd = 0.2 * M_PI / 180; + double omegaDetumbleEnd = 0.4 * M_PI / 180; double gainD = pow(10.0, -3.3); } detumbleParameter; }; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f01deab6..ce6abf48 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -57,21 +57,11 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::dcmEJ(now, *dcmEJ); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; - double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth; - - // TEST SECTION ! - // double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - // MatrixOperations::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, - // dcmTEST, dcmTEST, 3, 3, 3); - - MatrixOperations::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3); - MatrixOperations::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3); MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // Transformation between ECEF and Body frame diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 211bc50e..a412982d 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -31,7 +31,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){ rwMatrices =&(acsParameters_->rwMatrices); } -void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){ +void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){ //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index b537681e..2c92a7e3 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -42,7 +42,7 @@ class PtgCtrl { /* @brief: Calculates the needed torque for the pointing control mechanism * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ - void ptgGroundstation(const double mode, const double *qError, const double *deltaRate, + void ptgLaw(const double mode, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 7a20e9e2..899e199c 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -124,10 +124,11 @@ public: } - static void dcmEJ(timeval time, T1 * outputDcmEJ){ + static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){ /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame * @param: time Current time * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * outputDotDcmEJ Derivative of transformation matrix [3][3] * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff * Landis Markley and John L. Crassidis*/ double JD2000Floor = 0; @@ -162,6 +163,16 @@ public: outputDcmEJ[7] = 0; outputDcmEJ[8] = 1; + // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION + double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, + {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, + {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; + double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = 0.000072921158553; + double dotDcmEJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + MatrixOperations::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3); + MatrixOperations::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3); + } /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame From 20936faec6f7e2465b833dbc6225d32cf5940093 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Tue, 8 Nov 2022 13:48:50 +0100 Subject: [PATCH 005/119] added Antistiction, added Nadir Pointing, added performSafe() --- mission/controller/AcsController.cpp | 108 +++++++++++++-------- mission/controller/AcsController.h | 3 +- mission/controller/acs/AcsParameters.h | 7 +- mission/controller/acs/ActuatorCmd.cpp | 23 ++--- mission/controller/acs/ActuatorCmd.h | 12 ++- mission/controller/acs/Guidance.cpp | 67 +++++++++++++ mission/controller/acs/Guidance.h | 4 + mission/controller/acs/control/PtgCtrl.cpp | 35 ++++++- mission/controller/acs/control/PtgCtrl.h | 11 +++ 9 files changed, 213 insertions(+), 57 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 9199e1d4..d2bd0510 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -8,6 +8,7 @@ AcsController::AcsController(object_id_t objectId) navigation(&acsParameters), actuatorCmd(&acsParameters), guidance(&acsParameters), + safeCtrl(&acsParameters), detumble(&acsParameters), ptgCtrl(&acsParameters), detumbleCounter{0}, @@ -35,7 +36,7 @@ void AcsController::performControlOperation() { if (mode != MODE_OFF) { switch (submode) { case SUBMODE_SAFE: - // performSafe(); + performSafe(); break; case SUBMODE_DETUMBLE: @@ -45,9 +46,14 @@ void AcsController::performControlOperation() { case SUBMODE_PTG_GS: performPointingCtrl(); break; + case SUBMODE_PTG_SUN: - performPointingCtrlSun(); - break; + performPointingCtrl(); + break; + + case SUBMODE_PTG_NADIR: + performPointingCtrl(); + break; } } break; @@ -75,7 +81,40 @@ void AcsController::performControlOperation() { // DEBUG END } -void AcsController::performSafe() {} +void AcsController::performSafe() { + ACS::SensorValues sensorValues; + ACS::OutputValues outputValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &outputValues, &validMekf); + // Give desired satellite rate and sun direction to align + double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; + guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); +// IF MEKF is working + double magMomMtq[3] = {0, 0, 0}; + bool magMomMtqValid = false; + if ( !validMekf ) { // Valid = 0, Failed = 1 + safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid), + (outputValues.magFieldModel), &(outputValues.magFieldModelValid), + (outputValues.sunDirModel), &(outputValues.sunDirModelValid), + (outputValues.satRateMekf), &(outputValues.satRateMekfValid), + sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid); + } + else { + + safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid, + outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid), + outputValues.magFieldEst, &(outputValues.magFieldEstValid), + outputValues.magneticFieldVectorDerivative, &(outputValues.magneticFieldVectorDerivativeValid), + sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid); + + } + +} void AcsController::performDetumble() { ACS::SensorValues sensorValues; @@ -121,7 +160,18 @@ void AcsController::performPointingCtrl() { ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + + switch (submode) { + case SUBMODE_PTG_GS: + guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + break; + case SUBMODE_PTG_SUN: + guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); + break; + case SUBMODE_PTG_NADIR: + guidance.targetQuatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + break; + } double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate); double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -132,45 +182,21 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgNullspace( &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + + if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { + bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? + double rwSpeed[4] = {&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value)}; + ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + } + double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input actuatorCmd.cmdSpeedToRws( &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws, - rwTrqNs, cmdSpeedRws); - double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol - ptgCtrl.ptgDesaturation( - outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); -} - -void AcsController::performPointingCtrlSun() { - ACS::SensorValues sensorValues; - ACS::OutputValues outputValues; - - timeval now; // Übergabe ? - - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &outputValues, &validMekf); - double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); - double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; - guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate); - double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; - ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - double rwTrqNs[4] = {0, 0, 0, 0}; - ptgCtrl.ptgNullspace( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws, - rwTrqNs, cmdSpeedRws); + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol ptgCtrl.ptgDesaturation( outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index fda3f0ec..21a0c86d 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -9,6 +9,7 @@ #include "acs/Guidance.h" #include "acs/Navigation.h" #include "acs/SensorProcessing.h" +#include "acs/control/SafeCtrl.h" #include "acs/control/Detumble.h" #include "acs/control/PtgCtrl.h" #include "controllerdefinitions/AcsCtrlDefinitions.h" @@ -34,7 +35,6 @@ class AcsController : public ExtendedControllerBase { void performSafe(); void performDetumble(); void performPointingCtrl(); - void performPointingCtrlSun(); private: AcsParameters acsParameters; @@ -43,6 +43,7 @@ class AcsController : public ExtendedControllerBase { ActuatorCmd actuatorCmd; Guidance guidance; + SafeCtrl safeCtrl; Detumble detumble; PtgCtrl ptgCtrl; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index fe8654f5..ec535b75 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -776,6 +776,9 @@ class AcsParameters /*: public HasParametersIF*/ { double rw3orientationMatrix[3][3]; double inertiaWheel = 0.000028198; double maxTrq = 0.0032; // 3.2 [mNm] + double stictionSpeed = 80; //RPM + double stictionReleaseSpeed = 120; //RPM + double stictionTorque = 0.0006; } rwHandlingParameters; struct RwMatrices { @@ -842,10 +845,12 @@ class AcsParameters /*: public HasParametersIF*/ { double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; bool desatOn = true; + bool enableAntiStiction = true; double omegaEarth = 0.000072921158553; - } inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; + double nadirRefDirection[3] = {-1, 0, 0}; //Camera + } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; struct StrParameters { double exclusionAngle = 20 * M_PI / 180; diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 920989ed..00f36d24 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -23,26 +23,27 @@ ActuatorCmd::~ActuatorCmd(){ } -void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, - const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) { - using namespace Math; +void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { // Scaling the commanded torque to a maximum value - double torque[4] = {0, 0, 0, 0}; double maxTrq = acsParameters.rwHandlingParameters.maxTrq; - VectorOperations::add(rwTrqIn, rwTrqNS, torque, 4); double maxValue = 0; for (int i = 0; i < 4; i++) { // size of torque, always 4 ? - if (abs(torque[i]) > maxValue) { - maxValue = abs(torque[i]); + if (abs(rwTrq[i]) > maxValue) { + maxValue = abs(rwTrq[i]); } } if (maxValue > maxTrq) { - double scalingFactor = maxTrq / maxValue; - VectorOperations::mulScalar(torque, scalingFactor, torque, 4); + double scalingFactor = maxTrq / 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, double *rwCmdSpeed) { + using namespace Math; // Calculating the commanded speed in RPM for every reaction wheel double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; @@ -52,7 +53,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1 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; - VectorOperations::mulScalar(torque, factor, deltaSpeed, 4); + VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 1da2c89a..95d492c1 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -20,6 +20,14 @@ public: ActuatorCmd(AcsParameters *acsParameters_); //Input mode ? virtual ~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 + */ + void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); + /* * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction wheels, also * will calculate the needed revolutions per minute for the RWs, which will be given @@ -29,8 +37,8 @@ public: * 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 *rwTrqIn, - const double *rwTrqNS, double *rwCmdSpeed); + const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTorque, + double *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index ce6abf48..bd7ccf6c 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -234,6 +234,73 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou refSatRate[2] = 0; } +void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for Nadir pointing + //------------------------------------------------------------------------------------- + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; + double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); + + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + + // Transformation between ECEF and Body frame + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::toDcm(quatBJ, dcmBJ); + MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); + + // Target Direction in the body frame + double targetDirB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1); + + // rotation quaternion from two vectors + double refDir[3] = {0, 0, 0}; + refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0]; + refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1]; + refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[2]; + double noramlizedTargetDirB[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); + VectorOperations::normalize(refDir, refDir, 3); + double normTargetDirB = VectorOperations::norm(noramlizedTargetDirB, 3); + double normRefDir = VectorOperations::norm(refDir, 3); + double crossDir[3] = {0, 0, 0}; + double dotDirections = VectorOperations::dot(noramlizedTargetDirB, refDir); + VectorOperations::cross(noramlizedTargetDirB, refDir, crossDir); + targetQuat[0] = crossDir[0]; + targetQuat[1] = crossDir[1]; + targetQuat[2] = crossDir[2]; + targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections); + VectorOperations::normalize(targetQuat, targetQuat, 4); + + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + +} + void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3]) { double quatRef[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 7b05a251..eb58daf8 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -30,6 +30,10 @@ public: void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing + void targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); + // @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index a412982d..337a8d8f 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -16,7 +16,7 @@ #include #include -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} { loadAcsParameters(acsParameters_); } @@ -164,3 +164,36 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } + +void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw, + double* torqueCommand) { + for (uint8_t i = 0; i < 4; i++) { + if (rwAvailable[i]) { + if (torqueMemory[i] != 0) { + if ((omegaRw[i] * torqueMemory[i]) + > rwHandlingParameters->stictionReleaseSpeed) { + torqueMemory[i] = 0; + } else { + torqueCommand[i] = torqueMemory[i] + * rwHandlingParameters->stictionTorque; + } + } else { + if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) + && (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { + if (omegaRw[i] < omegaMemory[i]) { + torqueMemory[i] = -1; + } else { + torqueMemory[i] = 1; + } + + torqueCommand[i] = torqueMemory[i] + * 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 2c92a7e3..2a3bde5c 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -52,11 +52,22 @@ class PtgCtrl { void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs); + /* @brief: Commands the stiction torque in case wheel speed is to low + * @param: rwAvailable Boolean Flag for all reaction wheels + * omegaRw current wheel speed of reaction wheels + * torqueCommand modified torque after antistiction + */ + void rwAntistiction(const bool* rwAvailable, const double* omegaRw, + double* torqueCommand); + private: AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::RwMatrices *rwMatrices; + + double torqueMemory[4]; + double omegaMemory[4]; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ From 55dec574c5bd23825fbbd1f12dbb6d01c62c86c5 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Mon, 14 Nov 2022 17:16:47 +0100 Subject: [PATCH 006/119] added inertial pointing --- mission/controller/AcsController.cpp | 19 ++++++++++++------- mission/controller/AcsController.h | 3 ++- mission/controller/acs/AcsParameters.h | 10 ++++++---- mission/controller/acs/Guidance.cpp | 15 ++++++++++++--- mission/controller/acs/Guidance.h | 5 ++++- mission/controller/acs/control/PtgCtrl.cpp | 3 +-- mission/controller/acs/control/PtgCtrl.h | 2 +- 7 files changed, 38 insertions(+), 19 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d2bd0510..6e5e4473 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -43,7 +43,7 @@ void AcsController::performControlOperation() { performDetumble(); break; - case SUBMODE_PTG_GS: + case SUBMODE_PTG_TARGET: performPointingCtrl(); break; @@ -113,7 +113,8 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid); } - + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); } void AcsController::performDetumble() { @@ -154,7 +155,8 @@ void AcsController::performPointingCtrl() { ACS::SensorValues sensorValues; ACS::OutputValues outputValues; - timeval now; // Übergabe ? + timeval now; + Clock::getClock_timeval(&now); sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; @@ -162,14 +164,17 @@ void AcsController::performPointingCtrl() { double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; switch (submode) { - case SUBMODE_PTG_GS: + case SUBMODE_PTG_TARGET: guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_SUN: guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); break; case SUBMODE_PTG_NADIR: - guidance.targetQuatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + break; + case SUBMODE_PTG_INERTIAL: + guidance.inertialQuatPtg(targetQuat, refSatRate); break; } double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; @@ -188,8 +193,8 @@ void AcsController::performPointingCtrl() { if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - double rwSpeed[4] = {&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value)}; + int32_t rwSpeed[4] = {(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), + (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 21a0c86d..4e9d435a 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -26,9 +26,10 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_SAFE = 2; static const Submode_t SUBMODE_DETUMBLE = 3; - static const Submode_t SUBMODE_PTG_GS = 4; + static const Submode_t SUBMODE_PTG_TARGET = 4; static const Submode_t SUBMODE_PTG_NADIR = 5; static const Submode_t SUBMODE_PTG_SUN = 6; + static const Submode_t SUBMODE_PTG_INERTIAL = 7; protected: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index ec535b75..953f50db 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -45,10 +45,10 @@ class AcsParameters /*: public HasParametersIF*/ { } inertiaEIVE; struct MgmHandlingParameters { - float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; - float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; - float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; - float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; + float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; // Documentation not consistent + float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; // Documentation not consistent + float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; // Documentation not consistent + float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; // Documentation not consistent float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; float mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508}; @@ -850,6 +850,8 @@ class AcsParameters /*: public HasParametersIF*/ { double omegaEarth = 0.000072921158553; double nadirRefDirection[3] = {-1, 0, 0}; //Camera + double refQuatInertial[4] = {0, 0, 0, 1}; + double refRotRateInertial[3] = {0, 0, 0}; } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; struct StrParameters { diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index bd7ccf6c..b2f47953 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -32,9 +32,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- - // Calculation of target quaternion to groundstation + // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth + // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) double groundStationCart[3] = {0, 0, 0}; @@ -234,7 +234,7 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou refSatRate[2] = 0; } -void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, +void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing @@ -301,6 +301,15 @@ void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputVa } +void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { + for (int i = 0; i < 4; i++) { + targetQuat[i] = acsParameters.inertialModeControllerParameters.refQuatInertial[i]; + } + for (int i = 0; i < 3; i++) { + refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRateInertial[i]; + } +} + void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3]) { double quatRef[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index eb58daf8..6b0759fb 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -31,9 +31,12 @@ public: double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing - void targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate from parameters for inertial pointing + void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); + // @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 337a8d8f..fd9dd9eb 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -165,7 +165,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } -void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw, +void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, double* torqueCommand) { for (uint8_t i = 0; i < 4; i++) { if (rwAvailable[i]) { @@ -196,4 +196,3 @@ void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw, omegaMemory[i] = omegaRw[i]; } } -} diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 2a3bde5c..8cacd556 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -57,7 +57,7 @@ class PtgCtrl { * omegaRw current wheel speed of reaction wheels * torqueCommand modified torque after antistiction */ - void rwAntistiction(const bool* rwAvailable, const double* omegaRw, + void rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, double* torqueCommand); private: From 609d429161129e18581a62575327efc92b8dd482 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Tue, 22 Nov 2022 21:10:05 +0100 Subject: [PATCH 007/119] added Nadir FLP version --- mission/controller/AcsController.cpp | 2 +- mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/Guidance.cpp | 94 +++++++++++++++++++++++++- mission/controller/acs/Guidance.h | 6 ++ 4 files changed, 101 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 6e5e4473..7ef85399 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -171,7 +171,7 @@ void AcsController::performPointingCtrl() { guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); break; case SUBMODE_PTG_NADIR: - guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.quatNadirPtgFLPVersion(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 953f50db..3566d3fb 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -852,6 +852,7 @@ class AcsParameters /*: public HasParametersIF*/ { double nadirRefDirection[3] = {-1, 0, 0}; //Camera double refQuatInertial[4] = {0, 0, 0, 1}; double refRotRateInertial[3] = {0, 0, 0}; + int8_t nadirTimeElapsedMax = 10; } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; struct StrParameters { diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index b2f47953..0d0022fc 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -125,10 +125,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues // Calculation of reference rotation rate in case of star tracker blinding //------------------------------------------------------------------------------------- if (acsParameters.targetModeControllerParameters.avoidBlindStr) { - double sunDirJ[3] = {0, 0, 0}; double sunDirB[3] = {0, 0, 0}; if (outputValues->sunDirModelValid) { + double sunDirJ[3] = {0, 0, 0}; sunDirJ[0] = outputValues->sunDirModel[0]; sunDirJ[1] = outputValues->sunDirModel[1]; sunDirJ[2] = outputValues->sunDirModel[2]; @@ -301,6 +301,98 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues * } +void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]) { + + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for Nadir pointing + //------------------------------------------------------------------------------------- + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; + double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); + + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + + // Target Direction in the body frame + double targetDirJ[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); + + // negative x-Axis aligned with target (Camera position) + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + + // z-Axis parallel to long side of picture resolution + double zAxis[3] = {0, 0, 0}; + double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; + double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityJ, 3); + VectorOperations::cross(xAxis, velocityJ, zAxis); + VectorOperations::normalize(zAxis, zAxis, 3); + + // y-Axis completes RHS + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(zAxis, xAxis, yAxis); + + //Complete transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; + QuaternionOperations::fromDcm(dcmTgt,targetQuat); + + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec + + timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6)); + if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { + double qDiff[4] = {0, 0, 0, 0}; + VectorOperations::subtract(targetQuat, savedQuaternionNadir, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); + + double tgtQuatVec[3] = {targetQuat[0], targetQuat[1], targetQuat[2]}, + qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; + VectorOperations::cross(targetQuat, qDiff, sum1); + VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, targetQuat[3], sum3, 3); + VectorOperations::add(sum1, sum2, sum, 3); + VectorOperations::subtract(sum, sum3, sum, 3); + double omegaRefNew[3] = {0, 0, 0}; + VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); + + VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); + VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); + omegaRefSavedNadir[0] = omegaRefNew[0]; + omegaRefSavedNadir[1] = omegaRefNew[1]; + omegaRefSavedNadir[2] = omegaRefNew[2]; + } + else { + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + } + + timeSavedQuaternionNadir = now; + savedQuaternionNadir[0] = targetQuat[0]; + savedQuaternionNadir[1] = targetQuat[1]; + savedQuaternionNadir[2] = targetQuat[2]; + savedQuaternionNadir[3] = targetQuat[3]; +} + void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { for (int i = 0; i < 4; i++) { targetQuat[i] = acsParameters.inertialModeControllerParameters.refQuatInertial[i]; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 6b0759fb..80ef6cf4 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -34,6 +34,9 @@ public: void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); + void quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate from parameters for inertial pointing void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); @@ -48,6 +51,9 @@ public: private: AcsParameters acsParameters; bool strBlindAvoidFlag = false; + timeval timeSavedQuaternionNadir; + double savedQuaternionNadir[4] = {0, 0, 0, 0}; + double omegaRefSavedNadir[3] = {0, 0, 0}; }; #endif /* ACS_GUIDANCE_H_ */ From 8d1cbd9f8b64f00c9e40571efffb5eda3753dc72 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 24 Nov 2022 13:40:55 +0100 Subject: [PATCH 008/119] changed calculation of quaternion for target and sun pointing --- mission/controller/AcsController.cpp | 4 +- mission/controller/acs/AcsParameters.h | 4 +- mission/controller/acs/Guidance.cpp | 238 ++++++++++++++++++++++--- mission/controller/acs/Guidance.h | 8 +- 4 files changed, 221 insertions(+), 33 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7ef85399..211bea30 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -168,10 +168,10 @@ void AcsController::performPointingCtrl() { guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_SUN: - guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate); + guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_NADIR: - guidance.quatNadirPtgFLPVersion(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); break; case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 3566d3fb..24018ad9 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -850,8 +850,8 @@ class AcsParameters /*: public HasParametersIF*/ { double omegaEarth = 0.000072921158553; double nadirRefDirection[3] = {-1, 0, 0}; //Camera - double refQuatInertial[4] = {0, 0, 0, 1}; - double refRotRateInertial[3] = {0, 0, 0}; + double tgtQuatInertial[4] = {0, 0, 0, 1}; + double tgtRotRateInertial[3] = {0, 0, 0}; int8_t nadirTimeElapsedMax = 10; } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 0d0022fc..d1799d60 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -29,7 +29,7 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); } -void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, +void Guidance::targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude @@ -181,7 +181,124 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues } } -void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, +void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]) { + + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for target pointing + //------------------------------------------------------------------------------------- + // Transform longitude, latitude and altitude to cartesian coordiantes (earth + // fixed/centered frame) + double groundStationCart[3] = {0, 0, 0}; + + MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, + acsParameters.groundStationParameters.longitudeGs, + acsParameters.groundStationParameters.altitudeGs, + groundStationCart); + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; + double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + + // Target Direction and position vector in the inertial frame + double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); + MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); + + // negative x-Axis aligned with target (Camera/E-band transmitter position) + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + + // Transform velocity into inertial frame + double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; + double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityJ, 3); + + // orbital normal vector + double orbitalNormalJ[3] = {0, 0, 0}; + VectorOperations::cross(posSatJ, velocityJ, orbitalNormalJ); + VectorOperations::normalize(orbitalNormalJ, orbitalNormalJ, 3); + + // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(orbitalNormalJ, xAxis, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); + + // z-Axis completes RHS + double zAxis[3] = {0, 0, 0}; + VectorOperations::cross(xAxis, yAxis, zAxis); + + //Complete transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; + double quatInertialTarget[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget); + + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec + + timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6)); + if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { + double qDiff[4] = {0, 0, 0, 0}; + VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); + + double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, + qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; + VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::add(sum1, sum2, sum, 3); + VectorOperations::subtract(sum, sum3, sum, 3); + double omegaRefNew[3] = {0, 0, 0}; + VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); + + VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); + VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); + omegaRefSavedNadir[0] = omegaRefNew[0]; + omegaRefSavedNadir[1] = omegaRefNew[1]; + omegaRefSavedNadir[2] = omegaRefNew[2]; + } + else { + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + } + + timeSavedQuaternionNadir = now; + savedQuaternionNadir[0] = quatInertialTarget[0]; + savedQuaternionNadir[1] = quatInertialTarget[1]; + savedQuaternionNadir[2] = quatInertialTarget[2]; + savedQuaternionNadir[3] = quatInertialTarget[3]; + + // Transform in system relative to satellite frame + double quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); +} + +void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun @@ -194,30 +311,34 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou quatBJ[3] = outputValues->quatMekfBJ[3]; QuaternionOperations::toDcm(quatBJ, dcmBJ); - double sunDirJ[3] = {0, 0, 0}, sunDir[3] = {0, 0, 0}; + double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0}; if (outputValues->sunDirModelValid) { sunDirJ[0] = outputValues->sunDirModel[0]; sunDirJ[1] = outputValues->sunDirModel[1]; sunDirJ[2] = outputValues->sunDirModel[2]; - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDir, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); } else { - sunDir[0] = outputValues->sunDirEst[0]; - sunDir[1] = outputValues->sunDirEst[1]; - sunDir[2] = outputValues->sunDirEst[2]; + sunDirB[0] = outputValues->sunDirEst[0]; + sunDirB[1] = outputValues->sunDirEst[1]; + sunDirB[2] = outputValues->sunDirEst[2]; } + /* + // --------------------------------------------------------------------------- + // Old version of two vector quaternion (only one axis to align) + // --------------------------------------------------------------------------- double sunRef[3] = {0, 0, 0}; sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0]; sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1]; sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2]; double sunCross[3] = {0, 0, 0}; - VectorOperations::cross(sunDir, sunRef, sunCross); - double normSunDir = VectorOperations::norm(sunDir, 3); + VectorOperations::cross(sunDirB, sunRef, sunCross); + double normSunDir = VectorOperations::norm(sunDirB, 3); double normSunRef = VectorOperations::norm(sunRef, 3); - double dotSun = VectorOperations::dot(sunDir, sunRef); + double dotSun = VectorOperations::dot(sunDirB, sunRef); targetQuat[0] = sunCross[0]; targetQuat[1] = sunCross[1]; @@ -225,17 +346,73 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun); VectorOperations::normalize(targetQuat, targetQuat, 4); + */ - //------------------------------------------------------------------------------------- + //---------------------------------------------------------------------------- + // New version + //---------------------------------------------------------------------------- + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + + // positive z-Axis of EIVE in direction of sun + double zAxis[3] = {0 ,0 ,0}; + VectorOperations::normalize(sunDirB, zAxis, 3); + + // Position of the satellite in the earth/fixed frame via GPS and body + // velocity + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; + double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; + double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityJ, 3); + double velocityB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1); + + // Normale to velocity and sunDir + double crossVelSun[3] = {0, 0, 0}; + VectorOperations::cross(velocityB, sunDirB, crossVelSun); + + // y- Axis as cross of normal velSun and zAxis + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(crossVelSun, sunDirB, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); + + // complete RHS for x-Axis + double xAxis[3] = {0, 0, 0}; + VectorOperations::cross(yAxis, zAxis, xAxis); + + // Transformation matrix to Sun, no further transforamtions, reference is already + // the EIVE body frame + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; + double quatSun[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt,quatSun); + + targetQuat[0] = quatSun[0]; + targetQuat[1] = quatSun[1]; + targetQuat[2] = quatSun[2]; + targetQuat[3] = quatSun[3]; + + //---------------------------------------------------------------------------- // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- + //---------------------------------------------------------------------------- refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; } -void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- @@ -301,7 +478,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues * } -void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, +void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- @@ -351,7 +528,8 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp //Complete transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - QuaternionOperations::fromDcm(dcmTgt,targetQuat); + double quatInertialTarget[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget); //------------------------------------------------------------------------------------- // Calculation of reference rotation rate @@ -360,15 +538,15 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6)); if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(targetQuat, savedQuaternionNadir, qDiff, 4); + VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); VectorOperations::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); - double tgtQuatVec[3] = {targetQuat[0], targetQuat[1], targetQuat[2]}, + double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(targetQuat, qDiff, sum1); + VectorOperations::cross(quatInertialTarget, qDiff, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, targetQuat[3], sum3, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; @@ -387,18 +565,26 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp } timeSavedQuaternionNadir = now; - savedQuaternionNadir[0] = targetQuat[0]; - savedQuaternionNadir[1] = targetQuat[1]; - savedQuaternionNadir[2] = targetQuat[2]; - savedQuaternionNadir[3] = targetQuat[3]; + savedQuaternionNadir[0] = quatInertialTarget[0]; + savedQuaternionNadir[1] = quatInertialTarget[1]; + savedQuaternionNadir[2] = quatInertialTarget[2]; + savedQuaternionNadir[3] = quatInertialTarget[3]; + + // Transform in system relative to satellite frame + double quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { for (int i = 0; i < 4; i++) { - targetQuat[i] = acsParameters.inertialModeControllerParameters.refQuatInertial[i]; + targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i]; } for (int i = 0; i < 3; i++) { - refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRateInertial[i]; + refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 80ef6cf4..ba43f395 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -23,18 +23,20 @@ public: void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); // Function to get the target quaternion and refence rotation rate from gps position and position of the ground station + void targetQuatPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground station - void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, + void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing - void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + void quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); - void quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, + void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, double targetQuat[4], double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from parameters for inertial pointing From 2b445369fd0a66fa5f37ac7b236630817d4b9015 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Mon, 5 Dec 2022 21:31:52 +0100 Subject: [PATCH 009/119] acsParameters in kalman filter added --- mission/controller/acs/AcsParameters.h | 10 +++++----- mission/controller/acs/MultiplicativeKalmanFilter.cpp | 8 +++----- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 24018ad9..7fe33814 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -872,8 +872,8 @@ class AcsParameters /*: public HasParametersIF*/ { struct SunModelParameters { float domega = 36000.771; - float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee - float m_0 = 357.5256; // coefficients for mean anomaly + float omega_0 = 280.46 * M_PI / 180.; //282.94 * M_PI / 180.; // RAAN plus argument of perigee + float m_0 = 357.5277; //357.5256; // coefficients for mean anomaly float dm = 35999.049; // coefficients for mean anomaly float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis float e1 = 0.74508 * M_PI / 180.; @@ -886,10 +886,10 @@ class AcsParameters /*: public HasParametersIF*/ { double sensorNoiseSTR = 0.1 * M_PI / 180; double sensorNoiseSS = 8 * M_PI / 180; double sensorNoiseMAG = 4 * M_PI / 180; - double sensorNoiseRMU[3]; + double sensorNoiseGYR = 0.1 * M_PI / 180; - double sensorNoiseArwRmu; // Angular Random Walk - double sensorNoiseBsRMU; // Bias Stability + double sensorNoiseArwGYR = 3*0.0043*M_PI / sqrt(10) / 180; // Angular Random Walk + double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability } kalmanFilterParameters; struct MagnetorquesParameter { diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5f7facd2..3d97c880 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -51,7 +51,7 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool * sigmaSun = kalmanFilterParameters->sensorNoiseSS; sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - sigmaGyro = 0.1*3.141/180; // acs parameters + sigmaGyro = kalmanFilterParameters->sensorNoiseGYR; double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0}, normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0}; @@ -980,10 +980,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( /* ----------- PROPAGATION ----------*/ - //double sigmaU = kalmanFilterParameters->sensorNoiseBsRMU; - //double sigmaV = kalmanFilterParameters->sensorNoiseArwRmu; - double sigmaU = 3*3.141/180/3600; - double sigmaV = 3*0.0043*3.141/sqrt(10)/180; + double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; + double sigmaV = 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}}; From 72b503bb8f5352dcc2065e0ca04787708c332135 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 6 Dec 2022 09:21:43 +0100 Subject: [PATCH 010/119] fixed calculation of sun vectore model --- mission/controller/acs/AcsParameters.h | 7 ++++--- mission/controller/acs/SensorProcessing.cpp | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 828e4495..f7aa2b37 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -857,12 +857,13 @@ class AcsParameters /*: public HasParametersIF*/ { struct SunModelParameters { enum UseSunModel { NO = 0, YES = 3 }; uint8_t useSunModel; - float domega = 36000.771; - float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee + float domega = 36000.77005361; // 36000.771; + float omega_0 = + 280.4606184 * M_PI / 180.; // 282.94 * M_PI / 180.; // RAAN plus argument of perigee float m_0 = 357.5256; // coefficients for mean anomaly float dm = 35999.049; // coefficients for mean anomaly float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis - float e1 = 0.74508 * M_PI / 180.; + float e1 = 0.013 * M_PI / 180.; // 0.74508 * M_PI / 180.; float p1 = 6892. / 3600. * M_PI / 180.; // some parameter float p2 = 72. / 3600. * M_PI / 180.; // some parameter diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 4635e673..c02398ce 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -397,10 +397,10 @@ void SensorProcessing::processSus( // Julean Centuries double sunIjkModel[3] = {0.0, 0.0, 0.0}; - double JC2000 = JD2000 / 36525; + double JC2000 = JD2000 / 36525.; double meanLongitude = - (sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180; + sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.; double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.; double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) + From fac2fc4971b3dfbea6d9bdc2724ce255597e73dc Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 8 Dec 2022 09:52:42 +0100 Subject: [PATCH 011/119] - fixed IGRF-13 model - fixed GST calculation - fixed conversion ECI - ToDo: remove debug output, check normalization --- mission/controller/acs/Igrf13Model.cpp | 128 ++++++++++++++++++------- mission/controller/acs/Igrf13Model.h | 28 +++++- 2 files changed, 119 insertions(+), 37 deletions(-) diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index fcd95b68..ec1b3e08 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -1,22 +1,19 @@ -/* - * Igrf13Model.cpp - * - * Created on: 10 Mar 2022 - * Author: Robin Marquardt - */ - #include "Igrf13Model.h" -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include +#include + +#include #include "util/MathOperations.h" +using namespace Math; + Igrf13Model::Igrf13Model() {} Igrf13Model::~Igrf13Model() {} @@ -25,7 +22,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, double* magFieldModelInertial) { double phi = longitude, theta = gcLatitude; // geocentric /* Here is the co-latitude needed*/ - theta -= 90 * Math::PI / 180; + theta -= 90 * PI / 180; theta *= (-1); double rE = 6371200.0; // radius earth [m] @@ -43,7 +40,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, /* Calculation of Legendre Polynoms (normalised) */ if (n == m) { P2 = sin(theta) * P11; - dP2 = sin(theta) * dP11 - cos(theta) * P11; + dP2 = sin(theta) * dP11 + cos(theta) * P11; P11 = P2; P10 = P11; P20 = 0; @@ -70,11 +67,11 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[0] += pow(rE / (altitude + rE), (n + 2)) * (n + 1) * ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2); - /* gradient of scalar potential towards phi */ + /* gradient of scalar potential towards theta */ magFieldModel[1] += pow(rE / (altitude + rE), (n + 2)) * ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2); - /* gradient of scalar potential towards theta */ + /* gradient of scalar potential towards phi */ magFieldModel[2] += pow(rE / (altitude + rE), (n + 2)) * ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m); @@ -85,31 +82,51 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[1] *= -1; magFieldModel[2] *= (-1 / sin(theta)); - /* Next step: transform into inertial KOS (IJK)*/ + // std::cout << " X=" << -magFieldModel[1] << " Y=" << magFieldModel[2] + // << " Z=" << -magFieldModel[0] << std::endl; + + /* Next step: transform into inertial RF (IJK)*/ // Julean Centuries - double JD2000Floor = 0; + // double JD2000Floor = 0; + // double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + // JD2000Floor = floor(JD2000); + // double JC2000 = JD2000Floor / 36525.; + double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); - JD2000Floor = floor(JD2000); - double JC2000 = JD2000Floor / 36525; + double UT1 = JD2000 / 36525.; - double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) - - 0.000000026 * pow(JC2000, 3); // greenwich sidereal time - gst *= PI / 180; // convert to radians - double sec = - (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ? - double omega0 = 0.00007292115; // mean angular velocity earth [rad/s] - gst += omega0 * sec; + double gst = + 280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3); + gst = std::fmod(gst, 360.); + gst *= PI / 180.; + // std::cout << " GMST=" << gst * 180. / Math::PI << std::endl; + // double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) - + // 0.000000026 * pow(JC2000, 3); // greenwich sidereal time + // gst *= PI / 180; // convert to radians + // double sec = + // (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ? + // double omega0 = 0.00007292115; // mean angular velocity earth [rad/s] + // gst += omega0 * sec; + // std::cout << " GMST=" << gst * 180. / Math::PI << std::endl; double lst = gst + longitude; // local sidereal time [rad] + // std::cout << " LMST=" << lst * 180. / Math::PI << std::endl; - magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + - magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst); - magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + - magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst); - magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst); + magFieldModelInertial[0] = + (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) - + magFieldModel[2] * sin(lst); + magFieldModelInertial[1] = + (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) + + magFieldModel[2] * cos(lst); + magFieldModelInertial[2] = + magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude); double normVecMagFieldInert[3] = {0, 0, 0}; VectorOperations::normalize(magFieldModelInertial, normVecMagFieldInert, 3); + + magFieldModel[0] = 0; + magFieldModel[1] = 0; + magFieldModel[2] = 0; } void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { @@ -123,3 +140,50 @@ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { } } } + +void Igrf13Model::schmidtNormalization() { + double kronDelta = 0; + schmidtFactors[0][0] = 1; + // for (int n = 1; n <= igrfOrder; n++) { + // if (n == 1) { + // schmidtFactors[0][n - 1] = 1; + // } else { + // schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n; + // } + // + // for (int m = 1; m <= igrfOrder; m++) { + // if (m == 1) { + // kronDelta = 1; + // } else { + // kronDelta = 0; + // } + // schmidtFactors[m][n - 1] = + // schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m)); + // } + // } + + for (int n = 1; n <= igrfOrder; n++) { + for (int m = 0; m <= n; m++) { + if (m > 1) { + schmidtFactors[m][n - 1] = schmidtFactors[m - 1][n - 1] * pow((n - m + 1) / (n + m), .5); + } else if (m > 0) { + schmidtFactors[m][n - 1] = + schmidtFactors[m - 1][n - 1] * pow(2 * (n - m + 1) / (n + m), .5); + } else if (n == 1) { + schmidtFactors[m][n - 1] = 1; + } else { + schmidtFactors[m][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / (n); + } + } + } + + for (int i = 0; i <= igrfOrder; i++) { + for (int j = 0; j <= (igrfOrder - 1); j++) { + coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j]; + coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j]; + + svG[i][j] = schmidtFactors[i][j] * svG[i][j]; + svH[i][j] = schmidtFactors[i][j] * svH[i][j]; + } + } +} diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index aa7e8b73..7abe97cc 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -43,14 +43,15 @@ class Igrf13Model /*:public HasParametersIF*/ { * - timeOfMagMeasurement: time of actual measurement [s] * * Outputs: - * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ + * - magFieldModelInertial: Magnetic Field Vector in IJK RF [nT]*/ // Coefficient wary over year, could be updated sometimes. void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) double magFieldModel[3]; + void schmidtNormalization(); private: - const double coeffG[14][13] = { + double coeffG[14][13] = { {-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1}, {-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9}, {0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5}, @@ -66,7 +67,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5}, {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.4}}; // [m][n] in nT - const double coeffH[14][13] = { + double coeffH[14][13] = { {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}, {4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9}, {0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6}, @@ -82,7 +83,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4}, {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.6}}; // [m][n] in nT - const double svG[14][13] = { + double svG[14][13] = { {5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, @@ -98,7 +99,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT - const double svH[14][13] = { + double svH[14][13] = { {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}, {-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0}, @@ -113,6 +114,23 @@ class Igrf13Model /*:public HasParametersIF*/ { {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, 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, 0.0, 0.0, 0.0}}; // [m][n] in nT + + double schmidtFactors[14][13] = {{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, 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, 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, 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, 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, 0, 0}}; + ; + bool schmidtNorm = false; double updatedG[14][13]; double updatedH[14][13]; From e9dd0f8f6d943f477f6624b0fd9b32cc1ce06705 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 11:51:56 +0100 Subject: [PATCH 012/119] reverted to initial smidt normalization --- mission/controller/acs/Igrf13Model.cpp | 54 +++++++++++++------------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index ec1b3e08..960ab462 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -144,39 +144,39 @@ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { void Igrf13Model::schmidtNormalization() { double kronDelta = 0; schmidtFactors[0][0] = 1; - // for (int n = 1; n <= igrfOrder; n++) { - // if (n == 1) { - // schmidtFactors[0][n - 1] = 1; - // } else { - // schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n; - // } - // - // for (int m = 1; m <= igrfOrder; m++) { - // if (m == 1) { - // kronDelta = 1; - // } else { - // kronDelta = 0; - // } - // schmidtFactors[m][n - 1] = - // schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m)); - // } - // } - for (int n = 1; n <= igrfOrder; n++) { - for (int m = 0; m <= n; m++) { - if (m > 1) { - schmidtFactors[m][n - 1] = schmidtFactors[m - 1][n - 1] * pow((n - m + 1) / (n + m), .5); - } else if (m > 0) { - schmidtFactors[m][n - 1] = - schmidtFactors[m - 1][n - 1] * pow(2 * (n - m + 1) / (n + m), .5); - } else if (n == 1) { - schmidtFactors[m][n - 1] = 1; + if (n == 1) { + schmidtFactors[0][n - 1] = 1; + } else { + schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n; + } + for (int m = 1; m <= igrfOrder; m++) { + if (m == 1) { + kronDelta = 1; } else { - schmidtFactors[m][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / (n); + kronDelta = 0; } + schmidtFactors[m][n - 1] = + schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m)); } } + // for (int n = 1; n <= igrfOrder; n++) { + // for (int m = 0; m <= n; m++) { + // if (m > 1) { + // schmidtFactors[m][n - 1] = schmidtFactors[m - 1][n - 1] * pow((n - m + 1) / (n + m), + // .5); + // } else if (m > 0) { + // schmidtFactors[m][n - 1] = + // schmidtFactors[m - 1][n - 1] * pow(2 * (n - m + 1) / (n + m), .5); + // } else if (n == 1) { + // schmidtFactors[m][n - 1] = 1; + // } else { + // schmidtFactors[m][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / (n); + // } + // } + // } + for (int i = 0; i <= igrfOrder; i++) { for (int j = 0; j <= (igrfOrder - 1); j++) { coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j]; From f35c42aeadab0b5119f5fe03e25a7b8f6dc1b382 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 11:52:46 +0100 Subject: [PATCH 013/119] corrected calculations of Q and Phi --- .../acs/MultiplicativeKalmanFilter.cpp | 300 ++++++++---------- 1 file changed, 139 insertions(+), 161 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 199440d5..566ef4ca 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -14,6 +14,8 @@ #include #include +#include + #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" @@ -932,6 +934,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]}; double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]}; + sif::debug << "errQ=[" << errStateQuat[0] << "," << errStateQuat[1] << "," << errStateQuat[2] + << "]" << std::endl; + sif::debug << "errW_bias=[" << errStateGyroBias[0] << "," << errStateGyroBias[1] << "," + << errStateGyroBias[2] << "]" << std::endl; // State Vector Elements double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, @@ -968,179 +974,150 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( 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}}; double rotRateEst[3] = {0, 0, 0}; + sif::debug << "MEKF rateGYR= " << rateGYRs_[0] << " " << rateGYRs_[1] << " " << rateGYRs_[2] + << std::endl; + sif::debug << "MEKF bias= " << updatedGyroBias[0] << " " << updatedGyroBias[1] << " " + << updatedGyroBias[2] << std::endl; VectorOperations::subtract(rateGYRs_, updatedGyroBias, rotRateEst, 3); double normRotEst = VectorOperations::norm(rotRateEst, 3); double crossRotEst[3][3] = {{0, -rotRateEst[2], rotRateEst[1]}, {rotRateEst[2], 0, -rotRateEst[0]}, {-rotRateEst[1], rotRateEst[0], 0}}; + // ToDo // Discrete Process Noise Covariance Q double discProcessNoiseCov[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 covQ1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - covQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - transCovQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - if (normRotEst * sampleTime < 3.141 / 10) { - double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 3); - MatrixOperations::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); - double fact2 = -(0.5 * pow(sampleTime, 2) * pow(sigmaU, 2)); - MatrixOperations::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3); - MatrixOperations::transpose(*covQ2, *transCovQ2, 3); - double fact3 = sampleTime * pow(sigmaU, 2); - MatrixOperations::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3); + double covQ11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + 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); + MatrixOperations::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3); - discProcessNoiseCov[0][0] = covQ1[0][0]; - discProcessNoiseCov[0][1] = covQ1[0][1]; - discProcessNoiseCov[0][2] = covQ1[0][2]; - discProcessNoiseCov[0][3] = covQ2[0][0]; - discProcessNoiseCov[0][4] = covQ2[0][1]; - discProcessNoiseCov[0][5] = covQ2[0][2]; - discProcessNoiseCov[1][0] = covQ1[1][0]; - discProcessNoiseCov[1][1] = covQ1[1][1]; - discProcessNoiseCov[1][2] = covQ1[1][2]; - discProcessNoiseCov[1][3] = covQ2[1][0]; - discProcessNoiseCov[1][4] = covQ2[1][1]; - discProcessNoiseCov[1][5] = covQ2[1][2]; - discProcessNoiseCov[2][0] = covQ1[2][0]; - discProcessNoiseCov[2][1] = covQ1[2][1]; - discProcessNoiseCov[2][2] = covQ1[2][2]; - discProcessNoiseCov[2][3] = covQ2[2][0]; - discProcessNoiseCov[2][4] = covQ2[2][1]; - discProcessNoiseCov[2][5] = covQ2[2][2]; - discProcessNoiseCov[3][0] = transCovQ2[0][0]; - discProcessNoiseCov[3][1] = transCovQ2[0][1]; - discProcessNoiseCov[3][2] = transCovQ2[0][2]; - discProcessNoiseCov[3][3] = covQ3[0][0]; - discProcessNoiseCov[3][4] = covQ3[0][1]; - discProcessNoiseCov[3][5] = covQ3[0][2]; - discProcessNoiseCov[4][0] = transCovQ2[1][0]; - discProcessNoiseCov[4][1] = transCovQ2[1][1]; - discProcessNoiseCov[4][2] = transCovQ2[1][2]; - discProcessNoiseCov[4][3] = covQ3[1][0]; - discProcessNoiseCov[4][4] = covQ3[1][1]; - discProcessNoiseCov[4][5] = covQ3[1][2]; - discProcessNoiseCov[5][0] = transCovQ2[2][0]; - discProcessNoiseCov[5][1] = transCovQ2[2][1]; - discProcessNoiseCov[5][2] = transCovQ2[2][2]; - discProcessNoiseCov[5][3] = covQ3[2][0]; - discProcessNoiseCov[5][4] = covQ3[2][1]; - discProcessNoiseCov[5][5] = covQ3[2][2]; + double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2)); + MatrixOperations::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3); + std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double)); + + double fact22 = pow(sigmaU, 2) * sampleTime; + MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); } else { - // double fact1 = sampleTime*pow(sigmaV,2); - double covQ11[3][3], covQ12[3][3], covQ13[3][3]; - // MatrixOperations::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3); - double fact1 = (2 * normRotEst + sampleTime - 2 * sin(normRotEst * sampleTime) - - pow(normRotEst, 3) / 3 * pow(sampleTime, 3)) / - pow(normRotEst, 5); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3); - MatrixOperations::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3); - double fact2 = pow(sampleTime, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3); - MatrixOperations::subtract(*covQ12, *covQ11, *covQ11, 3, 3); - double fact3 = sampleTime * pow(sigmaV, 2); - MatrixOperations::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3); - MatrixOperations::add(*covQ13, *covQ11, *covQ1, 3, 3); + double fact22 = pow(sigmaU, 2) * sampleTime; + MatrixOperations::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3); - double covQ21[3][3], covQ22[3][3], covQ23[3][3]; - double fact4 = - (0.5 * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) / + 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)); + MatrixOperations::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3); + double fact12_1 = 1. / 2. * pow(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) / pow(normRotEst, 4); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3); - MatrixOperations::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3); - double fact5 = 0.5 * pow(sampleTime, 2); - MatrixOperations::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3); - MatrixOperations::add(*covQ22, *covQ21, *covQ21, 3, 3); - double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3); - MatrixOperations::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3); - MatrixOperations::subtract(*covQ23, *covQ21, *covQ21, 3, 3); - double fact7 = pow(sigmaU, 2); - MatrixOperations::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3); + MatrixOperations::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3); + MatrixOperations::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3); + MatrixOperations::subtract(*covQ12_0, *covQ12_1, *covQ12_01, 3, 3); + MatrixOperations::subtract(*covQ12_01, *covQ12_2, *covQ12, 3, 3); + MatrixOperations::multiplyScalar(*covQ12, pow(sigmaU, 2), *covQ12, 3, 3); + MatrixOperations::transpose(*covQ12, *covQ12trans, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3); - - discProcessNoiseCov[0][0] = covQ1[0][0]; - discProcessNoiseCov[0][1] = covQ1[0][1]; - discProcessNoiseCov[0][2] = covQ1[0][2]; - discProcessNoiseCov[0][3] = covQ2[0][0]; - discProcessNoiseCov[0][4] = covQ2[0][1]; - discProcessNoiseCov[0][5] = covQ2[0][2]; - discProcessNoiseCov[1][0] = covQ1[1][0]; - discProcessNoiseCov[1][1] = covQ1[1][1]; - discProcessNoiseCov[1][2] = covQ1[1][2]; - discProcessNoiseCov[1][3] = covQ2[1][0]; - discProcessNoiseCov[1][4] = covQ2[1][1]; - discProcessNoiseCov[1][5] = covQ2[1][2]; - discProcessNoiseCov[2][0] = covQ1[2][0]; - discProcessNoiseCov[2][1] = covQ1[2][1]; - discProcessNoiseCov[2][2] = covQ1[2][2]; - discProcessNoiseCov[2][3] = covQ2[2][0]; - discProcessNoiseCov[2][4] = covQ2[2][1]; - discProcessNoiseCov[2][5] = covQ2[2][2]; - discProcessNoiseCov[3][0] = covQ2[0][0]; - discProcessNoiseCov[3][1] = covQ2[0][1]; - discProcessNoiseCov[3][2] = covQ2[0][2]; - discProcessNoiseCov[3][3] = covQ3[0][0]; - discProcessNoiseCov[3][4] = covQ3[0][1]; - discProcessNoiseCov[3][5] = covQ3[0][2]; - discProcessNoiseCov[4][0] = covQ2[1][0]; - discProcessNoiseCov[4][1] = covQ2[1][1]; - discProcessNoiseCov[4][2] = covQ2[1][2]; - discProcessNoiseCov[4][3] = covQ3[1][0]; - discProcessNoiseCov[4][4] = covQ3[1][1]; - discProcessNoiseCov[4][5] = covQ3[1][2]; - discProcessNoiseCov[5][0] = covQ2[2][0]; - discProcessNoiseCov[5][1] = covQ2[2][1]; - discProcessNoiseCov[5][2] = covQ2[2][2]; - discProcessNoiseCov[5][3] = covQ3[2][0]; - discProcessNoiseCov[5][4] = covQ3[2][1]; - discProcessNoiseCov[5][5] = covQ3[2][2]; + double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3]; + double fact11_0 = pow(sigmaV, 2) * sampleTime; + MatrixOperations::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3); + double fact11_1 = 1. / 3. * pow(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); + 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); + MatrixOperations::multiplyScalar(*covQ11_12, pow(sigmaU, 2), *covQ11_12, 3, 3); + MatrixOperations::add(*covQ11_0, *covQ11_12, *covQ11, 3, 3); } + discProcessNoiseCov[0][0] = covQ11[0][0]; + discProcessNoiseCov[0][1] = covQ11[0][1]; + discProcessNoiseCov[0][2] = covQ11[0][2]; + discProcessNoiseCov[0][3] = covQ12[0][0]; + discProcessNoiseCov[0][4] = covQ12[0][1]; + discProcessNoiseCov[0][5] = covQ12[0][2]; + discProcessNoiseCov[1][0] = covQ11[1][0]; + discProcessNoiseCov[1][1] = covQ11[1][1]; + discProcessNoiseCov[1][2] = covQ11[1][2]; + discProcessNoiseCov[1][3] = covQ12[1][0]; + discProcessNoiseCov[1][4] = covQ12[1][1]; + discProcessNoiseCov[1][5] = covQ12[1][2]; + discProcessNoiseCov[2][0] = covQ11[2][0]; + discProcessNoiseCov[2][1] = covQ11[2][1]; + discProcessNoiseCov[2][2] = covQ11[2][2]; + discProcessNoiseCov[2][3] = covQ12[2][0]; + discProcessNoiseCov[2][4] = covQ12[2][1]; + discProcessNoiseCov[2][5] = covQ12[2][2]; + discProcessNoiseCov[3][0] = covQ12trans[0][0]; + discProcessNoiseCov[3][1] = covQ12trans[0][1]; + discProcessNoiseCov[3][2] = covQ12trans[0][2]; + discProcessNoiseCov[3][3] = covQ22[0][0]; + discProcessNoiseCov[3][4] = covQ22[0][1]; + discProcessNoiseCov[3][5] = covQ22[0][2]; + discProcessNoiseCov[4][0] = covQ12trans[1][0]; + discProcessNoiseCov[4][1] = covQ12trans[1][1]; + discProcessNoiseCov[4][2] = covQ12trans[1][2]; + discProcessNoiseCov[4][3] = covQ22[1][0]; + discProcessNoiseCov[4][4] = covQ22[1][1]; + discProcessNoiseCov[4][5] = covQ22[1][2]; + discProcessNoiseCov[5][0] = covQ12trans[2][0]; + discProcessNoiseCov[5][1] = covQ12trans[2][1]; + discProcessNoiseCov[5][2] = covQ12trans[2][2]; + discProcessNoiseCov[5][3] = covQ22[2][0]; + discProcessNoiseCov[5][4] = covQ22[2][1]; + discProcessNoiseCov[5][5] = covQ22[2][2]; + // ToDo // State Transition Matrix phi - double phi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, - phi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, 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[3][3], phi12[3][3]; - double fact1 = sin(normRotEst * sampleTime); - MatrixOperations::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3); - double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3); - MatrixOperations::multiplyScalar(*phi12, fact2, *phi12, 3, 3); - MatrixOperations::subtract(*identityMatrix3, *phi11, *phi11, 3, 3); - MatrixOperations::add(*phi11, *phi12, *phi1, 3, 3); + double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3]; + double fact11_1 = sin(normRotEst * sampleTime) / normRotEst; + MatrixOperations::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3); + double fact11_2 = (1 - cos(normRotEst * 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); + MatrixOperations::add(*phi11_01, *phi11_2, *phi11, 3, 3); - double phi21[3][3], phi22[3][3]; - MatrixOperations::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3); - MatrixOperations::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3); - MatrixOperations::subtract(*phi21, *phi22, *phi21, 3, 3); - double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3)); - MatrixOperations::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3); - MatrixOperations::multiplyScalar(*phi22, fact3, *phi22, 3, 3); - MatrixOperations::subtract(*phi21, *phi22, *phi2, 3, 3); + 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::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); + MatrixOperations::subtract(*phi12_01, *phi12_2, *phi12, 3, 3); - phi[0][0] = phi1[0][0]; - phi[0][1] = phi1[0][1]; - phi[0][2] = phi1[0][2]; - phi[0][3] = phi2[0][0]; - phi[0][4] = phi2[0][1]; - phi[0][5] = phi2[0][2]; - phi[1][0] = phi1[1][0]; - phi[1][1] = phi1[1][1]; - phi[1][2] = phi1[1][2]; - phi[1][3] = phi2[1][0]; - phi[1][4] = phi2[1][1]; - phi[1][5] = phi2[1][2]; - phi[2][0] = phi1[2][0]; - phi[2][1] = phi1[2][1]; - phi[2][2] = phi1[2][2]; - phi[2][3] = phi2[2][0]; - phi[2][4] = phi2[2][1]; - phi[2][5] = phi2[2][2]; + phi[0][0] = phi11[0][0]; + phi[0][1] = phi11[0][1]; + phi[0][2] = phi11[0][2]; + phi[0][3] = phi12[0][0]; + phi[0][4] = phi12[0][1]; + phi[0][5] = phi12[0][2]; + phi[1][0] = phi11[1][0]; + phi[1][1] = phi11[1][1]; + phi[1][2] = phi11[1][2]; + phi[1][3] = phi12[1][0]; + phi[1][4] = phi12[1][1]; + phi[1][5] = phi12[1][2]; + phi[2][0] = phi11[2][0]; + phi[2][1] = phi11[2][1]; + phi[2][2] = phi11[2][2]; + phi[2][3] = phi12[2][0]; + phi[2][4] = phi12[2][1]; + phi[2][5] = phi12[2][2]; // Propagated Quaternion - double rotSin[3] = {0, 0, 0}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + 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; VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); @@ -1148,25 +1125,26 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MathOperations::skewMatrix(rotSin, *skewSin); - MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3); - MatrixOperations::subtract(*omega1, *skewSin, *omega1, 3, 3); - double omega[4][4] = {{omega1[0][0], omega1[0][1], omega1[0][2], rotSin[0]}, - {omega1[1][0], omega1[1][1], omega1[1][2], rotSin[1]}, - {omega1[2][0], omega1[2][1], omega1[2][2], rotSin[2]}, + MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3); + double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::subtract(*rotCosMat, *skewSin, *subMatUL, 3, 3); + double omega[4][4] = {{subMatUL[0][0], subMatUL[0][1], subMatUL[0][2], rotSin[0]}, + {subMatUL[1][0], subMatUL[1][1], subMatUL[1][2], rotSin[1]}, + {subMatUL[2][0], subMatUL[2][1], subMatUL[2][2], rotSin[2]}, {-rotSin[0], -rotSin[1], -rotSin[2], rotCos}}; MatrixOperations::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1); // Update Covariance Matrix - double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6]; + double cov0[6][6], cov1[6][6], transPhi[6][6], transDiscTimeMatrix[6][6]; + MatrixOperations::transpose(*phi, *transPhi, 6); + MatrixOperations::multiply(*covMatPlus, *transPhi, *cov0, 6, 6, 6); + MatrixOperations::multiply(*phi, *cov0, *cov0, 6, 6, 6); + MatrixOperations::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6); MatrixOperations::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6); MatrixOperations::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6); - MatrixOperations::transpose(*phi, *transPhi, 6); - MatrixOperations::multiply(*covMatPlus, *transPhi, *cov2, 6, 6, 6); - MatrixOperations::multiply(*phi, *cov2, *cov2, 6, 6, 6); - - MatrixOperations::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6); + MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); validMekf = true; // Discrete Time Step From 5fe3ac09a5b312f8619f768b510749e38c6b86f6 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 14:30:42 +0100 Subject: [PATCH 014/119] Igrf13Model cleanup --- mission/controller/acs/Igrf13Model.cpp | 37 -------------------------- mission/controller/acs/Igrf13Model.h | 1 - 2 files changed, 38 deletions(-) diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index 960ab462..63992010 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -82,16 +82,6 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[1] *= -1; magFieldModel[2] *= (-1 / sin(theta)); - // std::cout << " X=" << -magFieldModel[1] << " Y=" << magFieldModel[2] - // << " Z=" << -magFieldModel[0] << std::endl; - - /* Next step: transform into inertial RF (IJK)*/ - // Julean Centuries - // double JD2000Floor = 0; - // double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); - // JD2000Floor = floor(JD2000); - // double JC2000 = JD2000Floor / 36525.; - double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); double UT1 = JD2000 / 36525.; @@ -99,18 +89,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, 280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3); gst = std::fmod(gst, 360.); gst *= PI / 180.; - // std::cout << " GMST=" << gst * 180. / Math::PI << std::endl; - - // double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) - - // 0.000000026 * pow(JC2000, 3); // greenwich sidereal time - // gst *= PI / 180; // convert to radians - // double sec = - // (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ? - // double omega0 = 0.00007292115; // mean angular velocity earth [rad/s] - // gst += omega0 * sec; - // std::cout << " GMST=" << gst * 180. / Math::PI << std::endl; double lst = gst + longitude; // local sidereal time [rad] - // std::cout << " LMST=" << lst * 180. / Math::PI << std::endl; magFieldModelInertial[0] = (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) - @@ -161,22 +140,6 @@ void Igrf13Model::schmidtNormalization() { } } - // for (int n = 1; n <= igrfOrder; n++) { - // for (int m = 0; m <= n; m++) { - // if (m > 1) { - // schmidtFactors[m][n - 1] = schmidtFactors[m - 1][n - 1] * pow((n - m + 1) / (n + m), - // .5); - // } else if (m > 0) { - // schmidtFactors[m][n - 1] = - // schmidtFactors[m - 1][n - 1] * pow(2 * (n - m + 1) / (n + m), .5); - // } else if (n == 1) { - // schmidtFactors[m][n - 1] = 1; - // } else { - // schmidtFactors[m][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / (n); - // } - // } - // } - for (int i = 0; i <= igrfOrder; i++) { for (int j = 0; j <= (igrfOrder - 1); j++) { coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j]; diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index 7abe97cc..f5cded71 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -130,7 +130,6 @@ class Igrf13Model /*:public HasParametersIF*/ { {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}}; ; - bool schmidtNorm = false; double updatedG[14][13]; double updatedH[14][13]; From 46945a8674d3c270b61f2d6cf6dcf12302058a1c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 14:45:20 +0100 Subject: [PATCH 015/119] MEKF cleanup --- .../acs/MultiplicativeKalmanFilter.cpp | 35 +++---------------- 1 file changed, 4 insertions(+), 31 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 566ef4ca..14c2a2ec 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1,10 +1,3 @@ -/* - * MultiplicativeKalmanFilter.cpp - * - * Created on: 4 Feb 2022 - * Author: rooob - */ - #include "MultiplicativeKalmanFilter.h" #include @@ -31,7 +24,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { inertiaEIVE = &(acsParameters_->inertiaEIVE); - kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ + kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); } void MultiplicativeKalmanFilter::reset() {} @@ -43,14 +36,11 @@ void MultiplicativeKalmanFilter::init( // check for valid mag/sun if (validMagField_ && validSS && validSSModel && validMagModel) { validInit = true; - // AcsParameters mekfEstParams; - // loadAcsParameters(&mekfEstParams); // QUEST ALGO ----------------------------------------------------------------------- double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; sigmaSun = kalmanFilterParameters->sensorNoiseSS; sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - - sigmaGyro = 0.1 * 3.141 / 180; // acs parameters + sigmaGyro = kalmanFilterParameters->sensorNoiseGYR; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -138,7 +128,6 @@ void MultiplicativeKalmanFilter::init( matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - /* vector*transpose(vector)*/ MatrixOperations::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3); MatrixOperations::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3); MatrixOperations::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3); @@ -201,8 +190,6 @@ void MultiplicativeKalmanFilter::init( initialCovarianceMatrix[5][3] = initGyroCov[2][0]; initialCovarianceMatrix[5][4] = initGyroCov[2][1]; initialCovarianceMatrix[5][5] = initGyroCov[2][2]; - // 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}}; } else { // no initialisation possible, no valid measurements validInit = false; @@ -216,8 +203,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) { // Check for GYR Measurements - // AcsParameters mekfEstParams; - // loadAcsParameters(&mekfEstParams); int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { { @@ -934,10 +919,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]}; double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]}; - sif::debug << "errQ=[" << errStateQuat[0] << "," << errStateQuat[1] << "," << errStateQuat[2] - << "]" << std::endl; - sif::debug << "errW_bias=[" << errStateGyroBias[0] << "," << errStateGyroBias[1] << "," - << errStateGyroBias[2] << "]" << std::endl; // State Vector Elements double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, @@ -966,25 +947,18 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( biasGYR[2] = updatedGyroBias[2]; /* ----------- PROPAGATION ----------*/ - // double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; - // double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR; - double sigmaU = 3 * 3.141 / 180 / 3600; - double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180; + double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR; + double sigmaV = 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}}; double rotRateEst[3] = {0, 0, 0}; - sif::debug << "MEKF rateGYR= " << rateGYRs_[0] << " " << rateGYRs_[1] << " " << rateGYRs_[2] - << std::endl; - sif::debug << "MEKF bias= " << updatedGyroBias[0] << " " << updatedGyroBias[1] << " " - << updatedGyroBias[2] << std::endl; VectorOperations::subtract(rateGYRs_, updatedGyroBias, rotRateEst, 3); double normRotEst = VectorOperations::norm(rotRateEst, 3); double crossRotEst[3][3] = {{0, -rotRateEst[2], rotRateEst[1]}, {rotRateEst[2], 0, -rotRateEst[0]}, {-rotRateEst[1], rotRateEst[0], 0}}; - // ToDo // Discrete Process Noise Covariance Q double discProcessNoiseCov[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}}; @@ -1072,7 +1046,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( discProcessNoiseCov[5][4] = covQ22[2][1]; discProcessNoiseCov[5][5] = covQ22[2][2]; - // ToDo // State Transition Matrix phi double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, From c6e8c40b2c2f4d5176baff1909ea358b04042f0f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 14:50:27 +0100 Subject: [PATCH 016/119] added schmidt normalization + cleanup --- mission/controller/acs/SensorProcessing.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c02398ce..6055b50d 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -139,6 +139,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const Igrf13Model igrf13; // So the line above should not be done here. Update: Can be done here as long updated coffs // stored in acsParameters ? + igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeOfMgmMeasurement); // maybe put a condition here, to only update after a full day, this // class function has around 700 steps to perform @@ -550,19 +551,18 @@ void SensorProcessing::processGyr( } } -void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, +void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude, const bool validGps, acsctrl::GpsDataProcessed *gpsDataProcessed) { // name to convert not process double gdLongitude, gcLatitude; if (validGps) { // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic - gdLongitude = gps0longitude * PI / 180; - double latitudeRad = gps0latitude * PI / 180; + gdLongitude = gpsLongitude * PI / 180; + double latitudeRad = gpsLatitude * PI / 180; double eccentricityWgs84 = 0.0818195; double factor = 1 - pow(eccentricityWgs84, 2); gcLatitude = atan(factor * tan(latitudeRad)); - // validGcLatitude = true; } { PoolReadGuard pg(gpsDataProcessed); From b49d37e15a2ed22808ec8b8b8672bc2e9a1c11a3 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 13 Dec 2022 11:51:03 +0100 Subject: [PATCH 017/119] clang --- mission/controller/AcsController.cpp | 26 +- mission/controller/acs/AcsParameters.h | 108 +++-- mission/controller/acs/ActuatorCmd.cpp | 4 +- mission/controller/acs/ActuatorCmd.h | 58 +-- mission/controller/acs/Guidance.cpp | 424 ++++++++++--------- mission/controller/acs/Guidance.h | 34 +- mission/controller/acs/Igrf13Model.h | 24 +- mission/controller/acs/SensorProcessing.cpp | 14 +- mission/controller/acs/control/Detumble.h | 13 +- mission/controller/acs/control/PtgCtrl.cpp | 106 +++-- mission/controller/acs/control/PtgCtrl.h | 5 +- mission/controller/acs/util/MathOperations.h | 336 +++++++-------- 12 files changed, 577 insertions(+), 575 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ef3c6cf4..9e400e75 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -51,7 +51,7 @@ void AcsController::performControlOperation() { case SUBMODE_DETUMBLE: performDetumble(); break; - case SUBMODE_PTG_SUN: + case SUBMODE_PTG_SUN: case SUBMODE_PTG_TARGET: case SUBMODE_PTG_NADIR: case SUBMODE_PTG_INERTIAL: @@ -248,9 +248,10 @@ void AcsController::performPointingCtrl() { &mekfData, &validMekf); double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - switch (submode) { + switch (submode) { case SUBMODE_PTG_TARGET: - guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate); + guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, + refSatRate); break; case SUBMODE_PTG_SUN: guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); @@ -261,7 +262,8 @@ void AcsController::performPointingCtrl() { case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); break; - } double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + } + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -277,16 +279,18 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { - bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - int32_t rwSpeed[4] = {(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), - (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; - ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? + int32_t rwSpeed[4] = { + (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), + (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); } double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); + actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 7315ca89..88c4ddb0 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -26,22 +26,22 @@ class AcsParameters /*: public HasParametersIF*/ { } onBoardParams; struct InertiaEIVE { - double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, - {-0.0001821456, 0.1701302, 0.0004748963}, - {-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021 + double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, + {-0.0001821456, 0.1701302, 0.0004748963}, + {-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021 // Possible inertia matrices - double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, - {-0.0001821456, 0.1701302, 0.0004748963}, - {-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021 - double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008}, - {-0.0001798426, 0.162240, 0.000475596}, - {-0.005008, 0.000475596, 0.060136}}; //19.11.2021 - double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207}, - {-0.0001836122, 0.16619787, 0.0083537}, - {-0.00501207, 0.0083537, 0.07192588}}; //19.11.2021 - double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767}, - {-0.000178376, 0.166172, -0.007403}, - {-0.005009767, -0.007403, 0.07195314}}; + double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, + {-0.0001821456, 0.1701302, 0.0004748963}, + {-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021 + double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008}, + {-0.0001798426, 0.162240, 0.000475596}, + {-0.005008, 0.000475596, 0.060136}}; // 19.11.2021 + double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207}, + {-0.0001836122, 0.16619787, 0.0083537}, + {-0.00501207, 0.0083537, 0.07192588}}; // 19.11.2021 + double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767}, + {-0.000178376, 0.166172, -0.007403}, + {-0.005009767, -0.007403, 0.07195314}}; } inertiaEIVE; struct MgmHandlingParameters { @@ -786,9 +786,9 @@ class AcsParameters /*: public HasParametersIF*/ { double rw2orientationMatrix[3][3]; double rw3orientationMatrix[3][3]; double inertiaWheel = 0.000028198; - double maxTrq = 0.0032; // 3.2 [mNm] - double stictionSpeed = 80; //RPM - double stictionReleaseSpeed = 120; //RPM + double maxTrq = 0.0032; // 3.2 [mNm] + double stictionSpeed = 80; // RPM + double stictionReleaseSpeed = 120; // RPM double stictionTorque = 0.0006; } rwHandlingParameters; @@ -796,31 +796,21 @@ class AcsParameters /*: public HasParametersIF*/ { double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, {0.0000, -0.9205, 0.0000, 0.9205}, {0.3907, 0.3907, 0.3907, 0.3907}}; -// double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, -// {0.2136, -0.3317, 1.0123}, -// {-0.8672, -0.1406, 0.1778}, -// {0.6426, 0.4794, 1.3603}}; -// where does the first pseudo inverse come frome - matlab gives result below - double pseudoInverse[4][3] = {{0.5432, 0, 0.6398}, - {0, -0.5432, 0.6398}, - {-0.5432, 0, 0.6398}, - {0, 0.5432, 0.6398}}; - double without0[4][3] = {{0, 0, 0}, - {0.5432, -0.5432, 1.2797}, - {-1.0864, 0, 0}, - {0.5432, 0.5432, 1.2797}}; - double without1[4][3] = {{0.5432, -0.5432, 1.2797}, - {0, 0, 0}, - {-0.5432, -0.5432, 1.2797}, - {0, 1.0864, 0}}; - double without2[4][3] = {{1.0864, 0, 0}, - {-0.5432, -0.5432, 1.2797}, - {0, 0, 0}, - {-0.5432, 0.5432, 1.2797}}; - double without3[4][3] = {{0.5432, 0.5432, 1.2797}, - {0, -1.0864, 0}, - {-0.5432, 0.5432, 1.2797}, - {0, 0, 0}}; + // double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, + // {0.2136, -0.3317, 1.0123}, + // {-0.8672, -0.1406, 0.1778}, + // {0.6426, 0.4794, 1.3603}}; + // where does the first pseudo inverse come frome - matlab gives result below + double pseudoInverse[4][3] = { + {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}}; + double without0[4][3] = { + {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}}; + double without1[4][3] = { + {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}}; + double without2[4][3] = { + {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; + double without3[4][3] = { + {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; } rwMatrices; @@ -828,9 +818,11 @@ class AcsParameters /*: public HasParametersIF*/ { double k_rate_mekf = 0.00059437; double k_align_mekf = 0.000056875; - double k_rate_no_mekf = 0.00059437;; - double k_align_no_mekf = 0.000056875;; - double sunMagAngleMin; // ??? + double k_rate_no_mekf = 0.00059437; + ; + double k_align_no_mekf = 0.000056875; + ; + double sunMagAngleMin; // ??? double sunTargetDir[3] = {0, 0, 1}; double satRateRef[3] = {0, 0, 0}; @@ -839,7 +831,7 @@ class AcsParameters /*: public HasParametersIF*/ { // ToDo: mutiple structs for different pointing mode controllers? struct PointingModeControllerParameters { - double refDirection[3] = {-1, 0, 0}; //Antenna + double refDirection[3] = {-1, 0, 0}; // Antenna double refRotRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 1}; bool avoidBlindStr = true; @@ -860,11 +852,12 @@ class AcsParameters /*: public HasParametersIF*/ { double omegaEarth = 0.000072921158553; - double nadirRefDirection[3] = {-1, 0, 0}; //Camera + double nadirRefDirection[3] = {-1, 0, 0}; // Camera double tgtQuatInertial[4] = {0, 0, 0, 1}; double tgtRotRateInertial[3] = {0, 0, 0}; int8_t nadirTimeElapsedMax = 10; - } pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; + } pointingModeControllerParameters, inertialModeControllerParameters, + nadirModeControllerParameters, targetModeControllerParameters; struct StrParameters { double exclusionAngle = 20 * M_PI / 180; @@ -872,7 +865,7 @@ class AcsParameters /*: public HasParametersIF*/ { } strParameters; struct GpsParameters { - double timeDiffVelocityMax = 30; //[s] + double timeDiffVelocityMax = 30; //[s] } gpsParameters; struct GroundStationParameters { @@ -883,10 +876,11 @@ class AcsParameters /*: public HasParametersIF*/ { struct SunModelParameters { float domega = 36000.771; - float omega_0 = 280.46 * M_PI / 180.; //282.94 * M_PI / 180.; // RAAN plus argument of perigee - float m_0 = 357.5277; //357.5256; // coefficients for mean anomaly - float dm = 35999.049; // coefficients for mean anomaly - float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis + float omega_0 = 280.46 * M_PI / 180.; // 282.94 * M_PI / 180.; // RAAN plus argument of + // perigee + float m_0 = 357.5277; // 357.5256; // coefficients for mean anomaly + float dm = 35999.049; // coefficients for mean anomaly + float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis float e1 = 0.74508 * M_PI / 180.; float p1 = 6892. / 3600. * M_PI / 180.; // some parameter @@ -899,8 +893,8 @@ class AcsParameters /*: public HasParametersIF*/ { double sensorNoiseMAG = 4 * M_PI / 180; double sensorNoiseGYR = 0.1 * M_PI / 180; - double sensorNoiseArwGYR = 3*0.0043*M_PI / sqrt(10) / 180; // Angular Random Walk - double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability + double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk + double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability } kalmanFilterParameters; struct MagnetorquesParameter { @@ -914,7 +908,7 @@ class AcsParameters /*: public HasParametersIF*/ { } magnetorquesParameter; struct DetumbleParameter { - uint8_t detumblecounter = 75; // 30 s + uint8_t detumblecounter = 75; // 30 s double omegaDetumbleStart = 2 * M_PI / 180; double omegaDetumbleEnd = 0.4 * M_PI / 180; double gainD = pow(10.0, -3.3); diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index f8c9b249..4b3bb28c 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -33,8 +33,8 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) { } if (maxValue > maxTrq) { - double scalingFactor = maxTrq / maxValue; - VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); + double scalingFactor = maxTrq / maxValue; + VectorOperations::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4); } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index cd61e639..918bce9b 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -6,38 +6,38 @@ #include "SensorProcessing.h" #include "SensorValues.h" -class ActuatorCmd{ -public: - ActuatorCmd(AcsParameters *acsParameters_); //Input mode ? - virtual ~ActuatorCmd(); +class ActuatorCmd { + public: + ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? + virtual ~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 - */ - void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); + /* + * @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 + */ + void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled); - /* - * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction wheels, also - * will calculate the needed revolutions per minute for the RWs, which will be given - * as Input to the RWs - * @param: rwTrqIn given torque from pointing controller - * rwTrqNS Nullspace torque - * 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, - double *rwCmdSpeed); + /* + * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction + * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given + * as Input to the RWs + * @param: rwTrqIn given torque from pointing controller + * rwTrqNS Nullspace torque + * 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, double *rwCmdSpeed); - /* - * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques - * - * @param: dipolMoment given dipol moment in spacecraft frame - * dipolMomentUnits resulting dipol moment for every unit - */ - void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits); + /* + * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques + * + * @param: dipolMoment given dipol moment in spacecraft frame + * dipolMomentUnits resulting dipol moment for every unit + */ + void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits); protected: private: diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index eeb161d0..199fce9c 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -47,9 +47,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; - double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; + double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); // Target direction in the ECEF frame @@ -174,125 +174,128 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData } } -void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for target pointing + //------------------------------------------------------------------------------------- + // Transform longitude, latitude and altitude to cartesian coordiantes (earth + // fixed/centered frame) + double groundStationCart[3] = {0, 0, 0}; - //------------------------------------------------------------------------------------- - // Calculation of target quaternion for target pointing - //------------------------------------------------------------------------------------- - // Transform longitude, latitude and altitude to cartesian coordiantes (earth - // fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; + MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, + acsParameters.groundStationParameters.longitudeGs, + acsParameters.groundStationParameters.altitudeGs, + groundStationCart); + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; + double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); - MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, - acsParameters.groundStationParameters.longitudeGs, - acsParameters.groundStationParameters.altitudeGs, - groundStationCart); - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; - double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + // Target Direction and position vector in the inertial frame + double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); + MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); - // Target Direction and position vector in the inertial frame - double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); + // negative x-Axis aligned with target (Camera/E-band transmitter position) + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - // negative x-Axis aligned with target (Camera/E-band transmitter position) - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + // Transform velocity into inertial frame + double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], + outputValues->gpsVelocity[2]}; + double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityJ, 3); - // Transform velocity into inertial frame - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; - double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityJ, 3); + // orbital normal vector + double orbitalNormalJ[3] = {0, 0, 0}; + VectorOperations::cross(posSatJ, velocityJ, orbitalNormalJ); + VectorOperations::normalize(orbitalNormalJ, orbitalNormalJ, 3); - // orbital normal vector - double orbitalNormalJ[3] = {0, 0, 0}; - VectorOperations::cross(posSatJ, velocityJ, orbitalNormalJ); - VectorOperations::normalize(orbitalNormalJ, orbitalNormalJ, 3); + // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(orbitalNormalJ, xAxis, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); - // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(orbitalNormalJ, xAxis, yAxis); - VectorOperations::normalize(yAxis, yAxis, 3); + // z-Axis completes RHS + double zAxis[3] = {0, 0, 0}; + VectorOperations::cross(xAxis, yAxis, zAxis); - // z-Axis completes RHS - double zAxis[3] = {0, 0, 0}; - VectorOperations::cross(xAxis, yAxis, zAxis); + // Complete transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; + double quatInertialTarget[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - //Complete transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget); + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double timeElapsed = + now.tv_sec + now.tv_usec * pow(10, -6) - + (timeSavedQuaternionNadir.tv_sec + + timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec, -6)); + if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { + double qDiff[4] = {0, 0, 0, 0}; + VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- - double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6)); - if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { - double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); + double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, + qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; + VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::add(sum1, sum2, sum, 3); + VectorOperations::subtract(sum, sum3, sum, 3); + double omegaRefNew[3] = {0, 0, 0}; + VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; - double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); - VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); - VectorOperations::add(sum1, sum2, sum, 3); - VectorOperations::subtract(sum, sum3, sum, 3); - double omegaRefNew[3] = {0, 0, 0}; - VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); + VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); + VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); + omegaRefSavedNadir[0] = omegaRefNew[0]; + omegaRefSavedNadir[1] = omegaRefNew[1]; + omegaRefSavedNadir[2] = omegaRefNew[2]; + } else { + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + } - VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); - VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); - omegaRefSavedNadir[0] = omegaRefNew[0]; - omegaRefSavedNadir[1] = omegaRefNew[1]; - omegaRefSavedNadir[2] = omegaRefNew[2]; - } - else { - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; - } + timeSavedQuaternionNadir = now; + savedQuaternionNadir[0] = quatInertialTarget[0]; + savedQuaternionNadir[1] = quatInertialTarget[1]; + savedQuaternionNadir[2] = quatInertialTarget[2]; + savedQuaternionNadir[3] = quatInertialTarget[3]; - timeSavedQuaternionNadir = now; - savedQuaternionNadir[0] = quatInertialTarget[0]; - savedQuaternionNadir[1] = quatInertialTarget[1]; - savedQuaternionNadir[2] = quatInertialTarget[2]; - savedQuaternionNadir[3] = quatInertialTarget[3]; - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); + // Transform in system relative to satellite frame + double quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -354,17 +357,18 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); // positive z-Axis of EIVE in direction of sun - double zAxis[3] = {0 ,0 ,0}; + double zAxis[3] = {0, 0, 0}; VectorOperations::normalize(sunDirB, zAxis, 3); // Position of the satellite in the earth/fixed frame via GPS and body // velocity double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; - double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; + double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; + double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], + outputValues->gpsVelocity[2]}; double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); @@ -387,9 +391,11 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou // Transformation matrix to Sun, no further transforamtions, reference is already // the EIVE body frame - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; double quatSun[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt,quatSun); + QuaternionOperations::fromDcm(dcmTgt, quatSun); targetQuat[0] = quatSun[0]; targetQuat[1] = quatSun[1]; @@ -404,16 +410,18 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou refSatRate[2] = 0; } -void Guidance::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing +void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, + ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], + double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; - double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, + double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; + double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); @@ -468,116 +476,118 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::Outp refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; - } -void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for Nadir pointing + //------------------------------------------------------------------------------------- + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; + double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); - //------------------------------------------------------------------------------------- - // Calculation of target quaternion for Nadir pointing - //------------------------------------------------------------------------------------- - // Position of the satellite in the earth/fixed frame via GPS - double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180; - double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - double targetDirE[3] = {0, 0, 0}; - VectorOperations::mulScalar(posSatE, -1, targetDirE, 3); + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); - // Transformation between ECEF and IJK frame - double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); - MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); - double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + // Target Direction in the body frame + double targetDirJ[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); - // Target Direction in the body frame - double targetDirJ[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); + // negative x-Axis aligned with target (Camera position) + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - // negative x-Axis aligned with target (Camera position) - double xAxis[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirJ, xAxis, 3); - VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + // z-Axis parallel to long side of picture resolution + double zAxis[3] = {0, 0, 0}; + double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], + outputValues->gpsVelocity[2]}; + double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); + MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); + VectorOperations::add(velPart1, velPart2, velocityJ, 3); + VectorOperations::cross(xAxis, velocityJ, zAxis); + VectorOperations::normalize(zAxis, zAxis, 3); - // z-Axis parallel to long side of picture resolution - double zAxis[3] = {0, 0, 0}; - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]}; - double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityJ, 3); - VectorOperations::cross(xAxis, velocityJ, zAxis); - VectorOperations::normalize(zAxis, zAxis, 3); + // y-Axis completes RHS + double yAxis[3] = {0, 0, 0}; + VectorOperations::cross(zAxis, xAxis, yAxis); - // y-Axis completes RHS - double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(zAxis, xAxis, yAxis); + // Complete transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; + double quatInertialTarget[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - //Complete transformation matrix - double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, {xAxis[2], yAxis[2], zAxis[2]}}; - double quatInertialTarget[4] = {0, 0, 0, 0}; - QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget); + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double timeElapsed = + now.tv_sec + now.tv_usec * pow(10, -6) - + (timeSavedQuaternionNadir.tv_sec + + timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec, -6)); + if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { + double qDiff[4] = {0, 0, 0, 0}; + VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- - double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6)); - if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { - double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1/timeElapsed, qDiff, 4); + double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, + qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; + VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::add(sum1, sum2, sum, 3); + VectorOperations::subtract(sum, sum3, sum, 3); + double omegaRefNew[3] = {0, 0, 0}; + VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; - double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); - VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); - VectorOperations::add(sum1, sum2, sum, 3); - VectorOperations::subtract(sum, sum3, sum, 3); - double omegaRefNew[3] = {0, 0, 0}; - VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); + VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); + VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); + omegaRefSavedNadir[0] = omegaRefNew[0]; + omegaRefSavedNadir[1] = omegaRefNew[1]; + omegaRefSavedNadir[2] = omegaRefNew[2]; + } else { + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + } - VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); - VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); - omegaRefSavedNadir[0] = omegaRefNew[0]; - omegaRefSavedNadir[1] = omegaRefNew[1]; - omegaRefSavedNadir[2] = omegaRefNew[2]; - } - else { - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; - } + timeSavedQuaternionNadir = now; + savedQuaternionNadir[0] = quatInertialTarget[0]; + savedQuaternionNadir[1] = quatInertialTarget[1]; + savedQuaternionNadir[2] = quatInertialTarget[2]; + savedQuaternionNadir[3] = quatInertialTarget[3]; - timeSavedQuaternionNadir = now; - savedQuaternionNadir[0] = quatInertialTarget[0]; - savedQuaternionNadir[1] = quatInertialTarget[1]; - savedQuaternionNadir[2] = quatInertialTarget[2]; - savedQuaternionNadir[3] = quatInertialTarget[3]; - - // Transform in system relative to satellite frame - double quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; - QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); + // Transform in system relative to satellite frame + double quatBJ[4] = {0, 0, 0, 0}; + quatBJ[0] = outputValues->quatMekfBJ[0]; + quatBJ[1] = outputValues->quatMekfBJ[1]; + quatBJ[2] = outputValues->quatMekfBJ[2]; + quatBJ[3] = outputValues->quatMekfBJ[3]; + QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { for (int i = 0; i < 4; i++) { - targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i]; + targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i]; } for (int i = 0; i < 3; i++) { - refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; + refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 234627cf..1b6bfcb6 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -21,27 +21,31 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); - // Function to get the target quaternion and refence rotation rate from gps position and position of the ground station - void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], - double refSatRate[3]); - void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + // Function to get the target quaternion and refence rotation rate from gps position and position + // of the ground station + void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]); + void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and refence rotation rate for sun pointing after ground station - void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate for sun pointing after ground + // station + void sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing - void quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate from gps position for Nadir + // pointing + void quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]); - void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]); + void quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, + double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and refence rotation rate from parameters for inertial pointing - void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); + // Function to get the target quaternion and refence rotation rate from parameters for inertial + // pointing + void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); // @note: compares target Quaternion and reference quaternion, also actual satellite rate and // desired diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index f5cded71..ea2c1044 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -114,21 +114,15 @@ class Igrf13Model /*:public HasParametersIF*/ { {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, 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, 0.0, 0.0, 0.0}}; // [m][n] in nT - - double schmidtFactors[14][13] = {{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, 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, 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, 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, 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, 0, 0}}; + + double schmidtFactors[14][13] = { + {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, 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, 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, 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, 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, 0, 0}}; ; double updatedG[14][13]; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 73d5fa81..5ce2cb77 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -574,11 +574,13 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Calculation of the satellite velocity in earth fixed frame double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}; // RADIANS OR DEGREE ? Function needs rad as input - MathOperations::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, posSatE); - if (validSavedPosSatE && (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { - VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); - double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE; - VectorOperations::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3); + MathOperations::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, + posSatE); + if (validSavedPosSatE && + (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { + VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); + double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE; + VectorOperations::mulScalar(deltaDistance, 1 / timeDiffGpsMeas, gpsVelocityE, 3); } } { @@ -600,7 +602,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong validSavedPosSatE = true; } else { - validGcLatitude = false; + validGcLatitude = false; } } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index f47ce86c..16bf82bc 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -35,15 +35,14 @@ class Detumble { ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); - ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); + 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 bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField, + const bool *magFieldValid, double *magMom); -private: - AcsParameters::DetumbleParameter* detumbleParameter; - AcsParameters::MagnetorquesParameter* magnetorquesParameter; + private: + AcsParameters::DetumbleParameter *detumbleParameter; + AcsParameters::MagnetorquesParameter *magnetorquesParameter; }; #endif /*ACS_CONTROL_DETUMBLE_H_*/ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 64666be1..9d4f8d6d 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -16,9 +16,7 @@ #include "../util/MathOperations.h" -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ - loadAcsParameters(acsParameters_); -} +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } PtgCtrl::~PtgCtrl() {} @@ -29,7 +27,8 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { rwMatrices = &(acsParameters_->rwMatrices); } -void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){ +void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate, + const double *rwPseudoInv, double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix //------------------------------------------------------------------------------------------------ @@ -84,30 +83,28 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt double pErrorSign[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(pError[i]) > 1) { - pErrorSign[i] = sign(pError[i]); - } - else { - pErrorSign[i] = pError[i]; - } - } -// Torque for quaternion error - double torqueQuat[3] = {0, 0, 0}; - MatrixOperations::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1); - VectorOperations::mulScalar(torqueQuat, -1, torqueQuat, 3); + if (abs(pError[i]) > 1) { + pErrorSign[i] = sign(pError[i]); + } else { + pErrorSign[i] = pError[i]; + } + } + // 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 - double torqueRate[3] = {0, 0, 0}; - MatrixOperations::multiply(*(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 - double torque[3] = {0, 0, 0}; - VectorOperations::add(torqueRate, torqueQuat, torque, 3); - MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); - VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); + // Torque for rate error + double torqueRate[3] = {0, 0, 0}; + MatrixOperations::multiply(*(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 + double torque[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueQuat, torque, 3); + MatrixOperations::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1); + VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, @@ -162,34 +159,31 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } -void PtgCtrl::rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, - double* torqueCommand) { - for (uint8_t i = 0; i < 4; i++) { - if (rwAvailable[i]) { - if (torqueMemory[i] != 0) { - if ((omegaRw[i] * torqueMemory[i]) - > rwHandlingParameters->stictionReleaseSpeed) { - torqueMemory[i] = 0; - } else { - torqueCommand[i] = torqueMemory[i] - * rwHandlingParameters->stictionTorque; - } - } else { - if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) - && (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { - if (omegaRw[i] < omegaMemory[i]) { - torqueMemory[i] = -1; - } else { - torqueMemory[i] = 1; - } +void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, + double *torqueCommand) { + for (uint8_t i = 0; i < 4; i++) { + if (rwAvailable[i]) { + if (torqueMemory[i] != 0) { + if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) { + torqueMemory[i] = 0; + } else { + torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque; + } + } else { + if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) && + (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) { + if (omegaRw[i] < omegaMemory[i]) { + torqueMemory[i] = -1; + } else { + torqueMemory[i] = 1; + } - torqueCommand[i] = torqueMemory[i] - * rwHandlingParameters->stictionTorque; - } - } - } else { - torqueMemory[i] = 0; - } - omegaMemory[i] = omegaRw[i]; - } - } + torqueCommand[i] = torqueMemory[i] * 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 24ccafae..d176b0c2 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -42,7 +42,7 @@ class PtgCtrl { * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void ptgLaw(const double mode, const double *qError, const double *deltaRate, - const double *rwPseudoInv, double *torqueRws); + const double *rwPseudoInv, double *torqueRws); void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, @@ -56,8 +56,7 @@ class PtgCtrl { * omegaRw current wheel speed of reaction wheels * torqueCommand modified torque after antistiction */ - void rwAntistiction(const bool* rwAvailable, const int32_t* omegaRw, - double* torqueCommand); + void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand); private: AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 90056927..56665c4d 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -64,12 +64,13 @@ class MathOperations { } } - static void convertDateToJD2000(const T1 time, T2 julianDate){ - // time = { Y, M, D, h, m,s} - // time in sec and microsec -> The Epoch (unixtime) - julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12)) - +floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440; - } + static void convertDateToJD2000(const T1 time, T2 julianDate) { + // time = { Y, M, D, h, m,s} + // time in sec and microsec -> The Epoch (unixtime) + julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) + + floor(275 * time[1] / 9) + time[2] + + (60 * time[3] + time[4] + (time(5) / 60)) / 1440; + } static T1 convertUnixToJD2000(timeval time) { // time = {{s},{us}} @@ -93,43 +94,42 @@ class MathOperations { outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2); } - static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){ - /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, - * longitude and altitude - * @param: lat geodetic latitude [rad] - * longi longitude [rad] - * alt altitude [m] - * cartesianOutput Cartesian Coordinates in ECEF (3x1) - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff - * Landis Markley and John L. Crassidis*/ - double radiusPolar = 6356752.314; - double radiusEqua = 6378137; + static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, + T2 *cartesianOutput) { + /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, + * longitude and altitude + * @param: lat geodetic latitude [rad] + * longi longitude [rad] + * alt altitude [m] + * cartesianOutput Cartesian Coordinates in ECEF (3x1) + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff + * Landis Markley and John L. Crassidis*/ + double radiusPolar = 6356752.314; + double radiusEqua = 6378137; - double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2)); - double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2)); + double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2)); + double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2)); - cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi); - cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); - cartesianOutput[2] = ((1 - pow(eccentricity,2)) * auxRadius + alt) * sin(lat); - - } - static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){ - /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame - * @param: time Current time - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * outputDotDcmEJ Derivative of transformation matrix [3][3] - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff - * Landis Markley and John L. Crassidis*/ - double JD2000Floor = 0; - double JD2000 = convertUnixToJD2000(time); - // Getting Julian Century from Day start : JD (Y,M,D,0,0,0) - JD2000Floor = floor(JD2000); - if ( ( JD2000 - JD2000Floor) < 0.5) { - JD2000Floor -= 0.5; - } - else { - JD2000Floor += 0.5; - } + cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi); + cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); + cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat); + } + static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { + /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame + * @param: time Current time + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * outputDotDcmEJ Derivative of transformation matrix [3][3] + * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff + * Landis Markley and John L. Crassidis*/ + double JD2000Floor = 0; + double JD2000 = convertUnixToJD2000(time); + // Getting Julian Century from Day start : JD (Y,M,D,0,0,0) + JD2000Floor = floor(JD2000); + if ((JD2000 - JD2000Floor) < 0.5) { + JD2000Floor -= 0.5; + } else { + JD2000Floor += 0.5; + } double JC2000 = JD2000Floor / 36525; double sec = (JD2000 - JD2000Floor) * 86400; @@ -152,150 +152,152 @@ class MathOperations { outputDcmEJ[7] = 0; outputDcmEJ[8] = 1; - // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION - double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, - {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, - {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; - double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; - double omegaEarth = 0.000072921158553; - double dotDcmEJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - MatrixOperations::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3); - MatrixOperations::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3); + // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION + double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, + {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, + {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; + double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = 0.000072921158553; + double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3); + MatrixOperations::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3); } + /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame + * give also the back the derivative of this matrix + * @param: unixTime Current time in Unix format + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * outputDotDcmEJ Derivative of transformation matrix [3][3] + * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und + Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff + * Oliver Zeile + * + https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/ + static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { + // TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 + //(initial Offset) International Atomic Time (TAI) - /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame - * give also the back the derivative of this matrix - * @param: unixTime Current time in Unix format - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * outputDotDcmEJ Derivative of transformation matrix [3][3] - * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und - Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff - * Oliver Zeile - * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/ - static void ecfToEciWithNutPre(timeval unixTime, T1 * outputDcmEJ, T1 * outputDotDcmEJ ) { + double JD2000UTC1 = convertUnixToJD2000(unixTime); -// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 (initial Offset) -// International Atomic Time (TAI) + // Julian Date / century from TT + timeval terestrialTime = unixTime; + terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; + double JD2000TT = convertUnixToJD2000(terestrialTime); + double JC2000TT = JD2000TT / 36525; - double JD2000UTC1 = convertUnixToJD2000(unixTime); + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth rotation Theta + //------------------------------------------------------------------------------------- + double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // Earth Rotation angle + double era = 0; + era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1); + // Greenwich Mean Sidereal Time + double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) - + 0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4); + double arcsecFactor = 1 * PI / (180 * 3600); + gmst2000 *= arcsecFactor; + gmst2000 += era; -// Julian Date / century from TT - timeval terestrialTime = unixTime; - terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; - double JD2000TT = convertUnixToJD2000(terestrialTime); - double JC2000TT = JD2000TT / 36525; + theta[0][0] = cos(gmst2000); + theta[0][1] = sin(gmst2000); + theta[0][2] = 0; + theta[1][0] = -sin(gmst2000); + theta[1][1] = cos(gmst2000); + theta[1][2] = 0; + theta[2][0] = 0; + theta[2][1] = 0; + theta[2][2] = 1; -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth rotation Theta -//------------------------------------------------------------------------------------- - double theta[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; -// Earth Rotation angle - double era = 0; - era = 2* PI *(0.779057273264 + 1.00273781191135448 * JD2000UTC1); -// Greenwich Mean Sidereal Time - double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT,2) - - 0.00009344 * pow(JC2000TT,3) + 0.00001882 * pow(JC2000TT,4); - double arcsecFactor = 1 * PI / (180 * 3600); - gmst2000 *= arcsecFactor; - gmst2000 += era; + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Precession P + //------------------------------------------------------------------------------------- + double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - theta[0][0] = cos(gmst2000); - theta[0][1] = sin(gmst2000); - theta[0][2] = 0; - theta[1][0] = -sin(gmst2000); - theta[1][1] = cos(gmst2000); - theta[1][2] = 0; - theta[2][0] = 0; - theta[2][1] = 0; - theta[2][2] = 1; + double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3); + double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3); + double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3); -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth Precession P -//------------------------------------------------------------------------------------- - double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + zeta *= arcsecFactor; + theta2 *= arcsecFactor; + ze *= arcsecFactor; - double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT,2) + 0.017998 * pow(JC2000TT,3); - double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT,2) - 0.041833 * pow(JC2000TT,3); - double ze = zeta + 0.79280 * pow(JC2000TT,2) + 0.000205 * pow(JC2000TT,3); + precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta); + precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta); + precession[2][0] = sin(theta2) * cos(zeta); + precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta); + precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta); + precession[2][1] = -sin(theta2) * sin(zeta); + precession[0][2] = -cos(ze) * sin(theta2); + precession[1][2] = -sin(ze) * sin(theta2); + precession[2][2] = cos(theta2); - zeta *= arcsecFactor; - theta2 *= arcsecFactor; - ze *= arcsecFactor; + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Nutation N + //------------------------------------------------------------------------------------- + double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // lunar asc node + double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + + 7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3); + Om *= arcsecFactor; + // delta psi approx + double dp = -17.2 * arcsecFactor * sin(Om); - precession[0][0]=-sin(ze)*sin(zeta)+cos(ze)*cos(theta2)*cos(zeta); - precession[1][0]=cos(ze)*sin(zeta)+sin(ze)*cos(theta2)*cos(zeta); - precession[2][0]=sin(theta2)*cos(zeta); - precession[0][1]=-sin(ze)*cos(zeta)-cos(ze)*cos(theta2)*sin(zeta); - precession[1][1]=cos(ze)*cos(zeta)-sin(ze)*cos(theta2)*sin(zeta); - precession[2][1]=-sin(theta2)*sin(zeta); - precession[0][2]=-cos(ze)*sin(theta2); - precession[1][2]=-sin(ze)*sin(theta2); - precession[2][2]=cos(theta2); + // delta eps approx + double de = 9.203 * arcsecFactor * cos(Om); -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth Nutation N -//------------------------------------------------------------------------------------- - double nutation[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; -// lunar asc node - double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + - 7.455 * pow(JC2000TT,2) + 0.008 * pow(JC2000TT,3); - Om *= arcsecFactor; -// delta psi approx - double dp = -17.2 * arcsecFactor *sin(Om); + // % true obliquity of the ecliptic eps p.71 (simplified) + double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; -// delta eps approx - double de = 9.203 * arcsecFactor *cos(Om); + nutation[0][0] = cos(dp); + nutation[1][0] = cos(e + de) * sin(dp); + nutation[2][0] = sin(e + de) * sin(dp); + nutation[0][1] = -cos(e) * sin(dp); + nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de); + nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de); + nutation[0][2] = -sin(e) * sin(dp); + nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de); + nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de); -// % true obliquity of the ecliptic eps p.71 (simplified) - double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; + //------------------------------------------------------------------------------------- + // Calculation of Derivative of rotation matrix from earth + //------------------------------------------------------------------------------------- + double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = 0.000072921158553; + MatrixOperations::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); + MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); - nutation[0][0]=cos(dp); - nutation[1][0]=cos(e+de)*sin(dp); - nutation[2][0]=sin(e+de)*sin(dp); - nutation[0][1]=-cos(e)*sin(dp); - nutation[1][1]=cos(e)*cos(e+de)*cos(dp)+sin(e)*sin(e+de); - nutation[2][1]=cos(e)*sin(e+de)*cos(dp)-sin(e)*cos(e+de); - nutation[0][2]=-sin(e)*sin(dp); - nutation[1][2]=sin(e)*cos(e+de)*cos(dp)-cos(e)*sin(e+de); - nutation[2][2]=sin(e)*sin(e+de)*cos(dp)+cos(e)*cos(e+de); + //------------------------------------------------------------------------------------- + // Calculation of transformation matrix and Derivative of transformation matrix + //------------------------------------------------------------------------------------- + double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); + MatrixOperations::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); -//------------------------------------------------------------------------------------- -// Calculation of Derivative of rotation matrix from earth -//------------------------------------------------------------------------------------- - double thetaDot[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - double dotMatrix[3][3] = {{0,1,0},{-1,0,0},{0,0,0}}; - double omegaEarth = 0.000072921158553; - MatrixOperations::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); - MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); + MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); + } + static void inverseMatrixDimThree(const T1 *matrix, T1 *output) { + int i, j; + double determinant; + double mat[3][3] = {{matrix[0], matrix[1], matrix[2]}, + {matrix[3], matrix[4], matrix[5]}, + {matrix[6], matrix[7], matrix[8]}}; -//------------------------------------------------------------------------------------- -// Calculation of transformation matrix and Derivative of transformation matrix -//------------------------------------------------------------------------------------- - double nutationPrecession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - MatrixOperations::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); - MatrixOperations::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); - - MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); - - } - static void inverseMatrixDimThree(const T1 *matrix, T1 * output){ - int i,j; - double determinant; - double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},{matrix[3], matrix[4], matrix[5]}, - {matrix[6], matrix[7], matrix[8]}}; - - for(i = 0; i < 3; i++) { - determinant = determinant + (mat[0][i] * (mat[1][(i+1)%3] * mat[2][(i+2)%3] - mat[1][(i+2)%3] * mat[2][(i+1)%3])); - } -// cout<<"\n\ndeterminant: "< Date: Wed, 14 Dec 2022 10:03:57 +0100 Subject: [PATCH 018/119] removed comments --- mission/controller/acs/AcsParameters.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 88c4ddb0..8bd10091 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -876,11 +876,11 @@ class AcsParameters /*: public HasParametersIF*/ { struct SunModelParameters { float domega = 36000.771; - float omega_0 = 280.46 * M_PI / 180.; // 282.94 * M_PI / 180.; // RAAN plus argument of + float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of // perigee - float m_0 = 357.5277; // 357.5256; // coefficients for mean anomaly - float dm = 35999.049; // coefficients for mean anomaly - float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis + float m_0 = 357.5277; // coefficients for mean anomaly + float dm = 35999.049; // coefficients for mean anomaly + float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis float e1 = 0.74508 * M_PI / 180.; float p1 = 6892. / 3600. * M_PI / 180.; // some parameter From d5677c20f7a7c437af84932afbaf4c2ba3f589b7 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 10:04:37 +0100 Subject: [PATCH 019/119] fixed gps velocity merge aftermath --- mission/controller/acs/SensorProcessing.cpp | 39 ++++++++++----------- mission/controller/acs/SensorProcessing.h | 5 +-- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 5ce2cb77..5b4267e7 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -559,29 +559,35 @@ void SensorProcessing::processGyr( } void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude, + const double gpsAltitude, const double gpsUnixSeconds, const bool validGps, + const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { // name to convert not process double gdLongitude, gcLatitude; if (validGps) { // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic - gdLongitude = gpsLongitude * PI / 180; - double latitudeRad = gpsLatitude * PI / 180; + gdLongitude = gpsLongitude * PI / 180.; + double latitudeRad = gpsLatitude * PI / 180.; double eccentricityWgs84 = 0.0818195; double factor = 1 - pow(eccentricityWgs84, 2); gcLatitude = atan(factor * tan(latitudeRad)); // Calculation of the satellite velocity in earth fixed frame - double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}; - // RADIANS OR DEGREE ? Function needs rad as input - MathOperations::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude, - posSatE); + double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; + MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE); if (validSavedPosSatE && - (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { + (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); - double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE; - VectorOperations::mulScalar(deltaDistance, 1 / timeDiffGpsMeas, gpsVelocityE, 3); + double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; + VectorOperations::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3); } + savedPosSatE[0] = posSatE[0]; + savedPosSatE[1] = posSatE[1]; + savedPosSatE[2] = posSatE[2]; + + timeOfSavedPosSatE = gpsUnixSeconds; + validSavedPosSatE = true; } { PoolReadGuard pg(gpsDataProcessed); @@ -594,15 +600,6 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong gpsDataProcessed->gcLatitude.value = 0.0; } } - savedPosSatE[0] = posSatE[0]; - savedPosSatE[1] = posSatE[1]; - savedPosSatE[2] = posSatE[2]; - - timeOfSavedPosSatE = gps0UnixSeconds; - validSavedPosSatE = true; - } - else { - validGcLatitude = false; } } @@ -614,9 +611,11 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, const AcsParameters *acsParameters) { sensorValues->update(); processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, + sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && - sensorValues->gpsSet.altitude.isValid()), - gpsDataProcessed); + sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.altitude.isValid() && + sensorValues->gpsSet.unixSeconds.isValid()), + &acsParameters->gpsParameters, gpsDataProcessed); processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index a3a8c5fb..77205669 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -65,7 +65,9 @@ class SensorProcessing { void processStr(); - void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, + void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude, + const double gpsUnixSeconds, const bool validGps, + const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed); double savedMgmVecTot[3]; @@ -73,7 +75,6 @@ class SensorProcessing { double savedSusVecTot[3]; timeval timeOfSavedSusDirEst; bool validMagField; - bool validGcLatitude; double savedPosSatE[3]; uint32_t timeOfSavedPosSatE; From 12a367f65f61ea1bc2374a93e9b699ac9e89061f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 10:13:28 +0100 Subject: [PATCH 020/119] fixed detumble merge aftermath --- mission/controller/acs/control/Detumble.cpp | 23 +++++++++------------ mission/controller/acs/control/Detumble.h | 2 +- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 1b0b674c..9053fc01 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -22,22 +22,19 @@ Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParamet Detumble::~Detumble() {} void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { - detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); + detumbleParameter = &(acsParameters_->detumbleParameter); magnetorquesParameter = &(acsParameters_->magnetorquesParameter); } -ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid, - const double *magField, const bool *magFieldValid, - double *magMom) { - - 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::bDotLaw(const double *magRate, const bool magRateValid, + const double *magField, const bool magFieldValid, double *magMom) { + 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, diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 16bf82bc..6a3e6eaa 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -25,7 +25,7 @@ class Detumble { static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE; static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters für this class + /* @brief: Load AcsParameters for this class * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ void loadAcsParameters(AcsParameters *acsParameters_); From b6c5796121b61881dc473ac90ad70bd492860189 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 10:31:20 +0100 Subject: [PATCH 021/119] fixed init for SensorProcessing and type of timeDiff --- mission/controller/acs/SensorProcessing.cpp | 3 +-- mission/controller/acs/SensorProcessing.h | 12 ++++++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 5b4267e7..f5d2568b 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -21,8 +21,7 @@ using namespace Math; -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) - : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {} +SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {} SensorProcessing::~SensorProcessing() {} diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 77205669..1a094dab 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -70,15 +70,15 @@ class SensorProcessing { const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed); - double savedMgmVecTot[3]; + double savedMgmVecTot[3] = {0.0, 0.0, 0.0}; timeval timeOfSavedMagFieldEst; - double savedSusVecTot[3]; + double savedSusVecTot[3] = {0.0, 0.0, 0.0}; timeval timeOfSavedSusDirEst; - bool validMagField; + bool validMagField = false; - double savedPosSatE[3]; - uint32_t timeOfSavedPosSatE; - bool validSavedPosSatE; + double savedPosSatE[3] = {0.0, 0.0, 0.0}; + double timeOfSavedPosSatE = 0.0; + bool validSavedPosSatE = false; const float zeroVector[3] = {0.0, 0.0, 0.0}; SusConverter susConverter; From c97d319b2a2619361dbb1c4c071d0a755b87af57 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 10:39:41 +0100 Subject: [PATCH 022/119] added gpsVelocity to gpsProcessed dataSet --- mission/controller/AcsController.cpp | 1 + mission/controller/AcsController.h | 1 + mission/controller/controllerdefinitions/AcsCtrlDefinitions.h | 4 +++- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 9e400e75..7d1ebc60 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -393,6 +393,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::GPS_VELOCITY, &gpsVelocity); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); // MEKF localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 844d85d3..ec1749b3 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -144,6 +144,7 @@ class AcsController : public ExtendedControllerBase { acsctrl::GpsDataProcessed gpsDataProcessed; PoolEntry gcLatitude = PoolEntry(); PoolEntry gdLongitude = PoolEntry(); + PoolEntry gpsVelocity = PoolEntry(3); // MEKF acsctrl::MekfData mekfData; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 2652ca3c..ab5ed8da 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, + GPS_VELOCITY, // MEKF SAT_ROT_RATE_MEKF, QUAT_MEKF, @@ -105,7 +106,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 = 2; +static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 3; static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; @@ -224,6 +225,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_vec_t gpsVelocity = lp_vec_t(sid.objectId, GPS_VELOCITY, this); private: }; From 8b415883e9e0c35d3fa9d4ce8b43366096a1526e Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 11:46:58 +0100 Subject: [PATCH 023/119] removed references to deprecated OutputValues and renamed guidance strategies --- mission/controller/AcsController.cpp | 15 +++-- mission/controller/acs/Guidance.cpp | 92 ++++++++++++---------------- mission/controller/acs/Guidance.h | 27 ++++---- 3 files changed, 63 insertions(+), 71 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7d1ebc60..9057f5e4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -249,15 +249,18 @@ void AcsController::performPointingCtrl() { double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; switch (submode) { - case SUBMODE_PTG_TARGET: - guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, - refSatRate); - break; case SUBMODE_PTG_SUN: - guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); break; + case SUBMODE_PTG_TARGET: + guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + break; + case SUBMODE_PTG_NADIR: - guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); + guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); break; case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 199fce9c..afd4d41e 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -30,9 +30,10 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); } -void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation or given latitude, longitude and altitude //------------------------------------------------------------------------------------- @@ -102,9 +103,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData // Calculation of reference rotation rate //------------------------------------------------------------------------------------- double velSatE[3] = {0, 0, 0}; - velSatE[0] = outputValues->gpsVelocity[0]; // sensorValues->gps0Velocity[0]; - velSatE[1] = outputValues->gpsVelocity[1]; // sensorValues->gps0Velocity[1]; - velSatE[2] = outputValues->gpsVelocity[2]; // sensorValues->gps0Velocity[2]; + std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; // Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E MatrixOperations::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); @@ -127,7 +126,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData if (acsParameters.targetModeControllerParameters.avoidBlindStr) { double sunDirB[3] = {0, 0, 0}; - if (outputValues->sunDirModelValid) { + if (susDataProcessed->sunIjkModel.isValid()) { double sunDirJ[3] = {0, 0, 0}; std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); @@ -174,8 +173,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData } } -void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MekfData *mekfData, timeval now, + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for target pointing //------------------------------------------------------------------------------------- @@ -217,8 +218,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // Transform velocity into inertial frame - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], - outputValues->gpsVelocity[2]}; + double velocityE[3]; + std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); @@ -251,7 +252,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec, -6)); + timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); @@ -287,38 +288,28 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; + std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } -void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- double quatBJ[4] = {0, 0, 0, 0}; double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; + std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::toDcm(quatBJ, dcmBJ); double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0}; - if (outputValues->sunDirModelValid) { - sunDirJ[0] = outputValues->sunDirModel[0]; - sunDirJ[1] = outputValues->sunDirModel[1]; - sunDirJ[2] = outputValues->sunDirModel[2]; + if (susDataProcessed->sunIjkModel.isValid()) { + std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } - - else { - sunDirB[0] = outputValues->sunDirEst[0]; - sunDirB[1] = outputValues->sunDirEst[1]; - sunDirB[2] = outputValues->sunDirEst[2]; + } else { + std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); } /* @@ -367,8 +358,8 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *ou double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], - outputValues->gpsVelocity[2]}; + double velocityE[3]; + std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); @@ -410,9 +401,8 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *ou refSatRate[2] = 0; } -void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, - ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], +void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + timeval now, double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing @@ -440,10 +430,7 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; + std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::toDcm(quatBJ, dcmBJ); MatrixOperations::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3); @@ -478,8 +465,10 @@ void Guidance::quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, refSatRate[2] = 0; } -void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], + double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion for Nadir pointing //------------------------------------------------------------------------------------- @@ -512,9 +501,8 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues * VectorOperations::mulScalar(xAxis, -1, xAxis, 3); // z-Axis parallel to long side of picture resolution - double zAxis[3] = {0, 0, 0}; - double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], - outputValues->gpsVelocity[2]}; + double zAxis[3] = {0, 0, 0}, velocityE[3]; + std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); @@ -539,7 +527,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues * double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec, -6)); + timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); @@ -575,10 +563,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues * // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; - quatBJ[0] = outputValues->quatMekfBJ[0]; - quatBJ[1] = outputValues->quatMekfBJ[1]; - quatBJ[2] = outputValues->quatMekfBJ[2]; - quatBJ[3] = outputValues->quatMekfBJ[3]; + std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } @@ -608,7 +593,6 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]}, {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}}; - double quatErrorComplete[4] = {0, 0, 0, 0}; MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); if (quatErrorComplete[3] < 0) { @@ -619,8 +603,8 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou quatError[1] = quatErrorComplete[1]; quatError[2] = quatErrorComplete[2]; - // target flag in matlab, importance, does look like it only gives - // feedback if pointing control is under 150 arcsec ?? + // target flag in matlab, importance, does look like it only gives feedback if pointing control is + // under 150 arcsec ?? } void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 1b6bfcb6..176d51a2 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -23,26 +23,31 @@ class Guidance { // Function to get the target quaternion and refence rotation rate from gps position and position // of the ground station - void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, + void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], + double refSatRate[3]); + void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], double refSatRate[3]); - void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, - acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4], - double refSatRate[3]); // Function to get the target quaternion and refence rotation rate for sun pointing after ground // station - void sunQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]); + void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4], + double refSatRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing - void quatNadirPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, + acsctrl::GpsDataProcessed *gpsDataProcessed, + acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], + double refSatRate[3]); + void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double refSatRate[3]); - void quatNadirPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, timeval now, - double targetQuat[4], double refSatRate[3]); - // Function to get the target quaternion and refence rotation rate from parameters for inertial // pointing void inertialQuatPtg(double targetQuat[4], double refSatRate[3]); From 3f2910c3a72632e939056587843ff447b2f1111d Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 11:50:29 +0100 Subject: [PATCH 024/119] changed order of submodes and their naming --- mission/controller/AcsController.cpp | 6 +++--- mission/controller/AcsController.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 9057f5e4..fa106d4c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -51,7 +51,7 @@ void AcsController::performControlOperation() { case SUBMODE_DETUMBLE: performDetumble(); break; - case SUBMODE_PTG_SUN: + case SUBMODE_IDLE: case SUBMODE_PTG_TARGET: case SUBMODE_PTG_NADIR: case SUBMODE_PTG_INERTIAL: @@ -249,7 +249,7 @@ void AcsController::performPointingCtrl() { double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; switch (submode) { - case SUBMODE_PTG_SUN: + case SUBMODE_IDLE: guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); break; @@ -452,7 +452,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode > 6) || (submode < 2)) { + if ((submode > 7) || (submode < 2)) { return INVALID_SUBMODE; } else { return returnvalue::OK; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index ec1749b3..de7cb23f 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -26,9 +26,9 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_SAFE = 2; static const Submode_t SUBMODE_DETUMBLE = 3; - static const Submode_t SUBMODE_PTG_TARGET = 4; + static const Submode_t SUBMODE_IDLE = 4; static const Submode_t SUBMODE_PTG_NADIR = 5; - static const Submode_t SUBMODE_PTG_SUN = 6; + static const Submode_t SUBMODE_PTG_TARGET = 6; static const Submode_t SUBMODE_PTG_INERTIAL = 7; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; From b3a2cc43670757662861e6af9b9e33d084b386c6 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 16 Dec 2022 13:59:49 +0100 Subject: [PATCH 025/119] fixed booleans in acs Parameters and enabled setting of parameters --- mission/controller/acs/AcsParameters.cpp | 1069 ++++++++++--------- mission/controller/acs/AcsParameters.h | 21 +- mission/controller/acs/SensorProcessing.cpp | 2 +- 3 files changed, 549 insertions(+), 543 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index cc9959ab..3205f7b2 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -3,542 +3,549 @@ #include #include -AcsParameters::AcsParameters(){}; //(uint8_t parameterModuleId) : - // parameterModuleId(parameterModuleId) {} +AcsParameters::AcsParameters() {} AcsParameters::~AcsParameters() {} -/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, +ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex) { - 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; - case 0x4: - parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight); - 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; + // 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.inertiaMatrixDeployed); + break; + case 0x2: + parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed); + break; + case 0x3: + parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1); + break; + case 0x4: + parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3); + 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; + case 0xF: + parameterWrapper->set(mgmHandlingParameters.mgm02variance); + break; + case 0x10: + parameterWrapper->set(mgmHandlingParameters.mgm13variance); + break; + case 0x11: + parameterWrapper->set(mgmHandlingParameters.mgm4variance); + 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; + case 0x4: + parameterWrapper->set(gyrHandlingParameters.gyr02variance); + break; + case 0x5: + parameterWrapper->set(gyrHandlingParameters.gyr13variance); + break; + case 0x6: + parameterWrapper->set(gyrHandlingParameters.preferAdis); + 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; + case 0x6: + parameterWrapper->set(rwHandlingParameters.stictionSpeed); + break; + case 0x7: + parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); + break; + case 0x8: + parameterWrapper->set(rwHandlingParameters.stictionTorque); + 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; + case 0x6: + parameterWrapper->set(rwMatrices.nullspace); + 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): // PointingModeControllerParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(targetModeControllerParameters.refDirection); + break; + case 0x1: + parameterWrapper->set(targetModeControllerParameters.refRotRate); + break; + case 0x2: + parameterWrapper->set(targetModeControllerParameters.quatRef); + break; + case 0x3: + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + break; + case 0x4: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + break; + case 0x5: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x6: + parameterWrapper->set(targetModeControllerParameters.blindRotRate); + break; + case 0x7: + parameterWrapper->set(targetModeControllerParameters.zeta); + break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.om); + break; + case 0x9: + parameterWrapper->set(targetModeControllerParameters.omMax); + break; + case 0xA: + parameterWrapper->set(targetModeControllerParameters.qiMin); + break; + case 0xB: + parameterWrapper->set(targetModeControllerParameters.gainNullspace); + break; + case 0xC: + parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + break; + case 0xD: + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + break; + case 0xE: + parameterWrapper->set(targetModeControllerParameters.desatOn); + break; + case 0xF: + parameterWrapper->set(targetModeControllerParameters.omegaEarth); + break; + case 0x10: + parameterWrapper->set(targetModeControllerParameters.nadirRefDirection); + break; + case 0x11: + parameterWrapper->set(targetModeControllerParameters.tgtQuatInertial); + break; + case 0x12: + parameterWrapper->set(targetModeControllerParameters.tgtRotRateInertial); + break; + case 0x13: + parameterWrapper->set(targetModeControllerParameters.nadirTimeElapsedMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xA): // 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 (0xB): // GpsParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(gpsParameters.timeDiffVelocityMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xC): // 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; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xD): // SunModelParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(sunModelParameters.domega); + break; + case 0x1: + parameterWrapper->set(sunModelParameters.omega_0); + break; + case 0x2: + parameterWrapper->set(sunModelParameters.m_0); + break; + case 0x3: + parameterWrapper->set(sunModelParameters.dm); + break; + case 0x4: + parameterWrapper->set(sunModelParameters.e); + break; + case 0x5: + parameterWrapper->set(sunModelParameters.e1); + break; + case 0x6: + parameterWrapper->set(sunModelParameters.p1); + break; + case 0x7: + parameterWrapper->set(sunModelParameters.p2); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xE): // KalmanFilterParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); + break; + case 0x1: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); + break; + case 0x2: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); + break; + case 0x3: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); + break; + case 0x4: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); + break; + case 0x5: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xF): // 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 (0x10): // 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; + case 0x3: + parameterWrapper->set(detumbleParameter.gainD); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + default: + return INVALID_IDENTIFIER_ID; } return returnvalue::OK; - } else { - return INVALID_DOMAIN_ID; - } -}*/ + // } else { + // return INVALID_DOMAIN_ID; + // } +} diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 8bd10091..3c88c9a1 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -12,15 +12,15 @@ typedef unsigned char uint8_t; -class AcsParameters /*: public HasParametersIF*/ { +class AcsParameters : public HasParametersIF { public: AcsParameters(); virtual ~AcsParameters(); - /* - virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, - ParameterWrapper *parameterWrapper, - const ParameterWrapper *newValues, uint16_t startAtIndex); - */ + + ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId, + ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues, + uint16_t startAtIndex) override; + struct OnBoardParams { double sampleTime = 0.4; // [s] } onBoardParams; @@ -776,8 +776,7 @@ class AcsParameters /*: public HasParametersIF*/ { pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; - enum PreferAdis { NO = 0, YES = 1 }; - uint8_t preferAdis = PreferAdis::YES; + uint8_t preferAdis = true; } gyrHandlingParameters; struct RwHandlingParameters { @@ -834,7 +833,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}; - bool avoidBlindStr = true; + uint8_t avoidBlindStr = true; double blindAvoidStart = 1.5; double blindAvoidStop = 2.5; double blindRotRate = 1 * M_PI / 180; @@ -847,8 +846,8 @@ class AcsParameters /*: public HasParametersIF*/ { double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; - bool desatOn = true; - bool enableAntiStiction = true; + uint8_t desatOn = true; + uint8_t enableAntiStiction = true; double omegaEarth = 0.000072921158553; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index f5d2568b..7bbefe08 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -530,7 +530,7 @@ void SensorProcessing::processGyr( // take ADIS measurements, if both avail // if just one ADIS measurement avail, perform sensor fusion double gyrVecTot[3] = {0.0, 0.0, 0.0}; - if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) { + if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == true) { double gyr02ValuesSum[3]; VectorOperations::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); VectorOperations::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); From e4936b1bedb3c0a08ceed13911cb0e8daa57c55c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Dec 2022 14:23:42 +0100 Subject: [PATCH 026/119] removed deprecated acs parameters and set sunMagAngleMin --- mission/controller/acs/AcsParameters.cpp | 26 ++++-------------------- mission/controller/acs/AcsParameters.h | 17 ++-------------- 2 files changed, 6 insertions(+), 37 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 3205f7b2..276204d2 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -219,12 +219,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, 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; } @@ -259,30 +253,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, 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: + case 0x1: parameterWrapper->set(rwHandlingParameters.maxTrq); break; - case 0x6: + case 0x2: parameterWrapper->set(rwHandlingParameters.stictionSpeed); break; - case 0x7: + case 0x3: parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed); break; - case 0x8: + case 0x4: parameterWrapper->set(rwHandlingParameters.stictionTorque); break; default: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 3c88c9a1..895eaf9b 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -760,9 +760,6 @@ class AcsParameters : public HasParametersIF { {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; - - float filterAlpha; - float sunThresh; } susHandlingParameters; struct GyrHandlingParameters { @@ -780,10 +777,6 @@ class AcsParameters : public HasParametersIF { } gyrHandlingParameters; struct RwHandlingParameters { - double rw0orientationMatrix[3][3]; - double rw1orientationMatrix[3][3]; - double rw2orientationMatrix[3][3]; - double rw3orientationMatrix[3][3]; double inertiaWheel = 0.000028198; double maxTrq = 0.0032; // 3.2 [mNm] double stictionSpeed = 80; // RPM @@ -795,11 +788,6 @@ class AcsParameters : public HasParametersIF { double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000}, {0.0000, -0.9205, 0.0000, 0.9205}, {0.3907, 0.3907, 0.3907, 0.3907}}; - // double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597}, - // {0.2136, -0.3317, 1.0123}, - // {-0.8672, -0.1406, 0.1778}, - // {0.6426, 0.4794, 1.3603}}; - // where does the first pseudo inverse come frome - matlab gives result below double pseudoInverse[4][3] = { {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}}; double without0[4][3] = { @@ -818,10 +806,9 @@ class AcsParameters : public HasParametersIF { double k_align_mekf = 0.000056875; double k_rate_no_mekf = 0.00059437; - ; double k_align_no_mekf = 0.000056875; - ; - double sunMagAngleMin; // ??? + + double sunMagAngleMin = 5 * M_PI / 180; double sunTargetDir[3] = {0, 0, 1}; double satRateRef[3] = {0, 0, 0}; From c338c4fb58c494596cfd4c2ff546906541a39125 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Dec 2022 14:24:03 +0100 Subject: [PATCH 027/119] fixed usage of sunMagAngleMin --- mission/controller/acs/control/SafeCtrl.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 54ae27ee..88f4de9b 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -129,10 +129,8 @@ 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 sinAngle = 0; - sinAngle = sin(acos(cos(cosAngleSunMag))); - - if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) { + double angleSunMag = acos(cosAngleSunMag); + if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { return; } From d9d75590972c777ad5c711c1d29d8628613cd26a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Dec 2022 14:34:20 +0100 Subject: [PATCH 028/119] fixed usage of wrong sample time for MEKF --- .../controller/acs/MultiplicativeKalmanFilter.cpp | 13 ++++++++----- mission/controller/acs/MultiplicativeKalmanFilter.h | 3 +-- mission/controller/acs/Navigation.cpp | 2 +- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 14c2a2ec..f54893a2 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -197,11 +197,14 @@ void MultiplicativeKalmanFilter::init( } // --------------- 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, 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, double sampleTime, + acsctrl::MekfData *mekfData) { // Check for GYR Measurements int MDF = 0; // Matrix Dimension Factor if (!validGYRs_) { diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 5eb47418..79d39510 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -61,7 +61,7 @@ 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, acsctrl::MekfData *mekfData); + const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); // Declaration of Events (like init) and memberships // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND @@ -79,7 +79,6 @@ class MultiplicativeKalmanFilter { AcsParameters::KalmanFilterParameters *kalmanFilterParameters; double quaternion_STR_SB[4]; bool validInit; - double sampleTime = 0.1; /*States*/ double initialQuaternion[4]; /*after reset?QUEST*/ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 921ae604..a9de5817 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -39,7 +39,7 @@ void 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(), + mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData); // VALIDS FOR QUAT AND RATE ?? } else { multiplicativeKalmanFilter.init( From bd52da8afdefa59724759f8293bf2c51dde91a43 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Dec 2022 09:59:42 +0100 Subject: [PATCH 029/119] set stictionSpeed to minimum commandable RW speed --- mission/controller/acs/AcsParameters.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 895eaf9b..e2f26c96 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -767,8 +767,8 @@ class AcsParameters : public HasParametersIF { double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}; double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; - // var = sqrt(sigma), 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 + /* var = sqrt(sigma), 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 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms @@ -779,7 +779,7 @@ class AcsParameters : public HasParametersIF { struct RwHandlingParameters { double inertiaWheel = 0.000028198; double maxTrq = 0.0032; // 3.2 [mNm] - double stictionSpeed = 80; // RPM + double stictionSpeed = 100; // 80; // RPM double stictionReleaseSpeed = 120; // RPM double stictionTorque = 0.0006; } rwHandlingParameters; From 341966e70920b57a7f24accd153eae3451450ac4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 9 Jan 2023 09:36:00 +0100 Subject: [PATCH 030/119] added GS target mode --- mission/controller/AcsController.cpp | 2 +- mission/controller/AcsController.h | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 235f7193..03f36b6f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -451,7 +451,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode > 7) || (submode < 2)) { + if ((submode > 8) || (submode < 2)) { return INVALID_SUBMODE; } else { return returnvalue::OK; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index de7cb23f..d2e11792 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -29,7 +29,8 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_IDLE = 4; static const Submode_t SUBMODE_PTG_NADIR = 5; static const Submode_t SUBMODE_PTG_TARGET = 6; - static const Submode_t SUBMODE_PTG_INERTIAL = 7; + static const Submode_t SUBMODE_PTG_TARGET_GS = 7; + static const Submode_t SUBMODE_PTG_INERTIAL = 8; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; static const Event SAFE_RATE_VIOLATION = From 10f1898b98b3118b04abb8b223774a7063fde5c9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 9 Jan 2023 09:36:30 +0100 Subject: [PATCH 031/119] fixed init of varibales --- mission/controller/acs/control/PtgCtrl.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index d176b0c2..c493b383 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -64,8 +64,8 @@ class PtgCtrl { AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::RwMatrices *rwMatrices; - double torqueMemory[4]; - double omegaMemory[4]; + double torqueMemory[4] = {0, 0, 0, 0}; + double omegaMemory[4] = {0, 0, 0, 0}; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ From 4e74dc7beee935e6e21ab717e90dd2d3d8a09b35 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 09:05:27 +0100 Subject: [PATCH 032/119] added target mode for GS flyby --- mission/controller/acs/Guidance.cpp | 5 +++++ mission/controller/acs/Guidance.h | 4 ++++ 2 files changed, 9 insertions(+) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index afd4d41e..e46abe37 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -292,6 +292,11 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); } +void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]) {} + void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 176d51a2..80993a0d 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -27,6 +27,10 @@ class Guidance { acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], double refSatRate[3]); + void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]); void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, From 887d193526c5ef7c46cf8996dfae8bd44a9d6ebf Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 09:06:09 +0100 Subject: [PATCH 033/119] added ReceiversParameterMessagesIF and ParameterHelper to ACS Ctrl --- mission/controller/AcsController.cpp | 1113 +++++++++++++------------- mission/controller/AcsController.h | 14 +- 2 files changed, 579 insertions(+), 548 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 03f36b6f..78a78c6f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -14,6 +14,7 @@ AcsController::AcsController(object_id_t objectId) detumble(&acsParameters), ptgCtrl(&acsParameters), detumbleCounter{0}, + parameterHelper(this), mgmDataRaw(this), mgmDataProcessed(this), susDataRaw(this), @@ -26,6 +27,23 @@ AcsController::AcsController(object_id_t objectId) actuatorCmdData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { + ReturnValue_t result = actionHelper.handleActionMessage(message); + if (result == returnvalue::OK) { + return result; + } + result = parameterHelper.handleParameterMessage(message); + if (result == returnvalue::OK) { + return result; + } + return result; +} + +MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); } + +ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId, + ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, + uint16_t startAtIndex) { return returnvalue::OK; } @@ -53,6 +71,7 @@ void AcsController::performControlOperation() { break; case SUBMODE_IDLE: case SUBMODE_PTG_TARGET: + case SUBMODE_PTG_TARGET_GS: case SUBMODE_PTG_NADIR: case SUBMODE_PTG_INERTIAL: performPointingCtrl(); @@ -83,577 +102,581 @@ void AcsController::performControlOperation() { copyGyrData(); } } -} + } -void AcsController::performSafe() { - ACS::SensorValues sensorValues; + void AcsController::performSafe() { + ACS::SensorValues sensorValues; - timeval now; - Clock::getClock_timeval(&now); + timeval now; + Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); - // Give desired satellite rate and sun direction to align - double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; - guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); - // IF MEKF is working - double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - bool magMomMtqValid = false; - if (validMekf == returnvalue::OK) { - safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), - mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), - susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), - mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), - sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); - } else { - safeCtrl.safeNoMekf( - now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), - susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), + // Give desired satellite rate and sun direction to align + double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; + guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); + // IF MEKF is working + double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; + bool magMomMtqValid = false; + if (validMekf == returnvalue::OK) { + safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), + mgmDataProcessed.magIgrfModel.value, + mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, + susDataProcessed.isValid(), mekfData.satRotRateMekf.value, + mekfData.satRotRateMekf.isValid(), sunTargetDir, satRateSafe, &errAng, + magMomMtq, &magMomMtqValid); + } else { + safeCtrl.safeNoMekf( + now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), + susDataProcessed.susVecTotDerivative.value, + susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, + mgmDataProcessed.mgmVecTotDerivative.isValid(), sunTargetDir, satRateSafe, &errAng, + magMomMtq, &magMomMtqValid); + } + + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + double zeroQuat[4] = {0, 0, 0, 0}; + std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double)); + ctrlValData.tgtQuat.setValid(false); + std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double)); + ctrlValData.errQuat.setValid(false); + ctrlValData.errAng.value = errAng; + ctrlValData.errAng.setValid(true); + ctrlValData.setValidity(true, false); + } + } + + // Detumble check and switch + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } else { + detumbleCounter = 0; + } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_DETUMBLE; + detumbleCounter = 0; + triggerEvent(SAFE_RATE_VIOLATION); + } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + int32_t zeroVec[4] = {0, 0, 0, 0}; + std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], + // torqueDuration); + // } + } + + void AcsController::performDetumble() { + ACS::SensorValues sensorValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + double magMomMtq[3] = {0, 0, 0}; + detumble.bDotLaw( mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), - sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); - } + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); - - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - double zeroQuat[4] = {0, 0, 0, 0}; - std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double)); - ctrlValData.tgtQuat.setValid(false); - std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double)); - ctrlValData.errQuat.setValid(false); - ctrlValData.errAng.value = errAng; - ctrlValData.errAng.setValid(true); - ctrlValData.setValidity(true, false); + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else { + detumbleCounter = 0; } - } - - // Detumble check and switch - if (mekfData.satRotRateMekf.isValid() && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else { - detumbleCounter = 0; - } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - submode = SUBMODE_DETUMBLE; - detumbleCounter = 0; - triggerEvent(SAFE_RATE_VIOLATION); - } - - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - int32_t zeroVec[4] = {0, 0, 0, 0}; - std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_SAFE; + detumbleCounter = 0; } - } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration); - // } -} -void AcsController::performDetumble() { - ACS::SensorValues sensorValues; - - timeval now; - Clock::getClock_timeval(&now); - - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - - double magMomMtq[3] = {0, 0, 0}; - detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, - mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); - - if (mekfData.satRotRateMekf.isValid() && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } else { - detumbleCounter = 0; - } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - submode = SUBMODE_SAFE; - detumbleCounter = 0; - } - - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); - } - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); + int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; + for (int i = 0; i < 3; ++i) { + cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); } - } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } -} - -void AcsController::performPointingCtrl() { - ACS::SensorValues sensorValues; - - timeval now; - Clock::getClock_timeval(&now); - - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - - double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - switch (submode) { - case SUBMODE_IDLE: - guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - break; - case SUBMODE_PTG_TARGET: - guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); - break; - - case SUBMODE_PTG_NADIR: - guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); - break; - case SUBMODE_PTG_INERTIAL: - guidance.inertialQuatPtg(targetQuat, refSatRate); - break; - } - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, - deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed - guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); - double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; - ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - double rwTrqNs[4] = {0, 0, 0, 0}; - ptgCtrl.ptgNullspace( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); - - if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { - bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - int32_t rwSpeed[4] = { - (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), - (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; - ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); + // } } - double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), + void AcsController::performPointingCtrl() { + ACS::SensorValues sensorValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; + switch (submode) { + case SUBMODE_IDLE: + guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); + break; + case SUBMODE_PTG_TARGET: + guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, + targetQuat, refSatRate); + break; + case SUBMODE_PTG_TARGET_GS: + guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, + now, targetQuat, refSatRate); + break; + case SUBMODE_PTG_NADIR: + guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + break; + case SUBMODE_PTG_INERTIAL: + guidance.inertialQuatPtg(targetQuat, refSatRate); + break; + } + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed + guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); + double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; + ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + double rwTrqNs[4] = {0, 0, 0, 0}; + ptgCtrl.ptgNullspace( + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + + if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { + bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? + int32_t rwSpeed[4] = { + (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), + (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + } + + double cmdSpeedRws[4] = {0, 0, 0, + 0}; // Should be given to the actuator reaction wheel as input + actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); + double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + ptgCtrl.ptgDesaturation(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), torqueRwsScaled, cmdSpeedRws); - double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol - ptgCtrl.ptgDesaturation(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); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); + &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); - } - int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; - for (int i = 0; i < 4; ++i) { - cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); + int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; + for (int i = 0; i < 3; ++i) { + cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); + } + int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; + for (int i = 0; i < 4; ++i) { + cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); + } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + actuatorCmdData.setValidity(true, true); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); + // } } - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); - actuatorCmdData.setValidity(true, true); - } + ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool & localDataPoolMap, + LocalDataPoolManager & poolManager) { + // MGM Raw + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); + poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0}); + // MGM Processed + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer); + localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf); + poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0}); + // SUS Raw + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw); + poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0}); + // SUS Processed + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer); + localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk); + poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0}); + // GYR Raw + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw); + poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0}); + // GYR Processed + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot); + poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0}); + // GPS Processed + localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); + poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); + // MEKF + localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); + localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); + poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0}); + // Ctrl Values + localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); + localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); + localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); + poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); + // Actuator CMD + localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); + localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); + localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); + poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0}); + return returnvalue::OK; } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } -} -ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, - LocalDataPoolManager &poolManager) { - // MGM Raw - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); - poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0}); - // MGM Processed - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer); - localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf); - poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0}); - // SUS Raw - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw); - poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0}); - // SUS Processed - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer); - localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk); - poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0}); - // GYR Raw - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw); - poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0}); - // GYR Processed - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot); - poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0}); - // GPS Processed - localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); - localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); - localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); - poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); - // MEKF - localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); - localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); - poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0}); - // Ctrl Values - localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); - localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); - localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); - poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); - // Actuator CMD - localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); - localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); - localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); - poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0}); - return returnvalue::OK; -} + LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { + switch (sid.ownerSetId) { + case acsctrl::MGM_SENSOR_DATA: + return &mgmDataRaw; + case acsctrl::MGM_PROCESSED_DATA: + return &mgmDataProcessed; + case acsctrl::SUS_SENSOR_DATA: + return &susDataRaw; + case acsctrl::SUS_PROCESSED_DATA: + return &susDataProcessed; + case acsctrl::GYR_SENSOR_DATA: + return &gyrDataRaw; + case acsctrl::GYR_PROCESSED_DATA: + return &gyrDataProcessed; + case acsctrl::GPS_PROCESSED_DATA: + return &gpsDataProcessed; + case acsctrl::MEKF_DATA: + return &mekfData; + case acsctrl::CTRL_VAL_DATA: + return &ctrlValData; + case acsctrl::ACTUATOR_CMD_DATA: + return &actuatorCmdData; + default: + return nullptr; + } + return nullptr; + } -LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { - switch (sid.ownerSetId) { - case acsctrl::MGM_SENSOR_DATA: - return &mgmDataRaw; - case acsctrl::MGM_PROCESSED_DATA: - return &mgmDataProcessed; - case acsctrl::SUS_SENSOR_DATA: - return &susDataRaw; - case acsctrl::SUS_PROCESSED_DATA: - return &susDataProcessed; - case acsctrl::GYR_SENSOR_DATA: - return &gyrDataRaw; - case acsctrl::GYR_PROCESSED_DATA: - return &gyrDataProcessed; - case acsctrl::GPS_PROCESSED_DATA: - return &gpsDataProcessed; - case acsctrl::MEKF_DATA: - return &mekfData; - case acsctrl::CTRL_VAL_DATA: - return &ctrlValData; - case acsctrl::ACTUATOR_CMD_DATA: - return &actuatorCmdData; - default: - return nullptr; + ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t * msToReachTheMode) { + if (mode == MODE_OFF) { + if (submode == SUBMODE_NONE) { + return returnvalue::OK; + } else { + return INVALID_SUBMODE; + } + } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { + if ((submode > 8) || (submode < 2)) { + return INVALID_SUBMODE; + } else { + return returnvalue::OK; + } + } + return INVALID_MODE; } - return nullptr; -} -ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t *msToReachTheMode) { - if (mode == MODE_OFF) { - if (submode == SUBMODE_NONE) { - return returnvalue::OK; - } else { - return INVALID_SUBMODE; + void AcsController::copyMgmData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.mgm0Lis3Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); + } } - } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode > 8) || (submode < 2)) { - return INVALID_SUBMODE; - } else { - return returnvalue::OK; + { + PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.mgm2Lis3Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.imtqMgmSet); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value, + 3 * sizeof(float)); + mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); + mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; + mgmDataRaw.actuationCalStatus.setValid( + sensorValues.imtqMgmSet.coilActuationStatus.isValid()); + } } } - return INVALID_MODE; -} -void AcsController::modeChanged(Mode_t mode, Submode_t submode) {} + void AcsController::copySusData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.susSets[0]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[1]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[2]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[3]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[4]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[5]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[6]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[7]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[8]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[9]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[10]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[11]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid()); + } + } + } -void AcsController::announceMode(bool recursive) {} - -void AcsController::copyMgmData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.mgm0Lis3Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); + void AcsController::copyGyrData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.gyr0AdisSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value; + gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value; + gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value; + gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() && + sensorValues.gyr0AdisSet.angVelocY.isValid() && + sensorValues.gyr0AdisSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr1L3gSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value; + gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value; + gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value; + gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() && + sensorValues.gyr1L3gSet.angVelocY.isValid() && + sensorValues.gyr1L3gSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr2AdisSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value; + gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value; + gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value; + gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() && + sensorValues.gyr2AdisSet.angVelocY.isValid() && + sensorValues.gyr2AdisSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr3L3gSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value; + gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value; + gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value; + gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() && + sensorValues.gyr3L3gSet.angVelocY.isValid() && + sensorValues.gyr3L3gSet.angVelocZ.isValid()); + } } } - { - PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.mgm2Lis3Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.imtqMgmSet); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value, - 3 * sizeof(float)); - mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); - mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; - mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid()); - } - } -} - -void AcsController::copySusData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.susSets[0]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[1]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[2]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[3]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[4]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[5]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[6]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[7]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[8]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[9]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[10]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[11]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid()); - } - } -} - -void AcsController::copyGyrData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.gyr0AdisSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value; - gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value; - gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value; - gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() && - sensorValues.gyr0AdisSet.angVelocY.isValid() && - sensorValues.gyr0AdisSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr1L3gSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value; - gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value; - gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value; - gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() && - sensorValues.gyr1L3gSet.angVelocY.isValid() && - sensorValues.gyr1L3gSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr2AdisSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value; - gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value; - gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value; - gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() && - sensorValues.gyr2AdisSet.angVelocY.isValid() && - sensorValues.gyr2AdisSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr3L3gSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value; - gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value; - gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value; - gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() && - sensorValues.gyr3L3gSet.angVelocY.isValid() && - sensorValues.gyr3L3gSet.angVelocZ.isValid()); - } - } -} diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index d2e11792..3ffcde85 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -3,6 +3,8 @@ #include #include +#include +#include #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" @@ -18,7 +20,7 @@ #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" -class AcsController : public ExtendedControllerBase { +class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; @@ -36,6 +38,12 @@ class AcsController : public ExtendedControllerBase { static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); //!< The limits for the rotation in safe mode were violated. + MessageQueueId_t getCommandQueue() const; + + ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId, + ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, + uint16_t startAtIndex) override; + protected: void performSafe(); void performDetumble(); @@ -54,6 +62,8 @@ class AcsController : public ExtendedControllerBase { uint8_t detumbleCounter; + ParameterHelper parameterHelper; + enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; @@ -68,8 +78,6 @@ class AcsController : public ExtendedControllerBase { // Mode abstract functions ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t* msToReachTheMode) override; - void modeChanged(Mode_t mode, Submode_t submode); - void announceMode(bool recursive); /* ACS Datasets */ IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); From 0854b1c77874844b831b451d738565335ac57fe8 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 09:16:13 +0100 Subject: [PATCH 034/119] ACS Ctrl getParameter forwarding to ACS Param --- mission/controller/AcsController.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 78a78c6f..7f69936c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -44,7 +44,8 @@ ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex) { - return returnvalue::OK; + return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues, + startAtIndex); } void AcsController::performControlOperation() { From 93ec49bf8deaaaa073dea28661cc1d49d95399c1 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 11:20:28 +0100 Subject: [PATCH 035/119] added 3 axes stabilized target mode for GS contact. renamed tgt coordinates acordingly --- mission/controller/acs/AcsParameters.cpp | 8 +- mission/controller/acs/AcsParameters.h | 10 +- mission/controller/acs/Guidance.cpp | 240 ++++++++++++----------- mission/controller/acs/Guidance.h | 6 +- 4 files changed, 136 insertions(+), 128 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 276204d2..2479eb53 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -412,16 +412,16 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xC): // GroundStationParameters + case (0xC): // PtgTargetParameters switch (parameterId & 0xFF) { case 0x0: - parameterWrapper->set(groundStationParameters.latitudeGs); + parameterWrapper->set(ptgTargetParameters.latitudeTgt); break; case 0x1: - parameterWrapper->set(groundStationParameters.longitudeGs); + parameterWrapper->set(ptgTargetParameters.longitudeTgt); break; case 0x2: - parameterWrapper->set(groundStationParameters.altitudeGs); + parameterWrapper->set(ptgTargetParameters.altitudeTgt); break; default: return INVALID_IDENTIFIER_ID; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index e2f26c96..df7b0882 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -854,11 +854,11 @@ class AcsParameters : public HasParametersIF { double timeDiffVelocityMax = 30; //[s] } gpsParameters; - struct GroundStationParameters { - double latitudeGs = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeGs = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeGs = 500; // [m] Altitude - } groundStationParameters; // Stuttgart + struct ptgTargetParameters { // Default is Stuttgart GS + double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude + double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude + double altitudeTgt = 500; // [m] Altitude + } ptgTargetParameters; struct SunModelParameters { float domega = 36000.771; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index e46abe37..cd3a7b95 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -39,12 +39,11 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: //------------------------------------------------------------------------------------- // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; + double targetCart[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, - acsParameters.groundStationParameters.longitudeGs, - acsParameters.groundStationParameters.altitudeGs, - groundStationCart); + MathOperations::cartesianFromLatLongAlt( + acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, + acsParameters.ptgTargetParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; @@ -55,7 +54,7 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: // Target direction in the ECEF frame double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -173,6 +172,48 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: } } +void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate) { + //------------------------------------------------------------------------------------- + // Calculation of reference rotation rate + //------------------------------------------------------------------------------------- + double timeElapsed = + now.tv_sec + now.tv_usec * pow(10, -6) - + (timeSavedQuaternionNadir.tv_sec + + timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); + if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { + double qDiff[4] = {0, 0, 0, 0}; + VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); + VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); + + double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, + qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; + VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); + VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::add(sum1, sum2, sum, 3); + VectorOperations::subtract(sum, sum3, sum, 3); + double omegaRefNew[3] = {0, 0, 0}; + VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); + + VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); + VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); + omegaRefSavedNadir[0] = omegaRefNew[0]; + omegaRefSavedNadir[1] = omegaRefNew[1]; + omegaRefSavedNadir[2] = omegaRefNew[2]; + } else { + refSatRate[0] = 0; + refSatRate[1] = 0; + refSatRate[2] = 0; + } + + timeSavedQuaternionNadir = now; + savedQuaternionNadir[0] = quatInertialTarget[0]; + savedQuaternionNadir[1] = quatInertialTarget[1]; + savedQuaternionNadir[2] = quatInertialTarget[2]; + savedQuaternionNadir[3] = quatInertialTarget[3]; +} + void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MekfData *mekfData, timeval now, @@ -182,12 +223,11 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, //------------------------------------------------------------------------------------- // Transform longitude, latitude and altitude to cartesian coordiantes (earth // fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; + double targetCart[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs, - acsParameters.groundStationParameters.longitudeGs, - acsParameters.groundStationParameters.altitudeGs, - groundStationCart); + MathOperations::cartesianFromLatLongAlt( + acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, + acsParameters.ptgTargetParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; @@ -195,7 +235,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, sensorValues->gpsSet.altitude.value, posSatE); double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -246,45 +286,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- - double timeElapsed = - now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); - if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { - double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; - double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); - VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); - VectorOperations::add(sum1, sum2, sum, 3); - VectorOperations::subtract(sum, sum3, sum, 3); - double omegaRefNew[3] = {0, 0, 0}; - VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); - - VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); - VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); - omegaRefSavedNadir[0] = omegaRefNew[0]; - omegaRefSavedNadir[1] = omegaRefNew[1]; - omegaRefSavedNadir[2] = omegaRefNew[2]; - } else { - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; - } - - timeSavedQuaternionNadir = now; - savedQuaternionNadir[0] = quatInertialTarget[0]; - savedQuaternionNadir[1] = quatInertialTarget[1]; - savedQuaternionNadir[2] = quatInertialTarget[2]; - savedQuaternionNadir[3] = quatInertialTarget[3]; + refRotationRate(now, quatInertialTarget, refSatRate); // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; @@ -295,7 +297,75 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, - double targetQuat[4], double refSatRate[3]) {} + double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion for ground station pointing + //------------------------------------------------------------------------------------- + // Transform longitude, latitude and altitude to cartesian coordiantes (earth + // fixed/centered frame) + double groundStationCart[3] = {0, 0, 0}; + + MathOperations::cartesianFromLatLongAlt( + acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, + acsParameters.ptgTargetParameters.altitudeTgt, groundStationCart); + // Position of the satellite in the earth/fixed frame via GPS + double posSatE[3] = {0, 0, 0}; + double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; + double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; + MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, + sensorValues->gpsSet.altitude.value, posSatE); + double targetDirE[3] = {0, 0, 0}; + VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + + // Transformation between ECEF and IJK frame + double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot); + MathOperations::inverseMatrixDimThree(*dcmEJ, *dcmJE); + + double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot); + + // Target Direction and position vector in the inertial frame + double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1); + MatrixOperations::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1); + + // negative x-Axis aligned with target (Camera/E-band transmitter position) + double xAxis[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirJ, xAxis, 3); + VectorOperations::mulScalar(xAxis, -1, xAxis, 3); + + // get Sun Vector Model in ECI as helper vector (element of x-z plane) + double sunJ[3]; + std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); + VectorOperations::normalize(sunJ, sunJ, 3); + + // calculate y-axis + double yAxis[3]; + VectorOperations::cross(sunJ, xAxis, yAxis); + VectorOperations::normalize(yAxis, yAxis, 3); + + // calculate z-axis + double zAxis[3]; + VectorOperations::cross(xAxis, yAxis, zAxis); + VectorOperations::normalize(zAxis, zAxis, 3); + + // Complete transformation matrix + double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, + {xAxis[1], yAxis[1], zAxis[1]}, + {xAxis[2], yAxis[2], zAxis[2]}}; + double quatInertialTarget[4] = {0, 0, 0, 0}; + QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); + + refRotationRate(now, quatInertialTarget, refSatRate); + + // Transform in system relative to satellite frame + double quatBJ[4] = {0, 0, 0, 0}; + std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double)); + QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat); +} void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, acsctrl::SusDataProcessed *susDataProcessed, @@ -317,32 +387,6 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); } - /* - // --------------------------------------------------------------------------- - // Old version of two vector quaternion (only one axis to align) - // --------------------------------------------------------------------------- - double sunRef[3] = {0, 0, 0}; - sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0]; - sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1]; - sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2]; - - double sunCross[3] = {0, 0, 0}; - VectorOperations::cross(sunDirB, sunRef, sunCross); - double normSunDir = VectorOperations::norm(sunDirB, 3); - double normSunRef = VectorOperations::norm(sunRef, 3); - double dotSun = VectorOperations::dot(sunDirB, sunRef); - - targetQuat[0] = sunCross[0]; - targetQuat[1] = sunCross[1]; - targetQuat[2] = sunCross[2]; - targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun); - - VectorOperations::normalize(targetQuat, targetQuat, 4); - */ - - //---------------------------------------------------------------------------- - // New version - //---------------------------------------------------------------------------- // Transformation between ECEF and IJK frame double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -526,45 +570,7 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - //------------------------------------------------------------------------------------- - // Calculation of reference rotation rate - //------------------------------------------------------------------------------------- - double timeElapsed = - now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); - if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { - double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; - double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); - VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); - VectorOperations::add(sum1, sum2, sum, 3); - VectorOperations::subtract(sum, sum3, sum, 3); - double omegaRefNew[3] = {0, 0, 0}; - VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); - - VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); - VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); - omegaRefSavedNadir[0] = omegaRefNew[0]; - omegaRefSavedNadir[1] = omegaRefNew[1]; - omegaRefSavedNadir[2] = omegaRefNew[2]; - } else { - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; - } - - timeSavedQuaternionNadir = now; - savedQuaternionNadir[0] = quatInertialTarget[0]; - savedQuaternionNadir[1] = quatInertialTarget[1]; - savedQuaternionNadir[2] = quatInertialTarget[2]; - savedQuaternionNadir[3] = quatInertialTarget[3]; + refRotationRate(now, quatInertialTarget, refSatRate); // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 80993a0d..ae232b31 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -21,8 +21,8 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); - // Function to get the target quaternion and refence rotation rate from gps position and position - // of the ground station + // Function to get the target quaternion and refence rotation rate from gps position and + // position of the ground station void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, acsctrl::GpsDataProcessed *gpsDataProcessed, acsctrl::MekfData *mekfData, timeval now, double targetQuat[4], @@ -61,6 +61,8 @@ class Guidance { void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], double quatErrorComplete[4], double quatError[3], double deltaRate[3]); + void refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate); + // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); From c79e17514c3f695750cd1d21d50d80b08905c1d3 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 13:51:55 +0100 Subject: [PATCH 036/119] changed sun and mgm model calc to always be executed --- mission/controller/acs/SensorProcessing.cpp | 139 +++++++++----------- mission/controller/acs/SensorProcessing.h | 1 - 2 files changed, 61 insertions(+), 79 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 7bbefe08..7e49fcdd 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -33,19 +33,35 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed) { + // ---------------- 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. + Igrf13Model igrf13; + // So the line above should not be done here. Update: Can be done here as long updated coffs + // stored in acsParameters ? + igrf13.schmidtNormalization(); + igrf13.updateCoeffGH(timeOfMgmMeasurement); + // maybe put a condition here, to only update after a full day, this + // class function has around 700 steps to perform + igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, + gpsAltitude, timeOfMgmMeasurement, magIgrfModel); + } if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float)); - std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double)); + float zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float)); mgmDataProcessed->setValidity(false, true); + std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); + mgmDataProcessed->magIgrfModel.setValid(gpsValid); } } return; @@ -138,20 +154,6 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const } timeOfSavedMagFieldEst = timeOfMgmMeasurement; - // ---------------- 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. - Igrf13Model igrf13; - // So the line above should not be done here. Update: Can be done here as long updated coffs - // stored in acsParameters ? - igrf13.schmidtNormalization(); - igrf13.updateCoeffGH(timeOfMgmMeasurement); - // maybe put a condition here, to only update after a full day, this - // class function has around 700 steps to perform - igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, - gpsAltitude, timeOfMgmMeasurement, magIgrfModel); - } { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -187,6 +189,25 @@ void SensorProcessing::processSus( timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed) { + /* -------- Sun Model Direction (IJK frame) ------- */ + double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); + + // Julean Centuries + double sunIjkModel[3] = {0.0, 0.0, 0.0}; + double JC2000 = JD2000 / 36525.; + + double meanLongitude = + sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.; + double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.; + + double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) + + sunModelParameters->p2 * sin(2 * meanAnomaly); + + double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000; + + sunIjkModel[0] = cos(eclipticLongitude); + sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); + sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); if (sus0valid) { sus0valid = susConverter.checkSunSensorData(sus0Value); } @@ -229,22 +250,24 @@ void SensorProcessing::processSus( { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float)); - std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double)); + float zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float)); susDataProcessed->setValidity(false, true); + std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); + susDataProcessed->sunIjkModel.setValid(true); } } return; @@ -263,16 +286,6 @@ void SensorProcessing::processSus( susParameters->sus0coeffBeta), sus0VecBody, 3, 3, 1); } - { - PoolReadGuard pg(susDataProcessed); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); - susDataProcessed->sus0vec.setValid(sus0valid); - if (!sus0valid) { - std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float)); - } - } - } if (sus1valid) { MatrixOperations::multiply( susParameters->sus1orientationMatrix[0], @@ -280,16 +293,6 @@ void SensorProcessing::processSus( susParameters->sus1coeffBeta), sus1VecBody, 3, 3, 1); } - { - PoolReadGuard pg(susDataProcessed); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); - susDataProcessed->sus1vec.setValid(sus1valid); - if (!sus1valid) { - std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float)); - } - } - } if (sus2valid) { MatrixOperations::multiply( susParameters->sus2orientationMatrix[0], @@ -397,27 +400,6 @@ void SensorProcessing::processSus( } } timeOfSavedSusDirEst = timeOfSusMeasurement; - - /* -------- Sun Model Direction (IJK frame) ------- */ - // if (useSunModel) eventuell - double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); - - // Julean Centuries - double sunIjkModel[3] = {0.0, 0.0, 0.0}; - double JC2000 = JD2000 / 36525.; - - double meanLongitude = - sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.; - double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.; - - double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) + - sunModelParameters->p2 * sin(2 * meanAnomaly); - - double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000; - - sunIjkModel[0] = cos(eclipticLongitude); - sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); - sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); { PoolReadGuard pg(susDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -474,6 +456,7 @@ void SensorProcessing::processGyr( { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { + double zeroVector[3] = {0.0, 0.0, 0.0}; std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double)); diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 1a094dab..49c659b0 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -79,7 +79,6 @@ class SensorProcessing { double savedPosSatE[3] = {0.0, 0.0, 0.0}; double timeOfSavedPosSatE = 0.0; bool validSavedPosSatE = false; - const float zeroVector[3] = {0.0, 0.0, 0.0}; SusConverter susConverter; AcsParameters acsParameters; From ac83e660169e7939536312cadf3ba6eff650d351 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 13:52:26 +0100 Subject: [PATCH 037/119] naming and frmt --- mission/controller/AcsController.cpp | 11 +++++------ mission/controller/acs/control/SafeCtrl.cpp | 9 ++++----- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7f69936c..1f9df068 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -146,10 +146,10 @@ void AcsController::performControlOperation() { { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { - double zeroQuat[4] = {0, 0, 0, 0}; - std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double)); + double unitQuat[4] = {0, 0, 0, 1}; + std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); ctrlValData.tgtQuat.setValid(false); - std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); ctrlValData.errQuat.setValid(false); ctrlValData.errAng.value = errAng; ctrlValData.errAng.setValid(true); @@ -178,10 +178,9 @@ void AcsController::performControlOperation() { { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { - int32_t zeroVec[4] = {0, 0, 0, 0}; - std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t)); + std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetTorque.setValid(false); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); + std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 88f4de9b..aa04cbb6 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -87,7 +87,7 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, return returnvalue::OK; } -// Will be the version in worst case scenario in event of no working MEKF (nor RMUs) +// Will be the version in worst case scenario in event of no working MEKF (nor GYRs) void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB, bool magRateBValid, double *sunDirRef, @@ -127,18 +127,17 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); /* Only valid if angle between sun direction and magnetic field direction - is sufficiently large */ - + * is sufficiently large */ double angleSunMag = acos(cosAngleSunMag); if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { return; } - // Rate for Torque Calculation + // Rate for Torque Calculation double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); - // Torque Align calculation + // Torque Align calculation double kRateNoMekf = 0, kAlignNoMekf = 0; kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; From a2626afebb1a5870cd25342811bd2777c889eba2 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 11 Jan 2023 13:42:48 +0100 Subject: [PATCH 038/119] added gpsPosition to gpsProcessedData --- mission/controller/AcsController.cpp | 1 + mission/controller/AcsController.h | 1 + mission/controller/acs/SensorProcessing.cpp | 10 ++++------ .../controllerdefinitions/AcsCtrlDefinitions.h | 4 +++- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1f9df068..362a5e1b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -421,6 +421,7 @@ void AcsController::performControlOperation() { // GPS Processed localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); // MEKF diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 3ffcde85..64f5c09f 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -153,6 +153,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes acsctrl::GpsDataProcessed gpsDataProcessed; PoolEntry gcLatitude = PoolEntry(); PoolEntry gdLongitude = PoolEntry(); + PoolEntry gpsPosition = PoolEntry(3); PoolEntry gpsVelocity = PoolEntry(3); // MEKF diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 7e49fcdd..5d68bc44 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -546,7 +546,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed) { // name to convert not process - double gdLongitude, gcLatitude; + double gdLongitude = 0, gcLatitude = 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.; @@ -576,11 +576,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong if (pg.getReadResult() == returnvalue::OK) { gpsDataProcessed->gdLongitude.value = gdLongitude; gpsDataProcessed->gcLatitude.value = gcLatitude; - gpsDataProcessed->setValidity(validGps, validGps); - if (!validGps) { - gpsDataProcessed->gdLongitude.value = 0.0; - gpsDataProcessed->gcLatitude.value = 0.0; - } + 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/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index ab5ed8da..0955581b 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, + GPS_POSITION, GPS_VELOCITY, // MEKF SAT_ROT_RATE_MEKF, @@ -106,7 +107,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 = 3; +static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4; static constexpr uint8_t MEKF_SET_ENTRIES = 2; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; @@ -225,6 +226,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_vec_t gpsPosition = lp_vec_t(sid.objectId, GPS_POSITION, this); lp_vec_t gpsVelocity = lp_vec_t(sid.objectId, GPS_VELOCITY, this); private: From 25867e76b196fa4cddf3a00773df34323e6175e9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 11 Jan 2023 16:00:30 +0100 Subject: [PATCH 039/119] fixed solar panels -> sun vector direction --- mission/controller/acs/Guidance.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index cd3a7b95..14ea5412 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -337,21 +337,25 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat VectorOperations::normalize(targetDirJ, xAxis, 3); VectorOperations::mulScalar(xAxis, -1, xAxis, 3); - // get Sun Vector Model in ECI as helper vector (element of x-z plane) + // get Sun Vector Model in ECI double sunJ[3]; std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); VectorOperations::normalize(sunJ, sunJ, 3); + // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector + // z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x + double xDotS = VectorOperations::dot(xAxis, sunJ); + xDotS /= pow(VectorOperations::norm(xAxis, 3), 2); + double sunParallel[3], zAxis[3]; + VectorOperations::mulScalar(xAxis, xDotS, sunParallel, 3); + VectorOperations::subtract(sunJ, sunParallel, zAxis, 3); + VectorOperations::normalize(zAxis, zAxis, 3); + // calculate y-axis double yAxis[3]; - VectorOperations::cross(sunJ, xAxis, yAxis); + VectorOperations::cross(zAxis, xAxis, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // calculate z-axis - double zAxis[3]; - VectorOperations::cross(xAxis, yAxis, zAxis); - VectorOperations::normalize(zAxis, zAxis, 3); - // Complete transformation matrix double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]}, {xAxis[1], yAxis[1], zAxis[1]}, @@ -383,8 +387,10 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me if (susDataProcessed->sunIjkModel.isValid()) { std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double)); MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } else { + } else if (susDataProcessed->susVecTot.isValid()) { std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double)); + } else { + return; } // Transformation between ECEF and IJK frame @@ -416,7 +422,7 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me double velocityB[3] = {0, 0, 0}; MatrixOperations::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1); - // Normale to velocity and sunDir + // Normal to velocity and sunDir double crossVelSun[3] = {0, 0, 0}; VectorOperations::cross(velocityB, sunDirB, crossVelSun); From 66e2d782b21505450954bcd07326bdf4722c7d85 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 12 Jan 2023 11:47:56 +0100 Subject: [PATCH 040/119] domainId now points to structs. sunVectorSafeLeop added --- mission/controller/acs/AcsParameters.cpp | 51 ++++++++++++------------ mission/controller/acs/AcsParameters.h | 2 + 2 files changed, 27 insertions(+), 26 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 2479eb53..332f1873 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -11,16 +11,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex) { - // if (domainId == parameterModuleId) { - switch (parameterId >> 8) { + switch (domainId) { case 0x0: // direct members - switch (parameterId & 0xFF) { + switch (parameterId) { default: return INVALID_IDENTIFIER_ID; } break; case 0x1: // OnBoardParams - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(onBoardParams.sampleTime); break; @@ -29,7 +28,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case 0x2: // InertiaEIVE - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(inertiaEIVE.inertiaMatrix); break; @@ -50,7 +49,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case 0x3: // MgmHandlingParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); break; @@ -110,7 +109,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case 0x4: // SusHandlingParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); break; @@ -224,7 +223,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0x5): // GyrHandlingParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); break; @@ -251,7 +250,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0x6): // RwHandlingParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(rwHandlingParameters.inertiaWheel); break; @@ -272,7 +271,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0x7): // RwMatrices - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(rwMatrices.alignmentMatrix); break; @@ -299,7 +298,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0x8): // SafeModeControllerParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(safeModeControllerParameters.k_rate_mekf); break; @@ -316,9 +315,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); break; case 0x5: - parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop); break; case 0x6: + parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + break; + case 0x7: parameterWrapper->set(safeModeControllerParameters.satRateRef); break; default: @@ -326,7 +328,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0x9): // PointingModeControllerParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(targetModeControllerParameters.refDirection); break; @@ -392,7 +394,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0xA): // StrParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(strParameters.exclusionAngle); break; @@ -404,7 +406,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0xB): // GpsParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(gpsParameters.timeDiffVelocityMax); break; @@ -413,7 +415,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0xC): // PtgTargetParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(ptgTargetParameters.latitudeTgt); break; @@ -428,7 +430,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0xD): // SunModelParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(sunModelParameters.domega); break; @@ -458,7 +460,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0xE): // KalmanFilterParameters - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); break; @@ -482,7 +484,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0xF): // MagnetorquesParameter - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); break; @@ -506,7 +508,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; case (0x10): // DetumbleParameter - switch (parameterId & 0xFF) { + switch (parameterId) { case 0x0: parameterWrapper->set(detumbleParameter.detumblecounter); break; @@ -524,10 +526,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, } break; default: - return INVALID_IDENTIFIER_ID; - } - return returnvalue::OK; - // } else { - // return INVALID_DOMAIN_ID; - // } + return INVALID_DOMAIN_ID; + } + return returnvalue::OK; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index df7b0882..d081c8aa 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -810,7 +810,9 @@ class AcsParameters : public HasParametersIF { double sunMagAngleMin = 5 * M_PI / 180; + double sunTargetDirLeop[3] = {0, .5, .5}; double sunTargetDir[3] = {0, 0, 1}; + double satRateRef[3] = {0, 0, 0}; } safeModeControllerParameters; From 27d6e95062c23dc5c09c59b9dadcce6c5b57a1e4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 12 Jan 2023 11:48:57 +0100 Subject: [PATCH 041/119] x-axis now points away of equator plane for sunPtgMode --- mission/controller/acs/Guidance.cpp | 45 +++++++++++++---------------- 1 file changed, 20 insertions(+), 25 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 14ea5412..16d4341c 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -23,7 +23,11 @@ Guidance::~Guidance() {} void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { for (int i = 0; i < 3; i++) { - sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; + if (false) { // ToDo: if file does not exist anymore + sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; + } else { + sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDirLeop[i]; + } satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i]; } @@ -230,10 +234,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, acsParameters.ptgTargetParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); + std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double)); double targetDirE[3] = {0, 0, 0}; VectorOperations::subtract(targetCart, posSatE, targetDirE, 3); @@ -406,34 +407,28 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me double zAxis[3] = {0, 0, 0}; VectorOperations::normalize(sunDirB, zAxis, 3); - // Position of the satellite in the earth/fixed frame via GPS and body - // velocity + // Position of satellite in ECEF double posSatE[3] = {0, 0, 0}; - double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; - double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180; - MathOperations::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad, - sensorValues->gpsSet.altitude.value, posSatE); - double velocityE[3]; - std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double)); - double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1); - MatrixOperations::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1); - VectorOperations::add(velPart1, velPart2, velocityJ, 3); - double velocityB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1); + std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); - // Normal to velocity and sunDir - double crossVelSun[3] = {0, 0, 0}; - VectorOperations::cross(velocityB, sunDirB, crossVelSun); + // Check whether Sat is above or below equator + // Assign helper vector (north pole inertial) accordingly + double helperVec[3] = {0, 0, 0}; + if (posSatE[2] > 0) { + double helperVec[2] = 1; + } else { + double helperVec[2] = -1; + } - // y- Axis as cross of normal velSun and zAxis + // double yAxis[3] = {0, 0, 0}; - VectorOperations::cross(crossVelSun, sunDirB, yAxis); + VectorOperations::cross(zAxis, helperVec, yAxis); VectorOperations::normalize(yAxis, yAxis, 3); - // complete RHS for x-Axis + // double xAxis[3] = {0, 0, 0}; VectorOperations::cross(yAxis, zAxis, xAxis); + VectorOperations::normalize(xAxis, xAxis, 3); // Transformation matrix to Sun, no further transforamtions, reference is already // the EIVE body frame From 4090941c3ea723cdc8949e3f2bc12881507712bb Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 12 Jan 2023 13:33:06 +0100 Subject: [PATCH 042/119] bugfix --- 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 16d4341c..2196d71b 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -415,9 +415,9 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me // Assign helper vector (north pole inertial) accordingly double helperVec[3] = {0, 0, 0}; if (posSatE[2] > 0) { - double helperVec[2] = 1; + helperVec[2] = 1; } else { - double helperVec[2] = -1; + helperVec[2] = -1; } // From 5e557d2d46b3ffdc37c788916cbc2f77ec3c25b4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 12 Jan 2023 14:49:37 +0100 Subject: [PATCH 043/119] first version of safe mode solar vector for leop --- mission/controller/acs/Guidance.cpp | 20 +++++++++++--------- mission/controller/acs/Guidance.h | 3 +++ 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 2196d71b..9027392e 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -13,6 +13,8 @@ #include #include +#include + #include "string.h" #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" @@ -22,16 +24,16 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParamete Guidance::~Guidance() {} void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { - for (int i = 0; i < 3; i++) { - if (false) { // ToDo: if file does not exist anymore - sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; - } else { - sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDirLeop[i]; - } - satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i]; + 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, + 3 * sizeof(double)); + } else { + std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop, + 3 * sizeof(double)); } - - // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); + std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef, + 3 * sizeof(double)); } void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index ae232b31..82ca2b77 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -73,6 +73,9 @@ class Guidance { timeval timeSavedQuaternionNadir; double savedQuaternionNadir[4] = {0, 0, 0, 0}; double omegaRefSavedNadir[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"; }; #endif /* ACS_GUIDANCE_H_ */ From d8c0ba19fd051380286cde94140c2c23a9b91097 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 12 Jan 2023 15:19:21 +0100 Subject: [PATCH 044/119] changed pointingParameters struct --- mission/controller/AcsController.cpp | 30 +++++++++- mission/controller/acs/AcsParameters.cpp | 15 ----- mission/controller/acs/AcsParameters.h | 66 ++++++++++++++-------- mission/controller/acs/Guidance.cpp | 66 +++++++++++----------- mission/controller/acs/Guidance.h | 10 ++-- mission/controller/acs/control/PtgCtrl.cpp | 19 ++++--- mission/controller/acs/control/PtgCtrl.h | 2 +- 7 files changed, 117 insertions(+), 91 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 362a5e1b..9b210369 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -269,30 +269,56 @@ void AcsController::performControlOperation() { &mekfData, &validMekf); double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; + double quatRef[4] = {0, 0, 0, 0}; + uint8_t enableAntiStiction = true; switch (submode) { case SUBMODE_IDLE: guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); + for ( uint8_t i = 0; i < 4; i++ ) { + quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; + } + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + break; case SUBMODE_PTG_TARGET: guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); + for ( uint8_t i = 0; i < 4; i++ ) { + quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; + } + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + break; case SUBMODE_PTG_TARGET_GS: guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); + + for ( uint8_t i = 0; i < 4; i++ ) { + quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; + } + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_NADIR: guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); + for ( uint8_t i = 0; i < 4; i++ ) { + quatRef[i] = acsParameters.nadirModeControllerParameters.quatRef[i]; + } + enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); + + for ( uint8_t i = 0; i < 4; i++ ) { + quatRef[i] = acsParameters.inertialModeControllerParameters.quatRef[i]; + } + enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; } double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed - guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; @@ -305,7 +331,7 @@ void AcsController::performControlOperation() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); - if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { + if ( enableAntiStiction ) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? int32_t rwSpeed[4] = { (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 2479eb53..31480403 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -372,21 +372,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0xE: parameterWrapper->set(targetModeControllerParameters.desatOn); break; - case 0xF: - parameterWrapper->set(targetModeControllerParameters.omegaEarth); - break; - case 0x10: - parameterWrapper->set(targetModeControllerParameters.nadirRefDirection); - break; - case 0x11: - parameterWrapper->set(targetModeControllerParameters.tgtQuatInertial); - break; - case 0x12: - parameterWrapper->set(targetModeControllerParameters.tgtRotRateInertial); - break; - case 0x13: - parameterWrapper->set(targetModeControllerParameters.nadirTimeElapsedMax); - break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index df7b0882..80a3a65f 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -815,35 +815,51 @@ class AcsParameters : public HasParametersIF { } safeModeControllerParameters; - // ToDo: mutiple structs for different pointing mode controllers? - struct PointingModeControllerParameters { - double refDirection[3] = {-1, 0, 0}; // Antenna - double refRotRate[3] = {0, 0, 0}; - double quatRef[4] = {0, 0, 0, 1}; - uint8_t avoidBlindStr = true; - double blindAvoidStart = 1.5; - double blindAvoidStop = 2.5; - double blindRotRate = 1 * M_PI / 180; + struct PointingLawParameters { - double zeta = 0.3; - double om = 0.3; - double omMax = 1 * M_PI / 180; - double qiMin = 0.1; - double gainNullspace = 0.01; + double zeta = 0.3; + double om = 0.3; + double omMax = 1 * M_PI / 180; + double qiMin = 0.1; + double gainNullspace = 0.01; - double desatMomentumRef[3] = {0, 0, 0}; - double deSatGainFactor = 1000; - uint8_t desatOn = true; - uint8_t enableAntiStiction = true; + double desatMomentumRef[3] = {0, 0, 0}; + double deSatGainFactor = 1000; + uint8_t desatOn = true; + uint8_t enableAntiStiction = true; - double omegaEarth = 0.000072921158553; + } pointingLawParameters; - double nadirRefDirection[3] = {-1, 0, 0}; // Camera - double tgtQuatInertial[4] = {0, 0, 0, 1}; - double tgtRotRateInertial[3] = {0, 0, 0}; - int8_t nadirTimeElapsedMax = 10; - } pointingModeControllerParameters, inertialModeControllerParameters, - nadirModeControllerParameters, targetModeControllerParameters; + struct TargetModeControllerParameters : PointingLawParameters { + double refDirection[3] = {-1, 0, 0}; // Antenna + double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to + // give this as an input- currently en calculation is done + double quatRef[4] = {0, 0, 0, 1}; + int8_t timeElapsedMax = 10; // rot rate calculations + + // Default is Stuttgart GS + double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude + double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude + double altitudeTgt = 500; // [m] + + // For one-axis control: + uint8_t avoidBlindStr = true; + double blindAvoidStart = 1.5; + double blindAvoidStop = 2.5; + double blindRotRate = 1 * M_PI / 180; + } targetModeControllerParameters; + + struct NadirModeControllerParameters : PointingLawParameters { + double refDirection[3] = {-1, 0, 0}; // Antenna + double quatRef[4] = {0, 0, 0, 1}; + int8_t timeElapsedMax = 10; // rot rate calculations + } nadirModeControllerParameters; + + struct InertialModeControllerParameters : PointingLawParameters { + double tgtQuat[4] = {0, 0, 0, 1}; + double refRotRate[3] = {0, 0, 0}; + double quatRef[4] = {0, 0, 0, 1}; + } inertialModeControllerParameters; struct StrParameters { double exclusionAngle = 20 * M_PI / 180; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 14ea5412..700fd521 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -42,8 +42,8 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, - acsParameters.ptgTargetParameters.altitudeTgt, targetCart); + acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; @@ -172,17 +172,17 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: } } -void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate) { +void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate) { //------------------------------------------------------------------------------------- // Calculation of reference rotation rate //------------------------------------------------------------------------------------- double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternionNadir.tv_sec + - timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); - if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { + (timeSavedQuaternion.tv_sec + + timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); + if (timeElapsed < timeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4); + VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, @@ -197,21 +197,21 @@ void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); - VectorOperations::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); - omegaRefSavedNadir[0] = omegaRefNew[0]; - omegaRefSavedNadir[1] = omegaRefNew[1]; - omegaRefSavedNadir[2] = omegaRefNew[2]; + VectorOperations::subtract(refSatRate, omegaRefSaved, refSatRate, 3); + omegaRefSaved[0] = omegaRefNew[0]; + omegaRefSaved[1] = omegaRefNew[1]; + omegaRefSaved[2] = omegaRefNew[2]; } else { refSatRate[0] = 0; refSatRate[1] = 0; refSatRate[2] = 0; } - timeSavedQuaternionNadir = now; - savedQuaternionNadir[0] = quatInertialTarget[0]; - savedQuaternionNadir[1] = quatInertialTarget[1]; - savedQuaternionNadir[2] = quatInertialTarget[2]; - savedQuaternionNadir[3] = quatInertialTarget[3]; + timeSavedQuaternion = now; + savedQuaternion[0] = quatInertialTarget[0]; + savedQuaternion[1] = quatInertialTarget[1]; + savedQuaternion[2] = quatInertialTarget[2]; + savedQuaternion[3] = quatInertialTarget[3]; } void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, @@ -226,8 +226,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, - acsParameters.ptgTargetParameters.altitudeTgt, targetCart); + acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; @@ -286,7 +286,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - refRotationRate(now, quatInertialTarget, refSatRate); + int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; + refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; @@ -306,8 +307,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat double groundStationCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, - acsParameters.ptgTargetParameters.altitudeTgt, groundStationCart); + acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; @@ -363,7 +364,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - refRotationRate(now, quatInertialTarget, refSatRate); + int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax; + refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; @@ -495,9 +497,9 @@ void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:: // rotation quaternion from two vectors double refDir[3] = {0, 0, 0}; - refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0]; - refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1]; - refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[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); @@ -576,7 +578,8 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, double quatInertialTarget[4] = {0, 0, 0, 0}; QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); - refRotationRate(now, quatInertialTarget, refSatRate); + int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax; + refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate); // Transform in system relative to satellite frame double quatBJ[4] = {0, 0, 0, 0}; @@ -586,20 +589,15 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { for (int i = 0; i < 4; i++) { - targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i]; + targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuat[i]; } for (int i = 0; i < 3; i++) { - refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i]; + refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRate[i]; } } -void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], +void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3], double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { - double quatRef[4] = {0, 0, 0, 0}; - quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; - quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; - quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2]; - quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; double satRate[3] = {0, 0, 0}; std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index ae232b31..e399af8a 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -58,10 +58,10 @@ class Guidance { // @note: compares target Quaternion and reference quaternion, also actual satellite rate and // desired - void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], + void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3], double quatErrorComplete[4], double quatError[3], double deltaRate[3]); - void refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate); + void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate); // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" @@ -70,9 +70,9 @@ class Guidance { private: AcsParameters acsParameters; bool strBlindAvoidFlag = false; - timeval timeSavedQuaternionNadir; - double savedQuaternionNadir[4] = {0, 0, 0, 0}; - double omegaRefSavedNadir[3] = {0, 0, 0}; + timeval timeSavedQuaternion; + double savedQuaternion[4] = {0, 0, 0, 0}; + double omegaRefSaved[3] = {0, 0, 0}; }; #endif /* ACS_GUIDANCE_H_ */ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 9d4f8d6d..53544b8c 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -21,7 +21,8 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter PtgCtrl::~PtgCtrl() {} void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); + // TODO: Here correct Parameters have to be loaded according to current submode + pointingLawParameters = &(acsParameters_->targetModeControllerParameters); inertiaEIVE = &(acsParameters_->inertiaEIVE); rwHandlingParameters = &(acsParameters_->rwHandlingParameters); rwMatrices = &(acsParameters_->rwMatrices); @@ -32,10 +33,10 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix //------------------------------------------------------------------------------------------------ - double om = pointingModeControllerParameters->om; - double zeta = pointingModeControllerParameters->zeta; - double qErrorMin = pointingModeControllerParameters->qiMin; - double omMax = pointingModeControllerParameters->omMax; + double om = pointingLawParameters->om; + double zeta = pointingLawParameters->zeta; + double qErrorMin = pointingLawParameters->qiMin; + double omMax = pointingLawParameters->omMax; double cInt = 2 * om * zeta; double kInt = 2 * pow(om, 2); @@ -110,7 +111,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes) { - if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { + if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { mgtDpDes[0] = 0; mgtDpDes[1] = 0; mgtDpDes[2] = 0; @@ -129,12 +130,12 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; VectorOperations::subtract( - momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); + momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3); // resulting magnetic dipole command double crossMomentumMagField[3] = {0, 0, 0}; VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; - factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; + factor = (pointingLawParameters->deSatGainFactor) / normMag; VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); } @@ -150,7 +151,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, wheelMomentum, 4); - double gainNs = pointingModeControllerParameters->gainNullspace; + 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, *nullSpaceMatrix, 4); diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index c493b383..85f13a7a 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -59,7 +59,7 @@ class PtgCtrl { void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand); private: - AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; + AcsParameters::PointingLawParameters *pointingLawParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::RwMatrices *rwMatrices; From 2f0eace82279d6b73166ea37aa7fc41d39554dda Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 12 Jan 2023 17:10:40 +0100 Subject: [PATCH 045/119] use std::memcpy --- mission/controller/acs/Guidance.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 700fd521..4ba72674 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -588,12 +588,8 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, } void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { - for (int i = 0; i < 4; i++) { - targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuat[i]; - } - for (int i = 0; i < 3; i++) { - refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRate[i]; - } + std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, 4 * sizeof(double)); + std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, 3 * sizeof(double)); } void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3], From ce3841a23d9c0011a92ef7e5552b261b15149fa2 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 12 Jan 2023 17:19:35 +0100 Subject: [PATCH 046/119] use std::memcpy --- mission/controller/AcsController.cpp | 22 +++++----------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 9b210369..dee2fb73 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -275,44 +275,32 @@ void AcsController::performControlOperation() { case SUBMODE_IDLE: guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_TARGET: guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_TARGET_GS: guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); - - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_NADIR: guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.nadirModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); - - for ( uint8_t i = 0; i < 4; i++ ) { - quatRef[i] = acsParameters.inertialModeControllerParameters.quatRef[i]; - } + std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; } From db763e394dd90e42a53ffb28a281b538981f96ec Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 09:54:03 +0100 Subject: [PATCH 047/119] corrected MGM raw set size and sus processed pool ids --- .../controller/controllerdefinitions/AcsCtrlDefinitions.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 0955581b..e3908bd0 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -101,7 +101,7 @@ enum PoolIds : lp_id_t { MTQ_TARGET_DIPOLE, }; -static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10; +static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6; static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 8; static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15; @@ -184,9 +184,9 @@ class SusDataProcessed : public StaticLocalDataSet { lp_vec_t sus6vec = lp_vec_t(sid.objectId, SUS_6_VEC, this); lp_vec_t sus7vec = lp_vec_t(sid.objectId, SUS_7_VEC, this); lp_vec_t sus8vec = lp_vec_t(sid.objectId, SUS_8_VEC, this); - lp_vec_t sus9vec = lp_vec_t(sid.objectId, SUS_8_VEC, this); - lp_vec_t sus10vec = lp_vec_t(sid.objectId, SUS_8_VEC, this); - lp_vec_t sus11vec = lp_vec_t(sid.objectId, SUS_8_VEC, this); + lp_vec_t sus9vec = lp_vec_t(sid.objectId, SUS_9_VEC, this); + lp_vec_t sus10vec = lp_vec_t(sid.objectId, SUS_10_VEC, this); + lp_vec_t sus11vec = lp_vec_t(sid.objectId, SUS_11_VEC, this); lp_vec_t susVecTot = lp_vec_t(sid.objectId, SUS_VEC_TOT, this); lp_vec_t susVecTotDerivative = lp_vec_t(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this); From 153c62337fca414e1075ae1d297534ccfaf31a88 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 10:07:14 +0100 Subject: [PATCH 048/119] fixed determinant init --- mission/controller/acs/util/MathOperations.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 56665c4d..b8d1fa4d 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -279,7 +279,7 @@ class MathOperations { } static void inverseMatrixDimThree(const T1 *matrix, T1 *output) { int i, j; - double determinant; + double determinant = 0; double mat[3][3] = {{matrix[0], matrix[1], matrix[2]}, {matrix[3], matrix[4], matrix[5]}, {matrix[6], matrix[7], matrix[8]}}; From 9791c11c993bd663c437b97ec274b0689dcfca93 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 10:25:55 +0100 Subject: [PATCH 049/119] fix --- mission/controller/acs/SensorProcessing.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 5d68bc44..dabe62ee 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -590,12 +590,12 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters) { sensorValues->update(); - processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, - sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, - (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && - sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.altitude.isValid() && - sensorValues->gpsSet.unixSeconds.isValid()), - &acsParameters->gpsParameters, gpsDataProcessed); + processGps( + sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, + sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value, + (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && + sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.unixSeconds.isValid()), + &acsParameters->gpsParameters, gpsDataProcessed); processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), From 8fa1d69db3b313235f4a660876cd3fbb05acbf5a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 15:23:20 +0100 Subject: [PATCH 050/119] removed rotation for sun pointing --- mission/controller/acs/Guidance.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 9027392e..af9909c1 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -413,14 +413,8 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me double posSatE[3] = {0, 0, 0}; std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); - // Check whether Sat is above or below equator - // Assign helper vector (north pole inertial) accordingly - double helperVec[3] = {0, 0, 0}; - if (posSatE[2] > 0) { - helperVec[2] = 1; - } else { - helperVec[2] = -1; - } + // Assign helper vector (north pole inertial) + double helperVec[3] = {0, 0, 1}; // double yAxis[3] = {0, 0, 0}; From 673c24a34d9b1e075f0b5a65934828aeb55f6615 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 15:43:40 +0100 Subject: [PATCH 051/119] frmt --- mission/controller/AcsController.cpp | 1092 +++++++++++++------------- 1 file changed, 544 insertions(+), 548 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 362a5e1b..85ef8d7a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -26,7 +26,7 @@ AcsController::AcsController(object_id_t objectId) ctrlValData(this), actuatorCmdData(this) {} -ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { +ReturnValue_t AcsController::handleCommandMessage(CommandMessage* message) { ReturnValue_t result = actionHelper.handleActionMessage(message); if (result == returnvalue::OK) { return result; @@ -103,581 +103,577 @@ void AcsController::performControlOperation() { copyGyrData(); } } - } +} - void AcsController::performSafe() { - ACS::SensorValues sensorValues; +void AcsController::performSafe() { + ACS::SensorValues sensorValues; - timeval now; - Clock::getClock_timeval(&now); + timeval now; + Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); - // Give desired satellite rate and sun direction to align - double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; - guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); - // IF MEKF is working - double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - bool magMomMtqValid = false; - if (validMekf == returnvalue::OK) { - safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), - mgmDataProcessed.magIgrfModel.value, - mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, - susDataProcessed.isValid(), mekfData.satRotRateMekf.value, - mekfData.satRotRateMekf.isValid(), sunTargetDir, satRateSafe, &errAng, - magMomMtq, &magMomMtqValid); - } else { - safeCtrl.safeNoMekf( - now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), - susDataProcessed.susVecTotDerivative.value, - susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, - mgmDataProcessed.mgmVecTotDerivative.isValid(), sunTargetDir, satRateSafe, &errAng, - magMomMtq, &magMomMtqValid); - } - - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); - - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0, 0, 0, 1}; - std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.tgtQuat.setValid(false); - std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.errQuat.setValid(false); - ctrlValData.errAng.value = errAng; - ctrlValData.errAng.setValid(true); - ctrlValData.setValidity(true, false); - } - } - - // Detumble check and switch - if (mekfData.satRotRateMekf.isValid() && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else { - detumbleCounter = 0; - } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - submode = SUBMODE_DETUMBLE; - detumbleCounter = 0; - triggerEvent(SAFE_RATE_VIOLATION); - } - - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); - } - } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], - // torqueDuration); - // } - } - - void AcsController::performDetumble() { - ACS::SensorValues sensorValues; - - timeval now; - Clock::getClock_timeval(&now); - - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - - double magMomMtq[3] = {0, 0, 0}; - detumble.bDotLaw( + // Give desired satellite rate and sun direction to align + double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; + guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); + // IF MEKF is working + double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; + bool magMomMtqValid = false; + if (validMekf == returnvalue::OK) { + safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), + mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), + susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), + mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), + sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + } else { + safeCtrl.safeNoMekf( + now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), + susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); - - if (mekfData.satRotRateMekf.isValid() && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } else { - detumbleCounter = 0; - } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - submode = SUBMODE_SAFE; - detumbleCounter = 0; - } - - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); - } - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); - } - } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } + sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - void AcsController::performPointingCtrl() { - ACS::SensorValues sensorValues; + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); - timeval now; - Clock::getClock_timeval(&now); - - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - - double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - switch (submode) { - case SUBMODE_IDLE: - guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - break; - case SUBMODE_PTG_TARGET: - guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, - targetQuat, refSatRate); - break; - case SUBMODE_PTG_TARGET_GS: - guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, - now, targetQuat, refSatRate); - break; - case SUBMODE_PTG_NADIR: - guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); - break; - case SUBMODE_PTG_INERTIAL: - guidance.inertialQuatPtg(targetQuat, refSatRate); - break; + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0, 0, 0, 1}; + std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.tgtQuat.setValid(false); + std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.errQuat.setValid(false); + ctrlValData.errAng.value = errAng; + ctrlValData.errAng.setValid(true); + ctrlValData.setValidity(true, false); } - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, - deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed - guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); - double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; - ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - double rwTrqNs[4] = {0, 0, 0, 0}; - ptgCtrl.ptgNullspace( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + } - if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { - bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - int32_t rwSpeed[4] = { - (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), - (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; - ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + // Detumble check and switch + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } else { + detumbleCounter = 0; + } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_DETUMBLE; + detumbleCounter = 0; + triggerEvent(SAFE_RATE_VIOLATION); + } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], + // torqueDuration); + // } +} - double cmdSpeedRws[4] = {0, 0, 0, - 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); - double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol - ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), - mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value), +void AcsController::performDetumble() { + ACS::SensorValues sensorValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + double magMomMtq[3] = {0, 0, 0}; + detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, + mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else { + detumbleCounter = 0; + } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_SAFE; + detumbleCounter = 0; + } + + int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; + for (int i = 0; i < 3; ++i) { + cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); + } + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); + // } +} + +void AcsController::performPointingCtrl() { + ACS::SensorValues sensorValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; + switch (submode) { + case SUBMODE_IDLE: + guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); + break; + case SUBMODE_PTG_TARGET: + guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + break; + case SUBMODE_PTG_TARGET_GS: + guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); + break; + case SUBMODE_PTG_NADIR: + guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + break; + case SUBMODE_PTG_INERTIAL: + guidance.inertialQuatPtg(targetQuat, refSatRate); + break; + } + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed + guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); + double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; + ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + double rwTrqNs[4] = {0, 0, 0, 0}; + ptgCtrl.ptgNullspace( + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + + if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { + bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? + int32_t rwSpeed[4] = { + (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), + (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + } + + double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input + actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); + &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); + double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + ptgCtrl.ptgDesaturation(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); + actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); - } - int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; - for (int i = 0; i < 4; ++i) { - cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); - } - - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); - actuatorCmdData.setValidity(true, true); - } - } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } + int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; + for (int i = 0; i < 3; ++i) { + cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); + } + int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; + for (int i = 0; i < 4; ++i) { + cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); } - ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool & localDataPoolMap, - LocalDataPoolManager & poolManager) { - // MGM Raw - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); - poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0}); - // MGM Processed - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer); - localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf); - poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0}); - // SUS Raw - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw); - poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0}); - // SUS Processed - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer); - localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk); - poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0}); - // GYR Raw - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw); - poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0}); - // GYR Processed - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot); - poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0}); - // GPS Processed - localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); - localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); - localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); - localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); - poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); - // MEKF - localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); - localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); - poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0}); - // Ctrl Values - localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); - localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); - localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); - poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); - // Actuator CMD - localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); - localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); - localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); - poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0}); - return returnvalue::OK; + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + actuatorCmdData.setValidity(true, true); + } } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); + // } +} - LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { - switch (sid.ownerSetId) { - case acsctrl::MGM_SENSOR_DATA: - return &mgmDataRaw; - case acsctrl::MGM_PROCESSED_DATA: - return &mgmDataProcessed; - case acsctrl::SUS_SENSOR_DATA: - return &susDataRaw; - case acsctrl::SUS_PROCESSED_DATA: - return &susDataProcessed; - case acsctrl::GYR_SENSOR_DATA: - return &gyrDataRaw; - case acsctrl::GYR_PROCESSED_DATA: - return &gyrDataProcessed; - case acsctrl::GPS_PROCESSED_DATA: - return &gpsDataProcessed; - case acsctrl::MEKF_DATA: - return &mekfData; - case acsctrl::CTRL_VAL_DATA: - return &ctrlValData; - case acsctrl::ACTUATOR_CMD_DATA: - return &actuatorCmdData; - default: - return nullptr; - } - return nullptr; - } +ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) { + // MGM Raw + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); + poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0}); + // MGM Processed + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer); + localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf); + poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0}); + // SUS Raw + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw); + poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0}); + // SUS Processed + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer); + localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk); + poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0}); + // GYR Raw + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw); + poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0}); + // GYR Processed + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot); + poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0}); + // GPS Processed + localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); + poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); + // MEKF + localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); + localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); + poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0}); + // Ctrl Values + localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); + localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); + localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); + poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); + // Actuator CMD + localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); + localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); + localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); + poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0}); + return returnvalue::OK; +} - ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t * msToReachTheMode) { - if (mode == MODE_OFF) { - if (submode == SUBMODE_NONE) { - return returnvalue::OK; - } else { - return INVALID_SUBMODE; - } - } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode > 8) || (submode < 2)) { - return INVALID_SUBMODE; - } else { - return returnvalue::OK; - } - } - return INVALID_MODE; +LocalPoolDataSetBase* AcsController::getDataSetHandle(sid_t sid) { + switch (sid.ownerSetId) { + case acsctrl::MGM_SENSOR_DATA: + return &mgmDataRaw; + case acsctrl::MGM_PROCESSED_DATA: + return &mgmDataProcessed; + case acsctrl::SUS_SENSOR_DATA: + return &susDataRaw; + case acsctrl::SUS_PROCESSED_DATA: + return &susDataProcessed; + case acsctrl::GYR_SENSOR_DATA: + return &gyrDataRaw; + case acsctrl::GYR_PROCESSED_DATA: + return &gyrDataProcessed; + case acsctrl::GPS_PROCESSED_DATA: + return &gpsDataProcessed; + case acsctrl::MEKF_DATA: + return &mekfData; + case acsctrl::CTRL_VAL_DATA: + return &ctrlValData; + case acsctrl::ACTUATOR_CMD_DATA: + return &actuatorCmdData; + default: + return nullptr; } + return nullptr; +} - void AcsController::copyMgmData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.mgm0Lis3Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); - } +ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t* msToReachTheMode) { + if (mode == MODE_OFF) { + if (submode == SUBMODE_NONE) { + return returnvalue::OK; + } else { + return INVALID_SUBMODE; } - { - PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.mgm2Lis3Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.imtqMgmSet); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value, - 3 * sizeof(float)); - mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); - mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; - mgmDataRaw.actuationCalStatus.setValid( - sensorValues.imtqMgmSet.coilActuationStatus.isValid()); - } + } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { + if ((submode > 8) || (submode < 2)) { + return INVALID_SUBMODE; + } else { + return returnvalue::OK; } } + return INVALID_MODE; +} - void AcsController::copySusData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.susSets[0]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[1]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[2]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[3]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[4]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[5]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[6]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[7]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[8]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[9]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[10]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[11]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid()); - } +void AcsController::copyMgmData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.mgm0Lis3Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); } } + { + PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.mgm2Lis3Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.imtqMgmSet); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value, + 3 * sizeof(float)); + mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); + mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; + mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid()); + } + } +} - void AcsController::copyGyrData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.gyr0AdisSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value; - gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value; - gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value; - gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() && - sensorValues.gyr0AdisSet.angVelocY.isValid() && - sensorValues.gyr0AdisSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr1L3gSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value; - gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value; - gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value; - gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() && - sensorValues.gyr1L3gSet.angVelocY.isValid() && - sensorValues.gyr1L3gSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr2AdisSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value; - gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value; - gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value; - gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() && - sensorValues.gyr2AdisSet.angVelocY.isValid() && - sensorValues.gyr2AdisSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr3L3gSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value; - gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value; - gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value; - gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() && - sensorValues.gyr3L3gSet.angVelocY.isValid() && - sensorValues.gyr3L3gSet.angVelocZ.isValid()); - } +void AcsController::copySusData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.susSets[0]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); } } + { + PoolReadGuard pg(&sensorValues.susSets[1]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[2]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[3]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[4]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[5]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[6]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[7]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[8]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[9]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[10]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[11]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid()); + } + } +} + +void AcsController::copyGyrData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.gyr0AdisSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value; + gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value; + gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value; + gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() && + sensorValues.gyr0AdisSet.angVelocY.isValid() && + sensorValues.gyr0AdisSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr1L3gSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value; + gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value; + gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value; + gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() && + sensorValues.gyr1L3gSet.angVelocY.isValid() && + sensorValues.gyr1L3gSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr2AdisSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value; + gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value; + gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value; + gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() && + sensorValues.gyr2AdisSet.angVelocY.isValid() && + sensorValues.gyr2AdisSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr3L3gSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value; + gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value; + gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value; + gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() && + sensorValues.gyr3L3gSet.angVelocY.isValid() && + sensorValues.gyr3L3gSet.angVelocZ.isValid()); + } + } +} From 9c4b2872e336c2ee91c65f81b03f212d7d1957db Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 17 Jan 2023 11:50:52 +0100 Subject: [PATCH 052/119] fixed susVecTotDerivative valid flag --- mission/controller/acs/SensorProcessing.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index dabe62ee..815b3a6b 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -397,6 +397,7 @@ void SensorProcessing::processSus( for (uint8_t i = 0; i < 3; i++) { susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; savedSusVecTot[i] = susVecTot[i]; + susVecTotDerivativeValid = true; } } timeOfSavedSusDirEst = timeOfSusMeasurement; From 44b384cd17d20be0d1168d26905e5a0ffc9a761c Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Tue, 17 Jan 2023 20:11:45 +0100 Subject: [PATCH 053/119] access to different paramset for every pointing mode --- mission/controller/AcsController.cpp | 89 +++++++++++++++++----- mission/controller/acs/control/PtgCtrl.cpp | 8 +- mission/controller/acs/control/PtgCtrl.h | 11 ++- 3 files changed, 76 insertions(+), 32 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index dee2fb73..f829c3e3 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -271,53 +271,106 @@ void AcsController::performControlOperation() { double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double quatRef[4] = {0, 0, 0, 0}; uint8_t enableAntiStiction = true; + + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed + double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + 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}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + switch (submode) { case SUBMODE_IDLE: guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, 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); break; + case SUBMODE_PTG_TARGET: guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, 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); break; + case SUBMODE_PTG_TARGET_GS: guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, 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); break; + case SUBMODE_PTG_NADIR: guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.nadirModeControllerParameters, 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); break; + case SUBMODE_PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); + ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, + &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation(&acsParameters.inertialModeControllerParameters, 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); break; } - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, - deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); - double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; - ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - double rwTrqNs[4] = {0, 0, 0, 0}; - ptgCtrl.ptgNullspace( - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); if ( enableAntiStiction ) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? @@ -333,12 +386,6 @@ void AcsController::performControlOperation() { &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); - double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol - ptgCtrl.ptgDesaturation(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); actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 53544b8c..6d41e85d 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -21,14 +21,12 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter PtgCtrl::~PtgCtrl() {} void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - // TODO: Here correct Parameters have to be loaded according to current submode - pointingLawParameters = &(acsParameters_->targetModeControllerParameters); inertiaEIVE = &(acsParameters_->inertiaEIVE); rwHandlingParameters = &(acsParameters_->rwHandlingParameters); rwMatrices = &(acsParameters_->rwMatrices); } -void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate, +void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix @@ -108,7 +106,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } -void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, +void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes) { if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { @@ -139,7 +137,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); } -void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, +void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 85f13a7a..b036d180 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -41,14 +41,14 @@ class PtgCtrl { /* @brief: Calculates the needed torque for the pointing control mechanism * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters */ - void ptgLaw(const double mode, const double *qError, const double *deltaRate, + void ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, - double *mgtDpDes); + void ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, + bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, + int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes); - void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + void ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs); /* @brief: Commands the stiction torque in case wheel speed is to low @@ -59,7 +59,6 @@ class PtgCtrl { void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand); private: - AcsParameters::PointingLawParameters *pointingLawParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::RwMatrices *rwMatrices; From 19c8703e15d745f1d910d3d1c78e8235ed2cff17 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 18 Jan 2023 11:38:09 +0100 Subject: [PATCH 054/119] corrected to actual used devicedef for L3 gyt --- mission/controller/acs/SensorValues.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index ef897207..ec1795fc 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,12 +1,12 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#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" #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" -#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h" #include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" From 638573757b8f691960fafac9981d6db1fc3966dd Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 19 Jan 2023 11:39:59 +0100 Subject: [PATCH 055/119] swapped SUS0 and SUS6 in ObjectFactory --- bsp_q7s/fmObjectFactory.cpp | 2 +- linux/ObjectFactory.cpp | 17 +++++++++++++---- linux/ObjectFactory.h | 2 +- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 239b3639..44b53225 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -39,7 +39,7 @@ void ObjectFactory::produce(void* args) { createRadSensorComponent(gpioComIF, *stackHandler); #endif #if OBSW_ADD_SUN_SENSORS == 1 - createSunSensorComponents(gpioComIF, spiMainComIF, pwrSwitcher, q7s::SPI_DEFAULT_DEV); + createSunSensorComponents(gpioComIF, spiMainComIF, pwrSwitcher, q7s::SPI_DEFAULT_DEV, true); #endif #if OBSW_ADD_ACS_BOARD == 1 diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index e35d7e96..af968ddd 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -30,7 +30,8 @@ #include "mission/system/tree/payloadModeTree.h" void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiComIF, - PowerSwitchIF* pwrSwitcher, std::string spiDev) { + PowerSwitchIF* pwrSwitcher, std::string spiDev, + bool swap0And6) { using namespace gpio; GpioCookie* gpioCookieSus = new GpioCookie(); GpioCallback* susgpio = nullptr; @@ -77,7 +78,11 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo #if OBSW_ADD_SUN_SENSORS == 1 SusFdir* fdir = nullptr; std::array susHandlers = {}; - SpiCookie* spiCookie = new SpiCookie(addresses::SUS_0, gpioIds::CS_SUS_0, SUS::MAX_CMD_SIZE, + gpioId_t gpioId = gpioIds::CS_SUS_0; + if (swap0And6) { + gpioId = gpioIds::CS_SUS_6; + } + SpiCookie* spiCookie = new SpiCookie(addresses::SUS_0, gpioId, SUS::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); susHandlers[0] = new SusHandler(objects::SUS_0_N_LOC_XFYFZM_PT_XF, 0, objects::SPI_MAIN_COM_IF, spiCookie); @@ -119,8 +124,12 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo fdir = new SusFdir(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); susHandlers[5]->setCustomFdir(fdir); - spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE, - spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ); + gpioId = gpioIds::CS_SUS_6; + if (swap0And6) { + gpioId = gpioIds::CS_SUS_0; + } + spiCookie = new SpiCookie(addresses::SUS_6, gpioId, SUS::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE, + spi::SUS_MAX1227_SPI_FREQ); susHandlers[6] = new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SPI_MAIN_COM_IF, spiCookie); fdir = new SusFdir(objects::SUS_6_R_LOC_XFYBZM_PT_XF); diff --git a/linux/ObjectFactory.h b/linux/ObjectFactory.h index 3704e9bd..bdfa846f 100644 --- a/linux/ObjectFactory.h +++ b/linux/ObjectFactory.h @@ -19,7 +19,7 @@ class AcsController; namespace ObjectFactory { void createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher, - std::string spiDev); + std::string spiDev, bool swap0And6); void createRtdComponents(std::string spiDev, GpioIF* gpioComIF, PowerSwitchIF* pwrSwitcher, SpiComIF* comIF); From e5b297a513c4238b87bced71eaa17d113d4c412a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 23 Jan 2023 15:58:57 +0100 Subject: [PATCH 056/119] removed deprecated code --- mission/controller/acs/Guidance.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index af9909c1..56355b42 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -409,10 +409,6 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me double zAxis[3] = {0, 0, 0}; VectorOperations::normalize(sunDirB, zAxis, 3); - // Position of satellite in ECEF - double posSatE[3] = {0, 0, 0}; - std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double)); - // Assign helper vector (north pole inertial) double helperVec[3] = {0, 0, 1}; From adef468c0bc902923f396a92bda4d68a99e7c880 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Mon, 23 Jan 2023 16:34:52 +0100 Subject: [PATCH 057/119] applyed CppStyle Format --- mission/controller/AcsController.cpp | 1270 ++++++++++---------- mission/controller/acs/AcsParameters.h | 59 +- mission/controller/acs/Guidance.cpp | 31 +- mission/controller/acs/Guidance.h | 8 +- mission/controller/acs/control/PtgCtrl.cpp | 15 +- mission/controller/acs/control/PtgCtrl.h | 14 +- 6 files changed, 714 insertions(+), 683 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f829c3e3..61ddae44 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -26,7 +26,7 @@ AcsController::AcsController(object_id_t objectId) ctrlValData(this), actuatorCmdData(this) {} -ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { +ReturnValue_t AcsController::handleCommandMessage(CommandMessage* message) { ReturnValue_t result = actionHelper.handleActionMessage(message); if (result == returnvalue::OK) { return result; @@ -103,642 +103,662 @@ void AcsController::performControlOperation() { copyGyrData(); } } - } +} - void AcsController::performSafe() { - ACS::SensorValues sensorValues; +void AcsController::performSafe() { + ACS::SensorValues sensorValues; - timeval now; - Clock::getClock_timeval(&now); + timeval now; + Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); - // Give desired satellite rate and sun direction to align - double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; - guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); - // IF MEKF is working - double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - bool magMomMtqValid = false; - if (validMekf == returnvalue::OK) { - safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), - mgmDataProcessed.magIgrfModel.value, - mgmDataProcessed.magIgrfModel.isValid(), susDataProcessed.sunIjkModel.value, - susDataProcessed.isValid(), mekfData.satRotRateMekf.value, - mekfData.satRotRateMekf.isValid(), sunTargetDir, satRateSafe, &errAng, - magMomMtq, &magMomMtqValid); - } else { - safeCtrl.safeNoMekf( - now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), - susDataProcessed.susVecTotDerivative.value, - susDataProcessed.susVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, - mgmDataProcessed.mgmVecTotDerivative.isValid(), sunTargetDir, satRateSafe, &errAng, - magMomMtq, &magMomMtqValid); - } - - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); - - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0, 0, 0, 1}; - std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.tgtQuat.setValid(false); - std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.errQuat.setValid(false); - ctrlValData.errAng.value = errAng; - ctrlValData.errAng.setValid(true); - ctrlValData.setValidity(true, false); - } - } - - // Detumble check and switch - if (mekfData.satRotRateMekf.isValid() && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else { - detumbleCounter = 0; - } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - submode = SUBMODE_DETUMBLE; - detumbleCounter = 0; - triggerEvent(SAFE_RATE_VIOLATION); - } - - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); - } - } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], - // torqueDuration); - // } - } - - void AcsController::performDetumble() { - ACS::SensorValues sensorValues; - - timeval now; - Clock::getClock_timeval(&now); - - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - - double magMomMtq[3] = {0, 0, 0}; - detumble.bDotLaw( + // Give desired satellite rate and sun direction to align + double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; + guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); + // IF MEKF is working + double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; + bool magMomMtqValid = false; + if (validMekf == returnvalue::OK) { + safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), + mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(), + susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), + mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), + sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + } else { + safeCtrl.safeNoMekf( + now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), + susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); + } - if (mekfData.satRotRateMekf.isValid() && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + + { + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0, 0, 0, 1}; + std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.tgtQuat.setValid(false); + std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); + ctrlValData.errQuat.setValid(false); + ctrlValData.errAng.value = errAng; + ctrlValData.errAng.setValid(true); + ctrlValData.setValidity(true, false); + } + } + + // Detumble check and switch + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } else { + detumbleCounter = 0; + } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_DETUMBLE; + detumbleCounter = 0; + triggerEvent(SAFE_RATE_VIOLATION); + } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], + // torqueDuration); + // } +} + +void AcsController::performDetumble() { + ACS::SensorValues sensorValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + double magMomMtq[3] = {0, 0, 0}; + detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, + mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, + mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } else { + detumbleCounter = 0; + } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_SAFE; + detumbleCounter = 0; + } + + int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; + for (int i = 0; i < 3; ++i) { + cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); + } + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); + // } +} + +void AcsController::performPointingCtrl() { + ACS::SensorValues sensorValues; + + timeval now; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); + + double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; + double quatRef[4] = {0, 0, 0, 0}; + uint8_t enableAntiStiction = true; + + double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, + deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed + double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + 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}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + + switch (submode) { + case SUBMODE_IDLE: + guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, + 4 * sizeof(double)); + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, + deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation( + &acsParameters.targetModeControllerParameters, 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); + + break; + + case SUBMODE_PTG_TARGET: + guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, + 4 * sizeof(double)); + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, + deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation( + &acsParameters.targetModeControllerParameters, 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); + break; + + case SUBMODE_PTG_TARGET_GS: + guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, + targetQuat, refSatRate); + std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, + 4 * sizeof(double)); + enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, + deltaRate); + ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation( + &acsParameters.targetModeControllerParameters, 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); + break; + + case SUBMODE_PTG_NADIR: + guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, + refSatRate); + std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); + enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, + deltaRate); + ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation( + &acsParameters.nadirModeControllerParameters, 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); + break; + + case SUBMODE_PTG_INERTIAL: + guidance.inertialQuatPtg(targetQuat, refSatRate); + std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, + 4 * sizeof(double)); + enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; + guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, + deltaRate); + ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, + *rwPseudoInv, torquePtgRws); + ptgCtrl.ptgNullspace( + &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); + actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); + ptgCtrl.ptgDesaturation( + &acsParameters.inertialModeControllerParameters, 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); + break; + } + + if (enableAntiStiction) { + bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? + int32_t rwSpeed[4] = { + (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), + (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); + } + + double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input + actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), + &(sensorValues.rw2Set.currSpeed.value), + &(sensorValues.rw3Set.currSpeed.value), + &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); + actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); + + int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; + for (int i = 0; i < 3; ++i) { + cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); + } + int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; + for (int i = 0; i < 4; ++i) { + cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); + } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + actuatorCmdData.setValidity(true, true); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); + // } +} + +ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) { + // MGM Raw + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); + poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0}); + // MGM Processed + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot); + localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer); + localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf); + poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0}); + // SUS Raw + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw); + poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0}); + // SUS Processed + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer); + localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk); + poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0}); + // GYR Raw + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw); + poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0}); + // GYR Processed + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc); + localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot); + poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0}); + // GPS Processed + localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); + localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); + poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); + // MEKF + localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); + localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); + poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0}); + // Ctrl Values + localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); + localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); + localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); + poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); + // Actuator CMD + localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); + localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); + localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); + poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0}); + return returnvalue::OK; +} + +LocalPoolDataSetBase* AcsController::getDataSetHandle(sid_t sid) { + switch (sid.ownerSetId) { + case acsctrl::MGM_SENSOR_DATA: + return &mgmDataRaw; + case acsctrl::MGM_PROCESSED_DATA: + return &mgmDataProcessed; + case acsctrl::SUS_SENSOR_DATA: + return &susDataRaw; + case acsctrl::SUS_PROCESSED_DATA: + return &susDataProcessed; + case acsctrl::GYR_SENSOR_DATA: + return &gyrDataRaw; + case acsctrl::GYR_PROCESSED_DATA: + return &gyrDataProcessed; + case acsctrl::GPS_PROCESSED_DATA: + return &gpsDataProcessed; + case acsctrl::MEKF_DATA: + return &mekfData; + case acsctrl::CTRL_VAL_DATA: + return &ctrlValData; + case acsctrl::ACTUATOR_CMD_DATA: + return &actuatorCmdData; + default: + return nullptr; + } + return nullptr; +} + +ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t* msToReachTheMode) { + if (mode == MODE_OFF) { + if (submode == SUBMODE_NONE) { + return returnvalue::OK; } else { - detumbleCounter = 0; + return INVALID_SUBMODE; } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { - submode = SUBMODE_SAFE; - detumbleCounter = 0; - } - - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); - } - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double)); - actuatorCmdData.rwTargetTorque.setValid(false); - std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); - actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); - actuatorCmdData.mtqTargetDipole.setValid(true); - actuatorCmdData.setValidity(true, false); - } - } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } - } - - void AcsController::performPointingCtrl() { - ACS::SensorValues sensorValues; - - timeval now; - Clock::getClock_timeval(&now); - - sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, - &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, - &mekfData, &validMekf); - - double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - double quatRef[4] = {0, 0, 0, 0}; - uint8_t enableAntiStiction = true; - - double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, - deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed - double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - 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}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol - - switch (submode) { - case SUBMODE_IDLE: - guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); - ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, 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); - - break; - - case SUBMODE_PTG_TARGET: - guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, - targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); - ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, 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); - break; - - case SUBMODE_PTG_TARGET_GS: - guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, - now, targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); - ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); - ptgCtrl.ptgDesaturation(&acsParameters.targetModeControllerParameters, 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); - break; - - case SUBMODE_PTG_NADIR: - guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, - refSatRate); - std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); - ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); - ptgCtrl.ptgDesaturation(&acsParameters.nadirModeControllerParameters, 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); - break; - - case SUBMODE_PTG_INERTIAL: - guidance.inertialQuatPtg(targetQuat, refSatRate); - std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); - enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; - guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate); - ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); - VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); - actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); - ptgCtrl.ptgDesaturation(&acsParameters.inertialModeControllerParameters, 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); - break; - } - - if ( enableAntiStiction ) { - bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - int32_t rwSpeed[4] = { - (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), - (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; - ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); - } - - double cmdSpeedRws[4] = {0, 0, 0, - 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); - - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); - } - int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; - for (int i = 0; i < 4; ++i) { - cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); - } - - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); - actuatorCmdData.setValidity(true, true); - } - } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } - } - - ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool & localDataPoolMap, - LocalDataPoolManager & poolManager) { - // MGM Raw - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); - poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0}); - // MGM Processed - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer); - localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf); - poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0}); - // SUS Raw - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw); - poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0}); - // SUS Processed - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer); - localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk); - poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0}); - // GYR Raw - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw); - poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0}); - // GYR Processed - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc); - localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot); - poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0}); - // GPS Processed - localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude); - localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude); - localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition); - localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity); - poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0}); - // MEKF - localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf); - localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf); - poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0}); - // Ctrl Values - localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); - localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); - localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); - poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0}); - // Actuator CMD - localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque); - localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); - localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); - poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0}); - return returnvalue::OK; - } - - LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { - switch (sid.ownerSetId) { - case acsctrl::MGM_SENSOR_DATA: - return &mgmDataRaw; - case acsctrl::MGM_PROCESSED_DATA: - return &mgmDataProcessed; - case acsctrl::SUS_SENSOR_DATA: - return &susDataRaw; - case acsctrl::SUS_PROCESSED_DATA: - return &susDataProcessed; - case acsctrl::GYR_SENSOR_DATA: - return &gyrDataRaw; - case acsctrl::GYR_PROCESSED_DATA: - return &gyrDataProcessed; - case acsctrl::GPS_PROCESSED_DATA: - return &gpsDataProcessed; - case acsctrl::MEKF_DATA: - return &mekfData; - case acsctrl::CTRL_VAL_DATA: - return &ctrlValData; - case acsctrl::ACTUATOR_CMD_DATA: - return &actuatorCmdData; - default: - return nullptr; - } - return nullptr; - } - - ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t * msToReachTheMode) { - if (mode == MODE_OFF) { - if (submode == SUBMODE_NONE) { - return returnvalue::OK; - } else { - return INVALID_SUBMODE; - } - } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode > 8) || (submode < 2)) { - return INVALID_SUBMODE; - } else { - return returnvalue::OK; - } - } - return INVALID_MODE; - } - - void AcsController::copyMgmData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.mgm0Lis3Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.mgm2Lis3Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, - 3 * sizeof(float)); - mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.imtqMgmSet); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value, - 3 * sizeof(float)); - mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); - mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; - mgmDataRaw.actuationCalStatus.setValid( - sensorValues.imtqMgmSet.coilActuationStatus.isValid()); - } + } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { + if ((submode > 8) || (submode < 2)) { + return INVALID_SUBMODE; + } else { + return returnvalue::OK; } } + return INVALID_MODE; +} - void AcsController::copySusData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.susSets[0]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[1]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[2]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[3]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[4]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[5]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[6]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[7]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[8]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[9]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[10]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.susSets[11]); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value, - 6 * sizeof(uint16_t)); - susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid()); - } +void AcsController::copyMgmData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.mgm0Lis3Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); } } - - void AcsController::copyGyrData() { - ACS::SensorValues sensorValues; - { - PoolReadGuard pg(&sensorValues.gyr0AdisSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value; - gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value; - gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value; - gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() && - sensorValues.gyr0AdisSet.angVelocY.isValid() && - sensorValues.gyr0AdisSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr1L3gSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value; - gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value; - gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value; - gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() && - sensorValues.gyr1L3gSet.angVelocY.isValid() && - sensorValues.gyr1L3gSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr2AdisSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value; - gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value; - gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value; - gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() && - sensorValues.gyr2AdisSet.angVelocY.isValid() && - sensorValues.gyr2AdisSet.angVelocZ.isValid()); - } - } - { - PoolReadGuard pg(&sensorValues.gyr3L3gSet); - if (pg.getReadResult() == returnvalue::OK) { - gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value; - gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value; - gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value; - gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() && - sensorValues.gyr3L3gSet.angVelocY.isValid() && - sensorValues.gyr3L3gSet.angVelocZ.isValid()); - } + { + PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); } } + { + PoolReadGuard pg(&sensorValues.mgm2Lis3Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.imtqMgmSet); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value, + 3 * sizeof(float)); + mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid()); + mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value; + mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid()); + } + } +} + +void AcsController::copySusData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.susSets[0]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[1]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[2]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[3]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[4]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[5]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[6]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[7]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[8]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[9]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[10]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.susSets[11]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid()); + } + } +} + +void AcsController::copyGyrData() { + ACS::SensorValues sensorValues; + { + PoolReadGuard pg(&sensorValues.gyr0AdisSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value; + gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value; + gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value; + gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() && + sensorValues.gyr0AdisSet.angVelocY.isValid() && + sensorValues.gyr0AdisSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr1L3gSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value; + gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value; + gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value; + gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() && + sensorValues.gyr1L3gSet.angVelocY.isValid() && + sensorValues.gyr1L3gSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr2AdisSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value; + gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value; + gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value; + gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() && + sensorValues.gyr2AdisSet.angVelocY.isValid() && + sensorValues.gyr2AdisSet.angVelocZ.isValid()); + } + } + { + PoolReadGuard pg(&sensorValues.gyr3L3gSet); + if (pg.getReadResult() == returnvalue::OK) { + gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value; + gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value; + gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value; + gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() && + sensorValues.gyr3L3gSet.angVelocY.isValid() && + sensorValues.gyr3L3gSet.angVelocZ.isValid()); + } + } +} diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 80a3a65f..a0e8af0f 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -816,49 +816,48 @@ class AcsParameters : public HasParametersIF { } safeModeControllerParameters; struct PointingLawParameters { + double zeta = 0.3; + double om = 0.3; + double omMax = 1 * M_PI / 180; + double qiMin = 0.1; + double gainNullspace = 0.01; - double zeta = 0.3; - double om = 0.3; - double omMax = 1 * M_PI / 180; - double qiMin = 0.1; - double gainNullspace = 0.01; - - double desatMomentumRef[3] = {0, 0, 0}; - double deSatGainFactor = 1000; - uint8_t desatOn = true; - uint8_t enableAntiStiction = true; + double desatMomentumRef[3] = {0, 0, 0}; + double deSatGainFactor = 1000; + uint8_t desatOn = true; + uint8_t enableAntiStiction = true; } pointingLawParameters; struct TargetModeControllerParameters : PointingLawParameters { - double refDirection[3] = {-1, 0, 0}; // Antenna - double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to - // give this as an input- currently en calculation is done - double quatRef[4] = {0, 0, 0, 1}; - int8_t timeElapsedMax = 10; // rot rate calculations + double refDirection[3] = {-1, 0, 0}; // Antenna + double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to + // give this as an input- currently en calculation is done + double quatRef[4] = {0, 0, 0, 1}; + int8_t timeElapsedMax = 10; // rot rate calculations - // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] + // Default is Stuttgart GS + double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude + double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude + double altitudeTgt = 500; // [m] - // For one-axis control: - uint8_t avoidBlindStr = true; - double blindAvoidStart = 1.5; - double blindAvoidStop = 2.5; - double blindRotRate = 1 * M_PI / 180; + // For one-axis control: + uint8_t avoidBlindStr = true; + double blindAvoidStart = 1.5; + double blindAvoidStop = 2.5; + double blindRotRate = 1 * M_PI / 180; } targetModeControllerParameters; struct NadirModeControllerParameters : PointingLawParameters { - double refDirection[3] = {-1, 0, 0}; // Antenna - double quatRef[4] = {0, 0, 0, 1}; - int8_t timeElapsedMax = 10; // rot rate calculations + double refDirection[3] = {-1, 0, 0}; // Antenna + double quatRef[4] = {0, 0, 0, 1}; + int8_t timeElapsedMax = 10; // rot rate calculations } nadirModeControllerParameters; struct InertialModeControllerParameters : PointingLawParameters { - double tgtQuat[4] = {0, 0, 0, 1}; - double refRotRate[3] = {0, 0, 0}; - double quatRef[4] = {0, 0, 0, 1}; + double tgtQuat[4] = {0, 0, 0, 1}; + double refRotRate[3] = {0, 0, 0}; + double quatRef[4] = {0, 0, 0, 1}; } inertialModeControllerParameters; struct StrParameters { diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4ba72674..48c148c9 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -42,7 +42,8 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.latitudeTgt, + acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS @@ -172,14 +173,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl: } } -void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate) { +void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *refSatRate) { //------------------------------------------------------------------------------------- // Calculation of reference rotation rate //------------------------------------------------------------------------------------- - double timeElapsed = - now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternion.tv_sec + - timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); + double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - + (timeSavedQuaternion.tv_sec + + timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); if (timeElapsed < timeElapsedMax) { double qDiff[4] = {0, 0, 0, 0}; VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); @@ -226,7 +227,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, double targetCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.latitudeTgt, + acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, targetCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; @@ -307,7 +309,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat double groundStationCart[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt( - acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt, + acsParameters.targetModeControllerParameters.latitudeTgt, + acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart); // Position of the satellite in the earth/fixed frame via GPS double posSatE[3] = {0, 0, 0}; @@ -588,13 +591,15 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues, } void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) { - std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, 4 * sizeof(double)); - std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, 3 * sizeof(double)); + std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, + 4 * sizeof(double)); + std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, + 3 * sizeof(double)); } -void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]) { - +void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], + double refSatRate[3], double quatErrorComplete[4], double quatError[3], + double deltaRate[3]) { double satRate[3] = {0, 0, 0}; std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index e399af8a..5203ee48 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -58,10 +58,12 @@ class Guidance { // @note: compares target Quaternion and reference quaternion, also actual satellite rate and // desired - void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3], - double quatErrorComplete[4], double quatError[3], double deltaRate[3]); + void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], + double refSatRate[3], double quatErrorComplete[4], double quatError[3], + double deltaRate[3]); - void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate); + void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], + double *refSatRate); // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // reation wheel maybe can be done in "commanding.h" diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 6d41e85d..352c0319 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -26,8 +26,9 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { rwMatrices = &(acsParameters_->rwMatrices); } -void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate, - const double *rwPseudoInv, double *torqueRws) { +void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *qError, const double *deltaRate, const double *rwPseudoInv, + double *torqueRws) { //------------------------------------------------------------------------------------------------ // Compute gain matrix K and P matrix //------------------------------------------------------------------------------------------------ @@ -106,7 +107,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameter VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } -void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, bool magFieldEstValid, double *satRate, +void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes) { if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { @@ -127,8 +129,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLaw VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); // calculating momentum error double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract( - momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3); + VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, + deltaMomentum, 3); // resulting magnetic dipole command double crossMomentumMagField[3] = {0, 0, 0}; VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); @@ -137,7 +139,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLaw VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); } -void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, +void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index b036d180..559216f7 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -41,14 +41,16 @@ class PtgCtrl { /* @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); + void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, + const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, - bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, - int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes); + void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + double *magFieldEst, bool magFieldEstValid, double *satRate, + int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, + double *mgtDpDes); - void ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, + const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs); /* @brief: Commands the stiction torque in case wheel speed is to low From 7e25f5012e81930d94a69ada71f324bc449b316c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 26 Jan 2023 17:09:34 +0100 Subject: [PATCH 058/119] transformation before calibration --- mission/controller/acs/SensorProcessing.cpp | 67 +++++++++++---------- 1 file changed, 34 insertions(+), 33 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index ac2e5752..a8fd1576 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -47,71 +47,72 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0}, mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0}, mgm4ValueNoBias[3] = {0, 0, 0}; - float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0}, - mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0}; float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0}, mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0}; + float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0}, + mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0}; float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; if (mgm0valid) { - VectorOperations::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias, - 3); + MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, + mgm0ValueBody, 3, 3, 1); + VectorOperations::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset, + mgm0ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias, mgm0ValueCalib, 3, 3, 1); - MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib, - mgm0ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { - sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i]; + sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; } } if (mgm1valid) { - VectorOperations::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias, - 3); + MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, + mgm1ValueBody, 3, 3, 1); + VectorOperations::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset, + mgm1ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias, mgm1ValueCalib, 3, 3, 1); - MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib, - mgm1ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { - sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i]; + sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; } } if (mgm2valid) { - VectorOperations::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias, - 3); + MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, + mgm2ValueBody, 3, 3, 1); + VectorOperations::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset, + mgm2ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias, mgm2ValueCalib, 3, 3, 1); - MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib, - mgm2ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { - sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i]; + sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; } } if (mgm3valid) { - VectorOperations::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias, - 3); + MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, + mgm3ValueBody, 3, 3, 1); + VectorOperations::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset, + mgm3ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias, mgm3ValueCalib, 3, 3, 1); - MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib, - mgm3ValueBody, 3, 3, 1); for (uint8_t i = 0; i < 3; i++) { - sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i]; + sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; } } if (mgm4valid) { - float mgm4ValueNT[3]; - VectorOperations::mulScalar(mgm4Value, 1e-3, mgm4ValueNT, 3); // uT to nT - VectorOperations::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset, + float mgm4ValueUT[3]; + VectorOperations::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3); // nT to uT + MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT, + mgm4ValueBody, 3, 3, 1); + VectorOperations::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias, mgm4ValueCalib, 3, 3, 1); - MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib, - mgm4ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { - sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i]; + sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; } } @@ -148,15 +149,15 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float)); mgmDataProcessed->mgm0vec.setValid(mgm0valid); - std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float)); mgmDataProcessed->mgm1vec.setValid(mgm1valid); - std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float)); mgmDataProcessed->mgm2vec.setValid(mgm2valid); - std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float)); mgmDataProcessed->mgm3vec.setValid(mgm3valid); - std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float)); mgmDataProcessed->mgm4vec.setValid(mgm4valid); std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double)); mgmDataProcessed->mgmVecTot.setValid(true); From 03ec64672bfbaf6da0b7d517088f1518038a19a4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 26 Jan 2023 17:15:04 +0100 Subject: [PATCH 059/119] also fixed validity flag --- mission/controller/acs/SensorProcessing.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index a8fd1576..5b4ddc84 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -129,6 +129,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const for (uint8_t i = 0; i < 3; i++) { mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; savedMgmVecTot[i] = mgmVecTot[i]; + mgmVecTotDerivativeValid = true; } } timeOfSavedMagFieldEst = timeOfMgmMeasurement; From 24d2405fc8f6841db776f2f1e7315ce60a497f24 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 27 Jan 2023 11:18:55 +0100 Subject: [PATCH 060/119] added calibration matrices for testing --- mission/controller/acs/AcsParameters.h | 36 ++++++++++++++------------ 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index a62b373d..2fd5ab27 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -37,23 +37,27 @@ class AcsParameters /*: public HasParametersIF*/ { float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; - float mgm0hardIronOffset[3] = {0.0, 0.0, 0.0};//{19.89364, -29.94111, -31.07508}; - float mgm1hardIronOffset[3] = {0.0, 0.0, 0.0};//{10.95500, -8.053403, -33.36383}; - float mgm2hardIronOffset[3] = {0.0, 0.0, 0.0};//{15.72181, -26.87090, -62.19010}; - float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0}; - float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0}; + float mgm0hardIronOffset[3] = {-3.009521e1, 1.600983e1, -3.025359e1}; + float mgm1hardIronOffset[3] = {-3.255443e1, 1.118625e1, -8.400313e0}; + float mgm2hardIronOffset[3] = {-6.194874e1, 1.561503e1, -2.911635e1}; + float mgm3hardIronOffset[3] = {-3.986654e1, 1.413371e1, -9.778686e0}; + float mgm4hardIronOffset[3] = {-3.046779e1, 1.690829e1, -6.597055e0}; - float mgm0softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};/*{{1420.727e-3, 9.352825e-3, -127.1979e-3}, - {9.352825e-3, 1031.965e-3, -80.02734e-3}, - {-127.1979e-3, -80.02734e-3, 934.8899e-3}};*/ - float mgm1softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};/*{{126.7325e-2, -4.146410e-2, -18.37963e-2}, - {-4.146410e-2, 109.3310e-2, -5.246314e-2}, - {-18.37963e-2, -5.246314e-2, 105.7300e-2}};*/ - float mgm2softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};/*{{143.0438e-2, 7.095763e-2, 15.67482e-2}, - {7.095763e-2, 99.65167e-2, -6.958760e-2}, - {15.67482e-2, -6.958760e-2, 94.50124e-2}};*/ - float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; - float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + float mgm0softIronInverse[3][3] = {{9.385532e-1, -1.261877e-1, -8.086912e-2}, + {-1.261877e-1, 1.415967e0, -6.953750e-3}, + {-8.086912e-2, -6.953750e-3, 1.014963e0}}; + float mgm1softIronInverse[3][3] = {{1.054248e0, -1.867005e-1, -4.884622e-2}, + {-1.867005e-1, 1.272026e0, -4.212676e-2}, + {-4.884622e-2, -4.212676e-2, 1.091061e0}}; + float mgm2softIronInverse[3][3] = {{9.474586e-1, 1.506282e-1, -7.002351e-2}, + {1.506282e-1, 1.427704e0, 7.247630e-2}, + {-7.002351e-2, 7.247630e-2, 9.822354e-1}}; + float mgm3softIronInverse[3][3] = {{1.082337e0, 1.971590e-1, -6.910370e-2}, + {1.971590e-1, 1.265329e0, 1.301571e-2}, + {-6.910370e-2, 1.301571e-2, 1.072459e0}}; + float mgm4softIronInverse[3][3] = {{1.094184e0, -1.765240e-2, -3.580871e-2}, + {-1.765240e-2, 1.250533e0, 8.302781e-3}, + {-3.580871e-2, 8.302781e-3, 1.325068e0}}; float mgm02variance[3] = {1, 1, 1}; float mgm13variance[3] = {1, 1, 1}; float mgm4variance[3] = {1, 1, 1}; From 45eadb8302ce00f9bd092d719492c08f8bcfdc62 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Tue, 31 Jan 2023 10:08:28 +0100 Subject: [PATCH 061/119] added new calibration matrices --- mission/controller/acs/AcsParameters.h | 40 +++++++++++++------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 2fd5ab27..1fe5f29c 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -37,27 +37,27 @@ class AcsParameters /*: public HasParametersIF*/ { float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; - float mgm0hardIronOffset[3] = {-3.009521e1, 1.600983e1, -3.025359e1}; - float mgm1hardIronOffset[3] = {-3.255443e1, 1.118625e1, -8.400313e0}; - float mgm2hardIronOffset[3] = {-6.194874e1, 1.561503e1, -2.911635e1}; - float mgm3hardIronOffset[3] = {-3.986654e1, 1.413371e1, -9.778686e0}; - float mgm4hardIronOffset[3] = {-3.046779e1, 1.690829e1, -6.597055e0}; + float mgm0hardIronOffset[3] = {3.303907e0,4.009162e0,-1.677380e1}; + float mgm1hardIronOffset[3] = {-2.494258e0,3.526886e0,3.593006e0}; + float mgm2hardIronOffset[3] = {-1.630907e1,5.543566e0,-1.619287e1}; + float mgm3hardIronOffset[3] = {-8.612960e-1,2.752825e0,-1.101764e0}; + float mgm4hardIronOffset[3] = {1.696131e0,4.624440e0,2.449785e-1}; - float mgm0softIronInverse[3][3] = {{9.385532e-1, -1.261877e-1, -8.086912e-2}, - {-1.261877e-1, 1.415967e0, -6.953750e-3}, - {-8.086912e-2, -6.953750e-3, 1.014963e0}}; - float mgm1softIronInverse[3][3] = {{1.054248e0, -1.867005e-1, -4.884622e-2}, - {-1.867005e-1, 1.272026e0, -4.212676e-2}, - {-4.884622e-2, -4.212676e-2, 1.091061e0}}; - float mgm2softIronInverse[3][3] = {{9.474586e-1, 1.506282e-1, -7.002351e-2}, - {1.506282e-1, 1.427704e0, 7.247630e-2}, - {-7.002351e-2, 7.247630e-2, 9.822354e-1}}; - float mgm3softIronInverse[3][3] = {{1.082337e0, 1.971590e-1, -6.910370e-2}, - {1.971590e-1, 1.265329e0, 1.301571e-2}, - {-6.910370e-2, 1.301571e-2, 1.072459e0}}; - float mgm4softIronInverse[3][3] = {{1.094184e0, -1.765240e-2, -3.580871e-2}, - {-1.765240e-2, 1.250533e0, 8.302781e-3}, - {-3.580871e-2, 8.302781e-3, 1.325068e0}}; + float mgm0softIronInverse[3][3] = {{9.687712e-1,1.144510e-2,-9.601610e-2}, + {1.144510e-2,1.391574e0,-1.169994e-1}, + {-9.601610e-2,-1.169994e-1,1.025668e0}}; + float mgm1softIronInverse[3][3] = {{1.086339e0,-4.341287e-2,-5.870281e-2}, + {-4.341287e-2,1.274588e0,-1.893176e-1}, + {-5.870281e-2,-1.893176e-1,1.077651e0}}; + float mgm2softIronInverse[3][3] = {{9.554406e-1,7.009618e-2,-7.924296e-2}, + {7.009618e-2,1.428436e0,1.504623e-1}, + {-7.924296e-2,1.504623e-1,9.978038e-1}}; + float mgm3softIronInverse[3][3] = {{1.091974e0,1.492663e-2-5.993296e-2}, + {1.492663e-2,1.263068e0,2.013953e-1}, + {-5.993296e-2,2.013953e-1,1.055083e0}}; + float mgm4softIronInverse[3][3] = {{1.100341e0,-7.430518e-4,-3.810031e-2}, + {-7.430518e-4,1.259561e0,-1.951225e-2}, + {-3.810031e-2,-1.951225e-2,1.342584e0}}; float mgm02variance[3] = {1, 1, 1}; float mgm13variance[3] = {1, 1, 1}; float mgm4variance[3] = {1, 1, 1}; From 9a056e6ad5c156c83f0c77b3722a0ab1466467d5 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Wed, 1 Feb 2023 15:37:10 +0100 Subject: [PATCH 062/119] run clang formating script --- mission/controller/acs/control/Detumble.cpp | 23 +++++++++------------ 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 9053fc01..705bf599 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -48,20 +48,17 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal magMom[i] = -dipolMax * sign(magRate[i]); } - return returnvalue::OK; - + return returnvalue::OK; } ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid, - const double *magField, const bool *magFieldValid, - double *magMom) { - - 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); - return returnvalue::OK; - + const double *magField, const bool *magFieldValid, + double *magMom) { + 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); + return returnvalue::OK; } From c2fb224feadabacabd41ae1e199d7dc34af0fd78 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 2 Feb 2023 17:06:26 +0100 Subject: [PATCH 063/119] ACS Update scheduling --- bsp_q7s/core/scheduling.cpp | 70 +- .../pollingSequenceFactory.cpp | 822 +++++++++--------- .../pollingsequence/pollingSequenceFactory.h | 4 +- mission/system/objects/AcsSubsystem.cpp | 18 +- 4 files changed, 462 insertions(+), 452 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index d8136d3a..ad8990de 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -144,11 +144,25 @@ void scheduling::initTasks() { #endif PeriodicTaskIF* comTask = factory->createPeriodicTask( - "CCSDS_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); + "COM_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = comTask->addComponent(objects::COM_SUBSYSTEM); if (result != returnvalue::OK) { scheduling::printAddObjectError("COM subsystem", objects::COM_SUBSYSTEM); } + +#if OBSW_ADD_SYRLINKS == 1 + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::PERFORM_OPERATION); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::SEND_WRITE); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::GET_WRITE); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::SEND_READ); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::GET_READ); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::PERFORM_OPERATION); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::SEND_WRITE); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::GET_WRITE); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::SEND_READ); + comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::GET_READ); +#endif + #if OBSW_ADD_CCSDS_IP_CORES == 1 result = comTask->addComponent(objects::CCSDS_HANDLER); if (result != returnvalue::OK) { @@ -182,12 +196,6 @@ void scheduling::initTasks() { } #endif /* OBSW_ADD_GPS_CTRL */ -#if OBSW_ADD_ACS_CTRL == 1 - acsCtrlTask->addComponent(objects::ACS_CONTROLLER); - if (result != returnvalue::OK) { - scheduling::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); - } -#endif #if OBSW_Q7S_EM == 1 acsCtrlTask->addComponent(objects::MGM_0_LIS3_HANDLER); acsCtrlTask->addComponent(objects::MGM_1_RM3100_HANDLER); @@ -438,10 +446,24 @@ void scheduling::initTasks() { void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec) { ReturnValue_t result = returnvalue::OK; + + FixedTimeslotTaskIF* acsPst = factory.createFixedTimeslotTask( + "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + result = pst::pstAcs(acsPst); + if (result != returnvalue::OK) { + if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { + sif::warning << "scheduling::initTasks: ACS PST is empty" << std::endl; + } else { + sif::error << "scheduling::initTasks: Creating ACS PST failed!" << std::endl; + } + } else { + taskVec.push_back(acsPst); + } + /* Polling Sequence Table Default */ #if OBSW_ADD_SPI_TEST_CODE == 0 FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask( - "MAIN_SPI", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc); + "MAIN_SPI", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc); result = pst::pstSpi(spiPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { @@ -454,37 +476,9 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction } #endif -#if OBSW_ADD_RW == 1 - FixedTimeslotTaskIF* rwPstTask = factory.createFixedTimeslotTask( - "RW_SPI", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 1.0, missedDeadlineFunc); - result = pst::pstSpiRw(rwPstTask); - if (result != returnvalue::OK) { - if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { - sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl; - } else { - sif::error << "scheduling::initTasks: Creating SPI PST failed!" << std::endl; - } - } else { - taskVec.push_back(rwPstTask); - } -#endif - - FixedTimeslotTaskIF* uartPst = factory.createFixedTimeslotTask( - "UART_PST", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc); - result = pst::pstUart(uartPst); - if (result != returnvalue::OK) { - if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { - sif::warning << "scheduling::initTasks: UART PST is empty" << std::endl; - } else { - sif::error << "scheduling::initTasks: Creating UART PST failed!" << std::endl; - } - } else { - taskVec.push_back(uartPst); - } - #if OBSW_ADD_I2C_TEST_CODE == 0 FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask( - "I2C_PST", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc); + "I2C_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc); result = pst::pstI2c(i2cPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { @@ -499,7 +493,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction #if OBSW_ADD_GOMSPACE_PCDU == 1 FixedTimeslotTaskIF* gomSpacePstTask = factory.createFixedTimeslotTask( - "GS_PST_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc); + "GS_PST_TASK", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc); result = pst::pstGompaceCan(gomSpacePstTask); if (result != returnvalue::OK) { if (result != FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 5fec0cd6..0fd6cbfd 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -15,36 +15,6 @@ #define RPI_TEST_GPS_HANDLER 0 #endif -ReturnValue_t pst::pstSpiRw(FixedTimeslotTaskIF *thisSequence) { - uint32_t length = thisSequence->getPeriodMs(); - static_cast(length); - thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - - thisSequence->addSlot(objects::RW1, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW2, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW3, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::RW4, length * 0.2, DeviceHandlerIF::SEND_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::GET_WRITE); - - thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::SEND_READ); - - thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::GET_READ); - return thisSequence->checkSequence(); -} - ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) { uint32_t length = thisSequence->getPeriodMs(); static_cast(length); @@ -56,271 +26,6 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) { thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ); #endif -#if OBSW_ADD_SUN_SENSORS == 1 - - bool addSus0 = true; - bool addSus1 = true; - bool addSus2 = true; - bool addSus3 = true; - bool addSus4 = true; - bool addSus5 = true; - bool addSus6 = true; - bool addSus7 = true; - bool addSus8 = true; - bool addSus9 = true; - bool addSus10 = true; - bool addSus11 = true; - - if (addSus0) { - /* Write setup */ - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4, - DeviceHandlerIF::GET_READ); - } - if (addSus1) { - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4, - DeviceHandlerIF::GET_READ); - } - if (addSus2) { - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4, - DeviceHandlerIF::GET_READ); - } - if (addSus3) { - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4, - DeviceHandlerIF::GET_READ); - } - if (addSus4) { - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4, - DeviceHandlerIF::GET_READ); - } - if (addSus5) { - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4, - DeviceHandlerIF::GET_READ); - } - - if (addSus6) { - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4, - DeviceHandlerIF::GET_READ); - } - - if (addSus7) { - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4, - DeviceHandlerIF::GET_READ); - } - - if (addSus8) { - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4, - DeviceHandlerIF::GET_READ); - } - - if (addSus9) { - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4, - DeviceHandlerIF::GET_READ); - } - - if (addSus10) { - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4, - DeviceHandlerIF::GET_READ); - } - - if (addSus11) { - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, - DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4, - DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4, - DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4, - DeviceHandlerIF::GET_READ); - } -#endif /* OBSW_ADD_SUN_SENSORS == 1 */ - #if OBSW_ADD_RAD_SENSORS == 1 /* Radiation sensor */ thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -330,74 +35,6 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) { thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ); #endif -#if OBSW_ADD_ACS_BOARD == 1 - bool enableAside = true; - bool enableBside = true; - if (enableAside) { - // A side - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.25, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ); - } - - if (enableBside) { - // B side - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.25, - DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ); - - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0, - DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ); - } -#endif /* OBSW_ADD_ACS_BOARD == 1 */ - return thisSequence->checkSequence(); } @@ -407,23 +44,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) { // Length of a communication cycle uint32_t length = thisSequence->getPeriodMs(); static_cast(length); -#if OBSW_ADD_MGT == 1 - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ); -#endif #if OBSW_ADD_BPX_BATTERY_HANDLER == 1 thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION); @@ -481,29 +101,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) { return thisSequence->checkSequence(); } -ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) { - // Length of a communication cycle - uint32_t length = thisSequence->getPeriodMs(); - static_cast(length); - -#if OBSW_ADD_SYRLINKS == 1 - thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); -#endif - -#if OBSW_ADD_STAR_TRACKER == 1 - thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); - thisSequence->addSlot(objects::STAR_TRACKER, length * 0.2, DeviceHandlerIF::SEND_WRITE); - thisSequence->addSlot(objects::STAR_TRACKER, length * 0.4, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::STAR_TRACKER, length * 0.6, DeviceHandlerIF::SEND_READ); - thisSequence->addSlot(objects::STAR_TRACKER, length * 0.8, DeviceHandlerIF::GET_READ); -#endif - return thisSequence->checkSequence(); -} - ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) { uint32_t length = thisSequence->getPeriodMs(); // PCDU handlers receives two messages and both must be handled @@ -576,3 +173,422 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) { } return returnvalue::OK; } + +ReturnValue_t pst::pstAcs(FixedTimeslotTaskIF *thisSequence) { + /* Length of a communication cycle */ + uint32_t length = thisSequence->getPeriodMs(); +#if OBSW_ADD_ACS_BOARD == 1 + bool enableAside = true; + bool enableBside = true; + if (enableAside) { + // A side + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); + } + + if (enableBside) { + // B side + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); + } +#endif /* OBSW_ADD_ACS_BOARD == 1 */ + // SUS: 16 ms +#if OBSW_ADD_SUN_SENSORS == 1 + + bool addSus0 = true; + bool addSus1 = true; + bool addSus2 = true; + bool addSus3 = true; + bool addSus4 = true; + bool addSus5 = true; + bool addSus6 = true; + bool addSus7 = true; + bool addSus8 = true; + bool addSus9 = true; + bool addSus10 = true; + bool addSus11 = true; + + if (addSus0) { + /* Write setup */ + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + if (addSus1) { + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + if (addSus2) { + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + if (addSus3) { + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + if (addSus4) { + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + if (addSus5) { + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + + if (addSus6) { + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + + if (addSus7) { + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + + if (addSus8) { + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + + if (addSus9) { + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + + if (addSus10) { + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325, + DeviceHandlerIF::GET_READ); + } + + if (addSus11) { + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, + DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325, + DeviceHandlerIF::GET_READ); + } +#endif /* OBSW_ADD_SUN_SENSORS == 1 */ + +#if OBSW_ADD_STAR_TRACKER == 1 + thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ); +#endif + +#if OBSW_ADD_MGT == 1 + // This is the MTM measurement cycle + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ); +#endif + + thisSequence->addSlot(objects::ACS_CONTROLLER, length * 0.1, 0); + +#if OBSW_ADD_MGT == 1 + // This is the torquing cycle. + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::GET_READ); +#endif + +#if OBSW_ADD_RW == 1 + thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION); + + thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::SEND_WRITE); + + thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::GET_WRITE); + + thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::SEND_READ); + + thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::GET_READ); + thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::GET_READ); +#endif + return returnvalue::OK; +} diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h index 1b585d81..2eadd19d 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h @@ -39,11 +39,9 @@ namespace pst { */ ReturnValue_t pstGompaceCan(FixedTimeslotTaskIF* thisSequence); -ReturnValue_t pstUart(FixedTimeslotTaskIF* thisSequence); - ReturnValue_t pstSpi(FixedTimeslotTaskIF* thisSequence); -ReturnValue_t pstSpiRw(FixedTimeslotTaskIF* thisSequence); +ReturnValue_t pstAcs(FixedTimeslotTaskIF* thisSequence); ReturnValue_t pstI2c(FixedTimeslotTaskIF* thisSequence); diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index 5ae387ac..f5b8a0ce 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -32,20 +32,22 @@ ReturnValue_t AcsSubsystem::initialize() { return ObjectManagerIF::CHILD_INIT_FAILED; ; } - result = manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_VIOLATION)); - if(result != returnvalue::OK) { - sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_VIOLATION failed" << std::endl; + result = + manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_VIOLATION)); + if (result != returnvalue::OK) { + sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_VIOLATION failed" << std::endl; } - result = manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_RECOVERY)); - if(result != returnvalue::OK) { - sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_RECOVERY failed" << std::endl; + result = + manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_RECOVERY)); + if (result != returnvalue::OK) { + sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_RECOVERY failed" << std::endl; } return Subsystem::initialize(); } void AcsSubsystem::performChildOperation() { - handleEventMessages(); - return Subsystem::performChildOperation(); + handleEventMessages(); + return Subsystem::performChildOperation(); } void AcsSubsystem::handleEventMessages() { From f6abe2b466d814b77b197580f2e7659bcdc965f2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 2 Feb 2023 17:09:59 +0100 Subject: [PATCH 064/119] remove code --- mission/system/tree/acsModeTree.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index f5a1df23..4429d15c 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -66,17 +66,6 @@ auto ACS_TABLE_PTG_TARGET_TRANS_0 = auto ACS_TABLE_PTG_TARGET_TRANS_1 = std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 3, FixedArrayList()); -/* -auto ACS_SEQUENCE_PTG_TARGET = - std::make_pair(acs::CtrlSubmode::TARGET_PT, FixedArrayList()); -auto ACS_TABLE_TARGET_PT_TGT = - std::make_pair((acs::CtrlSubmode::TARGET_PT << 24) | 1, FixedArrayList()); -auto ACS_TABLE_TARGET_PT_TRANS_0 = - std::make_pair((acs::CtrlSubmode::TARGET_PT << 24) | 2, FixedArrayList()); -auto ACS_TABLE_TARGET_PT_TRANS_1 = - std::make_pair((acs::CtrlSubmode::TARGET_PT << 24) | 3, FixedArrayList()); -*/ - void satsystem::acs::init() { ModeListEntry entry; buildOffSequence(ACS_SUBSYSTEM, entry); From 4590e1d9c9d9b2d0265c2c5c9926b20e2cdfc1a5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 2 Feb 2023 17:12:22 +0100 Subject: [PATCH 065/119] host compiles --- mission/acsDefs.h | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/acsDefs.h b/mission/acsDefs.h index ab182112..d17351a6 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -1,6 +1,7 @@ #ifndef MISSION_ACSDEFS_H_ #define MISSION_ACSDEFS_H_ +#include #include namespace acs { From 1e0d9561a29d9e97f08ff9ddb7980443098f32c8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 2 Feb 2023 17:16:15 +0100 Subject: [PATCH 066/119] changelog update --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index fe417267..19d666a4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,9 @@ change warranting a new major release: ## Changed +- Update ACS scheduling to represent the actual ACS design. There is one ACS PST now for all + timing sensitive ACS operations. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/365 - Update and tweak ACS subsystem to represent the actual ACS design - Event handling in the ACS subsystem for events triggered by the ACS controller. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/365 From f9ae4860d94b751022343f0f86da875070e48b18 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Feb 2023 14:31:22 +0100 Subject: [PATCH 067/119] typo --- mission/controller/AcsController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8ed396c5..d1039f8f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -153,7 +153,7 @@ void AcsController::performSafe() { } if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; - // Triggers detubmle mode transition in subsystem + // Triggers detumble mode transition in subsystem triggerEvent(acs::SAFE_RATE_VIOLATION); } From e4a227359ba510e3b1a8e0610eba3f039c06fa4a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Feb 2023 15:02:08 +0100 Subject: [PATCH 068/119] some bugfixes, syrlinks needs to remain in PST cause of some bug --- bsp_q7s/core/scheduling.cpp | 15 +-------------- .../pollingsequence/pollingSequenceFactory.cpp | 13 +++++++++++-- .../pollingsequence/pollingSequenceFactory.h | 2 +- 3 files changed, 13 insertions(+), 17 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index ad8990de..f5c7c558 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -150,19 +150,6 @@ void scheduling::initTasks() { scheduling::printAddObjectError("COM subsystem", objects::COM_SUBSYSTEM); } -#if OBSW_ADD_SYRLINKS == 1 - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::PERFORM_OPERATION); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::SEND_WRITE); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::GET_WRITE); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::SEND_READ); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::GET_READ); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::PERFORM_OPERATION); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::SEND_WRITE); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::GET_WRITE); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::SEND_READ); - comTask->addComponent(objects::SYRLINKS_HANDLER, DeviceHandlerIF::GET_READ); -#endif - #if OBSW_ADD_CCSDS_IP_CORES == 1 result = comTask->addComponent(objects::CCSDS_HANDLER); if (result != returnvalue::OK) { @@ -464,7 +451,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction #if OBSW_ADD_SPI_TEST_CODE == 0 FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask( "MAIN_SPI", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc); - result = pst::pstSpi(spiPst); + result = pst::pstSpiAndSyrlinks(spiPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl; diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 0fd6cbfd..efbb9578 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -15,8 +15,17 @@ #define RPI_TEST_GPS_HANDLER 0 #endif -ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) { +ReturnValue_t pst::pstSpiAndSyrlinks(FixedTimeslotTaskIF *thisSequence) { uint32_t length = thisSequence->getPeriodMs(); + +#if OBSW_ADD_SYRLINKS == 1 + thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); +#endif + static_cast(length); #if OBSW_ADD_PL_PCDU == 1 thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION); @@ -312,7 +321,7 @@ ReturnValue_t pst::pstAcs(FixedTimeslotTaskIF *thisSequence) { DeviceHandlerIF::SEND_WRITE); thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325, DeviceHandlerIF::GET_WRITE); - thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4, + thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325, DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325, DeviceHandlerIF::GET_READ); diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h index 2eadd19d..1dcfc30a 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h @@ -39,7 +39,7 @@ namespace pst { */ ReturnValue_t pstGompaceCan(FixedTimeslotTaskIF* thisSequence); -ReturnValue_t pstSpi(FixedTimeslotTaskIF* thisSequence); +ReturnValue_t pstSpiAndSyrlinks(FixedTimeslotTaskIF* thisSequence); ReturnValue_t pstAcs(FixedTimeslotTaskIF* thisSequence); From b8130265cde105ed94aeafeca3206840d9c44705 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Feb 2023 15:02:30 +0100 Subject: [PATCH 069/119] afmt --- mission/system/tree/acsModeTree.h | 1 - 1 file changed, 1 deletion(-) diff --git a/mission/system/tree/acsModeTree.h b/mission/system/tree/acsModeTree.h index c814ca3a..134a86a7 100644 --- a/mission/system/tree/acsModeTree.h +++ b/mission/system/tree/acsModeTree.h @@ -1,6 +1,5 @@ #include - namespace satsystem { namespace acs { From b9ed485d601ca1937312646ff4a5454c28eb0a94 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 3 Feb 2023 15:44:50 +0100 Subject: [PATCH 070/119] changed calibration matrices for SUS0 and SUS6 due to them being exchanged --- mission/controller/acs/AcsParameters.h | 220 ++++++++++++------------- 1 file changed, 110 insertions(+), 110 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 98997e2c..88802a46 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -78,13 +78,13 @@ class AcsParameters : public HasParametersIF { } mgmHandlingParameters; struct SusHandlingParameters { - float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07 + float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10 float sus1orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM06 float sus2orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM13 float sus3orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM14 float sus4orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM05 float sus5orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM02 - float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM10 + float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07 float sus7orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; // FM01 float sus8orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; // FM03 float sus9orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; // FM11 @@ -92,61 +92,61 @@ class AcsParameters : public HasParametersIF { float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM08 float sus0coeffAlpha[9][10] = { - {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707, - -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396, - -0.000124096561459262, 0.00040790566176981}, - {6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417, - 0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415, - 1.63114413539632e-05, -2.0187452317724e-05}, - {-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334, - 0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995, - -0.000249094601806692, 0.000231466668650876}, - {-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384, - -0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05, - -9.78534772816957e-06, -4.72083745991256e-05}, - {0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541, - -0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463, - 0.00010150571088124, -0.000367705461590854}, - {3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526, - -0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05, - 6.97244631313601e-05, 2.50519145070895e-05}, - {-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191, - -0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928, - -1.78466217018177e-05, -0.00058976234038192}, - {1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089, - -0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06, - -2.73407888222758e-05, 5.45131881032866e-06}, - {43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675, - 0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078, - -0.000889196131314391, -0.000509766491897672}}; + {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293, + 0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146, + 0.00091428505258307, 0.000259857066722932}, + {1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859, + 0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05, + -1.06065583714065e-05, -1.43899892666699e-05}, + {8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577, + 0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412, + -4.88501228194031e-06, -0.000434861113274231}, + {-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253, + 0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05, + 6.09619017310129e-05, 4.23908862836885e-05}, + {-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607, + 0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257, + 0.000150795563555828, -0.000279246491927943}, + {7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557, + -0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05, + 9.13233708460098e-05, -0.000206717750924323}, + {16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216, + -0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597, + -0.000532190902882765, 9.36018171121253e-05}, + {-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645, + -0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05, + 1.02609175615251e-05, -9.64717658954667e-06}, + {-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358, + -0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05, + 0.000868163357631254, 0.00120099606910313}}; float sus0coeffBeta[9][10] = { - {1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235, - -0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05, - -5.52178824394723e-06, 5.73935295303589e-05}, - {3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133, - 0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05, - -7.58501977656551e-05, -8.79809730730992e-05}, - {14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518, - 0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341, - -0.00020861690864945}, - {1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335, - -0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06, - -6.00508568552101e-05, 0.000101583822589461}, - {-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729, - -0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05, - 0.000316835483508523, 0.000151442890664899}, - {-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231, - 0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05, - -9.83662017231755e-05, 1.87078667116619e-05}, - {27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016, - -0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772, - -0.000285438191474895, -5.71327627917386e-05}, - {-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328, - -0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05, - -9.77555098179959e-05, 2.09624089449761e-05}, - {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461, - -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688, - 0.000141998709314758, 0.000101051304611037}}; + {12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917, + 0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05, + 0.000821479971871302, -4.5330528329465e-05}, + {1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823, + 0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05, + -4.04514487800062e-05, -7.6296149087237e-05}, + {5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941, + -0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412, + 0.0011723869613426, 0.000122882034141316}, + {2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172, + -0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05, + -5.87601932564404e-05, -3.92033314627704e-05}, + {2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898, + 0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264, + 0.000259925974231328, 0.000222437797823852}, + {1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167, + 0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05, + -4.33518182614169e-05, 4.66993119419691e-05}, + {-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108, + 0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429, + 0.000401546410974577, -0.000147563886502726}, + {1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231, + 0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05, + -5.3309466243918e-05, 6.20289310356821e-06}, + {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785, + 0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741, + -0.000186887396585446, 0.00119710704771344}}; float sus1coeffAlpha[9][10] = { {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, @@ -428,61 +428,61 @@ class AcsParameters : public HasParametersIF { -0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245, -0.000169538153750085, -0.000336529204350355}}; float sus6coeffAlpha[9][10] = { - {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293, - 0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146, - 0.00091428505258307, 0.000259857066722932}, - {1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859, - 0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05, - -1.06065583714065e-05, -1.43899892666699e-05}, - {8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577, - 0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412, - -4.88501228194031e-06, -0.000434861113274231}, - {-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253, - 0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05, - 6.09619017310129e-05, 4.23908862836885e-05}, - {-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607, - 0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257, - 0.000150795563555828, -0.000279246491927943}, - {7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557, - -0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05, - 9.13233708460098e-05, -0.000206717750924323}, - {16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216, - -0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597, - -0.000532190902882765, 9.36018171121253e-05}, - {-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645, - -0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05, - 1.02609175615251e-05, -9.64717658954667e-06}, - {-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358, - -0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05, - 0.000868163357631254, 0.00120099606910313}}; + {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707, + -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396, + -0.000124096561459262, 0.00040790566176981}, + {6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417, + 0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415, + 1.63114413539632e-05, -2.0187452317724e-05}, + {-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334, + 0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995, + -0.000249094601806692, 0.000231466668650876}, + {-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384, + -0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05, + -9.78534772816957e-06, -4.72083745991256e-05}, + {0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541, + -0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463, + 0.00010150571088124, -0.000367705461590854}, + {3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526, + -0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05, + 6.97244631313601e-05, 2.50519145070895e-05}, + {-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191, + -0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928, + -1.78466217018177e-05, -0.00058976234038192}, + {1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089, + -0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06, + -2.73407888222758e-05, 5.45131881032866e-06}, + {43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675, + 0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078, + -0.000889196131314391, -0.000509766491897672}}; float sus6coeffBeta[9][10] = { - {12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917, - 0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05, - 0.000821479971871302, -4.5330528329465e-05}, - {1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823, - 0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05, - -4.04514487800062e-05, -7.6296149087237e-05}, - {5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941, - -0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412, - 0.0011723869613426, 0.000122882034141316}, - {2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172, - -0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05, - -5.87601932564404e-05, -3.92033314627704e-05}, - {2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898, - 0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264, - 0.000259925974231328, 0.000222437797823852}, - {1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167, - 0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05, - -4.33518182614169e-05, 4.66993119419691e-05}, - {-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108, - 0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429, - 0.000401546410974577, -0.000147563886502726}, - {1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231, - 0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05, - -5.3309466243918e-05, 6.20289310356821e-06}, - {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785, - 0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741, - -0.000186887396585446, 0.00119710704771344}}; + {1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235, + -0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05, + -5.52178824394723e-06, 5.73935295303589e-05}, + {3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133, + 0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05, + -7.58501977656551e-05, -8.79809730730992e-05}, + {14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518, + 0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341, + -0.00020861690864945}, + {1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335, + -0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06, + -6.00508568552101e-05, 0.000101583822589461}, + {-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729, + -0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05, + 0.000316835483508523, 0.000151442890664899}, + {-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231, + 0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05, + -9.83662017231755e-05, 1.87078667116619e-05}, + {27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016, + -0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772, + -0.000285438191474895, -5.71327627917386e-05}, + {-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328, + -0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05, + -9.77555098179959e-05, 2.09624089449761e-05}, + {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461, + -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688, + 0.000141998709314758, 0.000101051304611037}}; float sus7coeffAlpha[9][10] = { {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, From 18af6228a0d617f47b0a56a457d9e57a825733ab Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 09:30:20 +0100 Subject: [PATCH 071/119] added final calibration matrices --- mission/controller/acs/AcsParameters.h | 41 +++++++++++++------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 1fe5f29c..ab8114bb 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -37,27 +37,28 @@ class AcsParameters /*: public HasParametersIF*/ { float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; - float mgm0hardIronOffset[3] = {3.303907e0,4.009162e0,-1.677380e1}; - float mgm1hardIronOffset[3] = {-2.494258e0,3.526886e0,3.593006e0}; - float mgm2hardIronOffset[3] = {-1.630907e1,5.543566e0,-1.619287e1}; - float mgm3hardIronOffset[3] = {-8.612960e-1,2.752825e0,-1.101764e0}; - float mgm4hardIronOffset[3] = {1.696131e0,4.624440e0,2.449785e-1}; + float mgm0hardIronOffset[3] = {6.116487, 6.796264, -19.188060}; + float mgm1hardIronOffset[3] = {-1.077152, 2.080583, 1.974483}; + float mgm2hardIronOffset[3] = {-19.285857, 5.401821, -16.096297}; + float mgm3hardIronOffset[3] = {-0.634033, 2.787695, 0.092036}; + float mgm4hardIronOffset[3] = {1.722133, 4.511870, 0.379980}; + + float mgm0softIronInverse[3][3] = {{1.163335, 0.137584, 0.203908}, + {0.137584, 0.625606, 0.045537}, + {0.203908, 0.045537, 1.095923}}; + float mgm1softIronInverse[3][3] = {{0.972071, 0.131166, 0.042151}, + {0.131166, 0.803260, 0.034779}, + {0.042151, 0.034779, 0.923417}}; + float mgm2softIronInverse[3][3] = {{1.234863, -0.157745, 0.070813}, + {-0.157745, 0.746707, -0.062080}, + {0.070813, -0.062080, 1.122514}}; + float mgm3softIronInverse[3][3] = {{0.956890, -0.134841, 0.054017}, + {-0.134841, 0.811361, -0.015383}, + {0.054017, -0.015383, 0.939153}}; + float mgm4softIronInverse[3][3] = {{0.913163, 0.011058, 0.023273}, + {0.011058, 0.792295, 0.002921}, + {0.023273, 0.002921, 0.739496}}; - float mgm0softIronInverse[3][3] = {{9.687712e-1,1.144510e-2,-9.601610e-2}, - {1.144510e-2,1.391574e0,-1.169994e-1}, - {-9.601610e-2,-1.169994e-1,1.025668e0}}; - float mgm1softIronInverse[3][3] = {{1.086339e0,-4.341287e-2,-5.870281e-2}, - {-4.341287e-2,1.274588e0,-1.893176e-1}, - {-5.870281e-2,-1.893176e-1,1.077651e0}}; - float mgm2softIronInverse[3][3] = {{9.554406e-1,7.009618e-2,-7.924296e-2}, - {7.009618e-2,1.428436e0,1.504623e-1}, - {-7.924296e-2,1.504623e-1,9.978038e-1}}; - float mgm3softIronInverse[3][3] = {{1.091974e0,1.492663e-2-5.993296e-2}, - {1.492663e-2,1.263068e0,2.013953e-1}, - {-5.993296e-2,2.013953e-1,1.055083e0}}; - float mgm4softIronInverse[3][3] = {{1.100341e0,-7.430518e-4,-3.810031e-2}, - {-7.430518e-4,1.259561e0,-1.951225e-2}, - {-3.810031e-2,-1.951225e-2,1.342584e0}}; float mgm02variance[3] = {1, 1, 1}; float mgm13variance[3] = {1, 1, 1}; float mgm4variance[3] = {1, 1, 1}; From a9fcb6a183d6de7263ed48ef944cb8049e7fa85b Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 09:46:19 +0100 Subject: [PATCH 072/119] update changelog --- CHANGELOG.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b15c4f12..c835c81c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,10 +17,17 @@ change warranting a new major release: # [unreleased] +## Changed + +- Readded calibration matrices for MGM calibration + ## Fixed - Bugfix in FSFW where the sequence flags of the PUS packets were set to continuation segment (0b00) instead of unsegmented (0b11). +- Bugfixes in 'SensofProcessing' where previously MGM values would be calibrated before being + transformed in body RF. However, the calibration values are in the body RF. Also fixed the + validity flag of 'mgmVecTotDerivative'. # [v1.23.0] 2023-02-01 From c8e376a625192f9ff46aaa0f493c54f73f0087b4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 10:03:51 +0100 Subject: [PATCH 073/119] add missing pool read guards --- linux/devices/GpsHyperionLinuxController.cpp | 61 +++++++++----------- linux/devices/GpsHyperionLinuxController.h | 7 ++- 2 files changed, 31 insertions(+), 37 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index 6bbbcfb7..3c19ca72 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -44,6 +44,7 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ } } if (mode == MODE_OFF) { + PoolReadGuard pg(&gpsSet); gpsSet.setValidity(false, true); } return returnvalue::OK; @@ -217,7 +218,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { modeCommanded = false; } gpsSet.setValidity(false, true); - } else if (gps.satellites_used > 0) { + } else if (gps.satellites_used > 0 && validFix) { gpsSet.setValidity(true, true); } @@ -250,46 +251,20 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { gpsSet.speed.setValid(false); } -#if LIBGPS_VERSION_MINOR <= 17 - gpsSet.unixSeconds.value = gps.fix.time; -#else - gpsSet.unixSeconds.value = gps.fix.time.tv_sec; -#endif timeval time = {}; - time.tv_sec = gpsSet.unixSeconds.value; #if LIBGPS_VERSION_MINOR <= 17 - double fractionalPart = gps.fix.time - std::floor(gps.fix.time); + gpsSet.unixSeconds.value = std::floor(gps.fix.time); + double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value; time.tv_usec = fractionalPart * 1000.0 * 1000.0; #else + gpsSet.unixSeconds.value = gps.fix.time.tv_sec; time.tv_usec = gps.fix.time.tv_nsec / 1000; #endif - std::time_t t = std::time(nullptr); - if (time.tv_sec == t) { - timeIsConstantCounter++; - } else { - timeIsConstantCounter = 0; - } - if (timeInit and validFix) { - if (not utility::timeSanityCheck()) { -#if OBSW_VERBOSE_LEVEL >= 1 - time_t timeRaw = time.tv_sec; - std::tm *timeTm = std::gmtime(&timeRaw); - sif::info << "Setting invalid system time from GPS data directly: " - << std::put_time(timeTm, "%c %Z") << std::endl; -#endif - // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb.. - Clock::setClock(&time); - } - timeInit = false; - } - // If the received time does not change anymore for whatever reason, do not set it here - // to avoid stale times. Also, don't do it too often often to avoid jumping times - if (timeIsConstantCounter < 20 and timeUpdateCd.hasTimedOut()) { - // Update the system time here for now. NTP seems to be unable to do so for whatever reason. - // Further tests have shown that the time seems to be set by NTPD after some time.. - // Clock::setClock(&time); - timeUpdateCd.resetTimer(); - } + time.tv_sec = gpsSet.unixSeconds.value; + // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC + // and no time file available) we set it with the roughly valid time from the GPS. + // NTP might only work if the time difference between sys time and current time is not too large. + overwriteTimeIfNotSane(time, validFix); Clock::TimeOfDay_t timeOfDay = {}; Clock::convertTimevalToTimeOfDay(&time, &timeOfDay); @@ -325,3 +300,19 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } return returnvalue::OK; } + +void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) { + if (not timeInit and validFix) { + if (not utility::timeSanityCheck()) { +#if OBSW_VERBOSE_LEVEL >= 1 + time_t timeRaw = time.tv_sec; + std::tm *timeTm = std::gmtime(&timeRaw); + sif::info << "Overwriting invalid system time from GPS data directly: " + << std::put_time(timeTm, "%c %Z") << std::endl; +#endif + // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb.. + Clock::setClock(&time); + } + timeInit = true; + } +} diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h index d2dc91b6..5b0a84cd 100644 --- a/linux/devices/GpsHyperionLinuxController.h +++ b/linux/devices/GpsHyperionLinuxController.h @@ -59,17 +59,20 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { ReadModes readMode = ReadModes::SOCKET; Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); bool modeCommanded = true; - bool timeInit = true; + bool timeInit = false; bool gpsNotOpenSwitch = true; bool gpsReadFailedSwitch = true; bool debugHyperionGps = false; int32_t noModeSetCntr = 0; - uint32_t timeIsConstantCounter = 0; Countdown timeUpdateCd = Countdown(60); // Returns true if the function should be called again or false if other // controller handling can be done. bool readGpsDataFromGpsd(); + // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC) + // we set it with the roughly valid time from the GPS. For some reason, NTP might only work + // if the time difference between sys time and current time is not too large + void overwriteTimeIfNotSane(timeval time, bool validFix); }; #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */ From 07ca7388427e69586b6b3e7357e1ad20ab33fdb2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 10:12:28 +0100 Subject: [PATCH 074/119] some more tweaks --- linux/devices/GpsHyperionLinuxController.cpp | 23 ++++++++++++++------ 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index 3c19ca72..c3e7bc73 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -225,25 +225,34 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { gpsSet.satInUse.value = gps.satellites_used; gpsSet.satInView.value = gps.satellites_visible; + bool latValid = false; if (std::isfinite(gps.fix.latitude)) { // Negative latitude -> South direction gpsSet.latitude.value = gps.fix.latitude; - } else { - gpsSet.latitude.setValid(false); + if(gps.fix.mode >= 2) { + latValid = true; + } } + gpsSet.latitude.setValid(latValid); + bool longValid = false; if (std::isfinite(gps.fix.longitude)) { // Negative longitude -> West direction gpsSet.longitude.value = gps.fix.longitude; - } else { - gpsSet.longitude.setValid(false); + if(gps.fix.mode >= 2) { + longValid = true; + } } + gpsSet.latitude.setValid(longValid); + bool altitudeValid = false; if (std::isfinite(gps.fix.altitude)) { gpsSet.altitude.value = gps.fix.altitude; - } else { - gpsSet.altitude.setValid(false); + if(gps.fix.mode == 3) { + altitudeValid = true; + } } + gpsSet.altitude.setValid(altitudeValid); if (std::isfinite(gps.fix.speed)) { gpsSet.speed.value = gps.fix.speed; @@ -277,7 +286,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { if (debugHyperionGps) { sif::info << "-- Hyperion GPS Data --" << std::endl; #if LIBGPS_VERSION_MINOR <= 17 - time_t timeRaw = gps.fix.time; + time_t timeRaw = gpsSet.unixSeconds.value; #else time_t timeRaw = gps.fix.time.tv_sec; #endif From 5d0a1858cce1a4cbe0b72893db8fd54baac2b07b Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 10:20:36 +0100 Subject: [PATCH 075/119] onboard handling of GYR values changed from deg/s to rad/s --- mission/controller/acs/AcsParameters.h | 4 ++-- mission/controller/acs/SensorProcessing.cpp | 4 ++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index ae174236..88802a46 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -915,8 +915,8 @@ class AcsParameters : public HasParametersIF { struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s - double omegaDetumbleStart = 2; - double omegaDetumbleEnd = 0.4; + double omegaDetumbleStart = 2 * M_PI / 180; + double omegaDetumbleEnd = 0.4 * M_PI / 180; double gainD = pow(10.0, -3.3); } detumbleParameter; }; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 9a1a9637..bfb38afc 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -477,6 +477,7 @@ void SensorProcessing::processGyr( const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); + VectorOperations::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; @@ -486,6 +487,7 @@ void SensorProcessing::processGyr( const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); + VectorOperations::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; @@ -495,6 +497,7 @@ void SensorProcessing::processGyr( const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); + VectorOperations::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; @@ -504,6 +507,7 @@ void SensorProcessing::processGyr( const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); + VectorOperations::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; From d6aa6875ccb88c2cca7f9ffd9d50698b081d4526 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 10:22:12 +0100 Subject: [PATCH 076/119] typos and other minor adaptions --- linux/devices/GpsHyperionLinuxController.cpp | 67 +++++++++++--------- 1 file changed, 38 insertions(+), 29 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index c3e7bc73..7554b805 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -199,19 +199,17 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } bool validFix = false; - static_cast(validFix); // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix - int newFixMode = gps.fix.mode; - if (newFixMode == 2 or newFixMode == 3) { + if (gps.fix.mode == 2 or gps.fix.mode == 3) { validFix = true; } - if (gpsSet.fixMode.value != newFixMode) { - triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFixMode); + if (gpsSet.fixMode.value != gps.fix.mode) { + triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode); } - gpsSet.fixMode.value = newFixMode; + gpsSet.fixMode.value = gps.fix.mode; if (gps.fix.mode == 0 or gps.fix.mode == 1) { if (modeCommanded and maxTimeToReachFix.hasTimedOut()) { - // We are supposed to be on and functioning, but not fix was found + // We are supposed to be on and functioning, but no fix was found if (mode == MODE_ON or mode == MODE_NORMAL) { mode = MODE_OFF; } @@ -229,7 +227,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { if (std::isfinite(gps.fix.latitude)) { // Negative latitude -> South direction gpsSet.latitude.value = gps.fix.latitude; - if(gps.fix.mode >= 2) { + if (gps.fix.mode >= 2) { latValid = true; } } @@ -239,7 +237,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { if (std::isfinite(gps.fix.longitude)) { // Negative longitude -> West direction gpsSet.longitude.value = gps.fix.longitude; - if(gps.fix.mode >= 2) { + if (gps.fix.mode >= 2) { longValid = true; } } @@ -248,7 +246,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { bool altitudeValid = false; if (std::isfinite(gps.fix.altitude)) { gpsSet.altitude.value = gps.fix.altitude; - if(gps.fix.mode == 3) { + if (gps.fix.mode == 3) { altitudeValid = true; } } @@ -260,29 +258,40 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { gpsSet.speed.setValid(false); } - timeval time = {}; + if (TIME_SET != (TIME_SET & gps.set)) { + timeval time = {}; #if LIBGPS_VERSION_MINOR <= 17 - gpsSet.unixSeconds.value = std::floor(gps.fix.time); - double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value; - time.tv_usec = fractionalPart * 1000.0 * 1000.0; + gpsSet.unixSeconds.value = std::floor(gps.fix.time); + double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value; + time.tv_usec = fractionalPart * 1000.0 * 1000.0; #else - gpsSet.unixSeconds.value = gps.fix.time.tv_sec; - time.tv_usec = gps.fix.time.tv_nsec / 1000; + gpsSet.unixSeconds.value = gps.fix.time.tv_sec; + time.tv_usec = gps.fix.time.tv_nsec / 1000; #endif - time.tv_sec = gpsSet.unixSeconds.value; - // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC - // and no time file available) we set it with the roughly valid time from the GPS. - // NTP might only work if the time difference between sys time and current time is not too large. - overwriteTimeIfNotSane(time, validFix); + time.tv_sec = gpsSet.unixSeconds.value; + // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC + // and no time file available) we set it with the roughly valid time from the GPS. + // NTP might only work if the time difference between sys time and current time is not too + // large. + overwriteTimeIfNotSane(time, validFix); + Clock::TimeOfDay_t timeOfDay = {}; + Clock::convertTimevalToTimeOfDay(&time, &timeOfDay); + gpsSet.year = timeOfDay.year; + gpsSet.month = timeOfDay.month; + gpsSet.day = timeOfDay.day; + gpsSet.hours = timeOfDay.hour; + gpsSet.minutes = timeOfDay.minute; + gpsSet.seconds = timeOfDay.second; + } else { + gpsSet.unixSeconds.setValid(false); + gpsSet.year.setValid(false); + gpsSet.month.setValid(false); + gpsSet.day.setValid(false); + gpsSet.hours.setValid(false); + gpsSet.minutes.setValid(false); + gpsSet.seconds.setValid(false); + } - Clock::TimeOfDay_t timeOfDay = {}; - Clock::convertTimevalToTimeOfDay(&time, &timeOfDay); - gpsSet.year = timeOfDay.year; - gpsSet.month = timeOfDay.month; - gpsSet.day = timeOfDay.day; - gpsSet.hours = timeOfDay.hour; - gpsSet.minutes = timeOfDay.minute; - gpsSet.seconds = timeOfDay.second; if (debugHyperionGps) { sif::info << "-- Hyperion GPS Data --" << std::endl; #if LIBGPS_VERSION_MINOR <= 17 From 5f9a445aa22f87a9f4ffd80d8df5f3e7b6a189cd Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 10:29:28 +0100 Subject: [PATCH 077/119] added bias removal for GYR --- mission/controller/acs/AcsParameters.h | 6 ++++++ mission/controller/acs/SensorProcessing.cpp | 4 ++++ 2 files changed, 10 insertions(+) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 88802a46..1ffb33c7 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -770,6 +770,12 @@ class AcsParameters : public HasParametersIF { double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}; double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; + + double gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004}; + double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359}; + 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 * assumed to be equal for the same class of sensors */ float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index bfb38afc..0496e7e9 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -477,6 +477,7 @@ void SensorProcessing::processGyr( const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); + VectorOperations::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3); VectorOperations::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; @@ -487,6 +488,7 @@ void SensorProcessing::processGyr( const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); + VectorOperations::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3); VectorOperations::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; @@ -497,6 +499,7 @@ void SensorProcessing::processGyr( const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); + VectorOperations::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3); VectorOperations::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; @@ -507,6 +510,7 @@ void SensorProcessing::processGyr( const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); + VectorOperations::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3); VectorOperations::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3); for (uint8_t i = 0; i < 3; i++) { sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; From b6022d2ff15306aab9bce23019184824804405b5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 10:34:09 +0100 Subject: [PATCH 078/119] should be covered by PST now --- bsp_q7s/core/scheduling.cpp | 29 ----------------------------- 1 file changed, 29 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index c85c7ff0..150efa97 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -216,35 +216,6 @@ void scheduling::initTasks() { scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM); } -#if OBSW_Q7S_EM == 1 - acsSysTask->addComponent(objects::MGM_0_LIS3_HANDLER); - acsSysTask->addComponent(objects::MGM_1_RM3100_HANDLER); - acsSysTask->addComponent(objects::MGM_2_LIS3_HANDLER); - acsSysTask->addComponent(objects::MGM_3_RM3100_HANDLER); - acsSysTask->addComponent(objects::IMTQ_HANDLER); - acsSysTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF); - acsSysTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF); - acsSysTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB); - acsSysTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB); - acsSysTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB); - acsSysTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB); - acsSysTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF); - acsSysTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF); - acsSysTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF); - acsSysTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); - acsSysTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); - acsSysTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); - acsSysTask->addComponent(objects::GYRO_0_ADIS_HANDLER); - acsSysTask->addComponent(objects::GYRO_1_L3G_HANDLER); - acsSysTask->addComponent(objects::GYRO_2_ADIS_HANDLER); - acsSysTask->addComponent(objects::GYRO_3_L3G_HANDLER); - acsSysTask->addComponent(objects::GPS_CONTROLLER); - acsSysTask->addComponent(objects::STAR_TRACKER); - acsSysTask->addComponent(objects::RW1); - acsSysTask->addComponent(objects::RW2); - acsSysTask->addComponent(objects::RW3); - acsSysTask->addComponent(objects::RW4); -#endif #if OBSW_ADD_RTD_DEVICES == 1 PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask( "TCS_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc); From d6b60723d9e6e3942cbb095e8123d422c93b6ee4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 12:53:30 +0100 Subject: [PATCH 079/119] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 38789e05..69c94645 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 38789e053b65cfa14604fc625e7bcc8ca03a3f17 +Subproject commit 69c94645df81c2fc9d538a0f19d2c6552e1c73a7 From e3cc45d7e3cb8540a8a755d5e3a1b61eab3fff96 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 13:09:42 +0100 Subject: [PATCH 080/119] merge aftermath: - fixed missing declarations of functions - fixed use of deprecated submode definitions - changed naming of some submodes --- mission/acsDefs.h | 6 ++--- mission/controller/AcsController.cpp | 18 ++++++------- mission/controller/AcsController.h | 5 ++++ mission/system/tree/acsModeTree.cpp | 39 ++++++++++++++-------------- 4 files changed, 37 insertions(+), 31 deletions(-) diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 8869d6ff..72a445c8 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -10,11 +10,11 @@ enum CtrlSubmode { OFF = HasModesIF::MODE_OFF, SAFE = 10, DETUMBLE = 11, - IDLE = 12, - PTG_TARGET_NADIR = 13, + PTG_IDLE = 12, + PTG_NADIR = 13, PTG_TARGET = 14, PTG_TARGET_GS = 15, - PTG_TARGET_INERTIAL = 16, + PTG_INERTIAL = 16, }; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 868f353a..ff06558d 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -71,11 +71,11 @@ void AcsController::performControlOperation() { case acs::DETUMBLE: performDetumble(); break; - case acs::IDLE: + case acs::PTG_IDLE: case acs::PTG_TARGET: - case acs::PTG_TARGET_GS: - case acs::PTG_TARGET_NADIR: - case acs::PTG_TARGET_INERTIAL: + case acs::PTG_TARGET_GS: + case acs::PTG_NADIR: + case acs::PTG_INERTIAL: performPointingCtrl(); break; } @@ -282,7 +282,7 @@ void AcsController::performPointingCtrl() { double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol switch (submode) { - case SUBMODE_IDLE: + case acs::PTG_IDLE: guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, @@ -306,7 +306,7 @@ void AcsController::performPointingCtrl() { break; - case SUBMODE_PTG_TARGET: + case acs::PTG_TARGET: guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, @@ -329,7 +329,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; - case SUBMODE_PTG_TARGET_GS: + case acs::PTG_TARGET_GS: guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef, @@ -352,7 +352,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; - case SUBMODE_PTG_NADIR: + case acs::PTG_NADIR: guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double)); @@ -374,7 +374,7 @@ void AcsController::performPointingCtrl() { &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); break; - case SUBMODE_PTG_INERTIAL: + case acs::PTG_INERTIAL: guidance.inertialQuatPtg(targetQuat, refSatRate); std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef, 4 * sizeof(double)); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 265af19c..6a18e988 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -26,6 +26,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes AcsController(object_id_t objectId); + MessageQueueId_t getCommandQueue() const; + ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId, + ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, + uint16_t startAtIndex) override; + protected: void performSafe(); void performDetumble(); diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index 849959f9..88b4db19 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -55,13 +55,14 @@ auto ACS_TABLE_SAFE_TRANS_0 = auto ACS_TABLE_SAFE_TRANS_1 = std::make_pair((acs::CtrlSubmode::SAFE << 24) | 3, FixedArrayList()); -auto ACS_SEQUENCE_IDLE = std::make_pair(acs::CtrlSubmode::IDLE, FixedArrayList()); +auto ACS_SEQUENCE_IDLE = + std::make_pair(acs::CtrlSubmode::PTG_IDLE, FixedArrayList()); auto ACS_TABLE_IDLE_TGT = - std::make_pair((acs::CtrlSubmode::IDLE << 24) | 1, FixedArrayList()); + std::make_pair((acs::CtrlSubmode::PTG_IDLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_0 = - std::make_pair((acs::CtrlSubmode::IDLE << 24) | 2, FixedArrayList()); + std::make_pair((acs::CtrlSubmode::PTG_IDLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_1 = - std::make_pair((acs::CtrlSubmode::IDLE << 24) | 3, FixedArrayList()); + std::make_pair((acs::CtrlSubmode::PTG_IDLE << 24) | 3, FixedArrayList()); auto ACS_TABLE_PTG_TRANS_0 = std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 2, FixedArrayList()); @@ -81,18 +82,18 @@ auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 = std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_NADIR = - std::make_pair(acs::CtrlSubmode::PTG_TARGET_NADIR, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 1, - FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair( - (acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList()); + std::make_pair(acs::CtrlSubmode::PTG_NADIR, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TGT = + std::make_pair((acs::CtrlSubmode::PTG_NADIR << 24) | 1, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = + std::make_pair((acs::CtrlSubmode::PTG_NADIR << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_INERTIAL = - std::make_pair(acs::CtrlSubmode::PTG_TARGET_INERTIAL, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair( - (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair( - (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList()); + std::make_pair(acs::CtrlSubmode::PTG_INERTIAL, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = + std::make_pair((acs::CtrlSubmode::PTG_INERTIAL << 24) | 1, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = + std::make_pair((acs::CtrlSubmode::PTG_INERTIAL << 24) | 3, FixedArrayList()); void satsystem::acs::init() { ModeListEntry entry; @@ -291,7 +292,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build IDLE target - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); @@ -307,7 +308,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); // Build IDLE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second); ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true); // Build IDLE sequence @@ -399,7 +400,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_NADIR, + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_NADIR, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), @@ -485,7 +486,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL, + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); @@ -498,7 +499,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL, + iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)), From afbaec8e2d7b92020c60590939b4e7423150175c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 13:10:57 +0100 Subject: [PATCH 081/119] removed deprecated struct --- mission/controller/acs/AcsParameters.h | 9 --------- 1 file changed, 9 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 1ffb33c7..b905cf47 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -74,7 +74,6 @@ class AcsParameters : public HasParametersIF { float mgm02variance[3] = {1, 1, 1}; float mgm13variance[3] = {1, 1, 1}; float mgm4variance[3] = {1, 1, 1}; - } mgmHandlingParameters; struct SusHandlingParameters { @@ -823,7 +822,6 @@ class AcsParameters : public HasParametersIF { double sunTargetDir[3] = {0, 0, 1}; double satRateRef[3] = {0, 0, 0}; - } safeModeControllerParameters; struct PointingLawParameters { @@ -837,7 +835,6 @@ class AcsParameters : public HasParametersIF { double deSatGainFactor = 1000; uint8_t desatOn = true; uint8_t enableAntiStiction = true; - } pointingLawParameters; struct TargetModeControllerParameters : PointingLawParameters { @@ -880,12 +877,6 @@ class AcsParameters : public HasParametersIF { double timeDiffVelocityMax = 30; //[s] } gpsParameters; - struct ptgTargetParameters { // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] Altitude - } ptgTargetParameters; - struct SunModelParameters { float domega = 36000.771; float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of From 454d56935c09274b35c22be8bc31f3ce483abed0 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 13:11:57 +0100 Subject: [PATCH 082/119] fixed getParameter to match defined structs again --- mission/controller/acs/AcsParameters.cpp | 200 +++++++++++++++++------ 1 file changed, 148 insertions(+), 52 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 6bbb5519..13642fe9 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -237,12 +237,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); break; case 0x4: - parameterWrapper->set(gyrHandlingParameters.gyr02variance); + parameterWrapper->set(gyrHandlingParameters.gyr0bias); break; case 0x5: - parameterWrapper->set(gyrHandlingParameters.gyr13variance); + parameterWrapper->set(gyrHandlingParameters.gyr1bias); break; case 0x6: + parameterWrapper->set(gyrHandlingParameters.gyr2bias); + break; + case 0x7: + parameterWrapper->set(gyrHandlingParameters.gyr3bias); + break; + case 0x8: + parameterWrapper->set(gyrHandlingParameters.gyr02variance); + break; + case 0x9: + parameterWrapper->set(gyrHandlingParameters.gyr13variance); + break; + case 0xA: parameterWrapper->set(gyrHandlingParameters.preferAdis); break; default: @@ -327,58 +339,157 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0x9): // PointingModeControllerParameters + case (0x9): // TargetModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(targetModeControllerParameters.refDirection); - break; - case 0x1: - parameterWrapper->set(targetModeControllerParameters.refRotRate); - break; - case 0x2: - parameterWrapper->set(targetModeControllerParameters.quatRef); - break; - case 0x3: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); - break; - case 0x4: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); - break; - case 0x5: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); - break; - case 0x6: - parameterWrapper->set(targetModeControllerParameters.blindRotRate); - break; - case 0x7: parameterWrapper->set(targetModeControllerParameters.zeta); break; - case 0x8: + case 0x1: parameterWrapper->set(targetModeControllerParameters.om); break; - case 0x9: + case 0x2: parameterWrapper->set(targetModeControllerParameters.omMax); break; - case 0xA: + case 0x3: parameterWrapper->set(targetModeControllerParameters.qiMin); break; - case 0xB: + case 0x4: parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; - case 0xC: + case 0x5: parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); break; - case 0xD: + case 0x6: parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); break; - case 0xE: + case 0x7: parameterWrapper->set(targetModeControllerParameters.desatOn); break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + break; + case 0x9: + parameterWrapper->set(targetModeControllerParameters.refDirection); + break; + case 0xA: + parameterWrapper->set(targetModeControllerParameters.refRotRate); + break; + case 0xB: + parameterWrapper->set(targetModeControllerParameters.quatRef); + break; + case 0xC: + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + break; + case 0xD: + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + break; + case 0xE: + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + break; + case 0xF: + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + break; + case 0x10: + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + break; + case 0x11: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + break; + case 0x12: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x13: + parameterWrapper->set(targetModeControllerParameters.blindRotRate); + break; default: return INVALID_IDENTIFIER_ID; } break; - case (0xA): // StrParameters + case (0xA): // NadirModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(nadirModeControllerParameters.zeta); + break; + case 0x1: + parameterWrapper->set(nadirModeControllerParameters.om); + break; + case 0x2: + parameterWrapper->set(nadirModeControllerParameters.omMax); + break; + case 0x3: + parameterWrapper->set(nadirModeControllerParameters.qiMin); + break; + case 0x4: + parameterWrapper->set(nadirModeControllerParameters.gainNullspace); + break; + case 0x5: + parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef); + break; + case 0x6: + parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); + break; + case 0x7: + parameterWrapper->set(nadirModeControllerParameters.desatOn); + break; + case 0x8: + parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); + break; + case 0x9: + parameterWrapper->set(nadirModeControllerParameters.refDirection); + break; + case 0xA: + parameterWrapper->set(nadirModeControllerParameters.quatRef); + break; + case 0xC: + parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xB): // InertialModeControllerParameters + switch (parameterId) { + case 0x0: + parameterWrapper->set(inertialModeControllerParameters.zeta); + break; + case 0x1: + parameterWrapper->set(inertialModeControllerParameters.om); + break; + case 0x2: + parameterWrapper->set(inertialModeControllerParameters.omMax); + break; + case 0x3: + parameterWrapper->set(inertialModeControllerParameters.qiMin); + break; + case 0x4: + parameterWrapper->set(inertialModeControllerParameters.gainNullspace); + break; + case 0x5: + parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef); + break; + case 0x6: + parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); + break; + case 0x7: + parameterWrapper->set(inertialModeControllerParameters.desatOn); + break; + case 0x8: + parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); + break; + case 0x9: + parameterWrapper->set(inertialModeControllerParameters.tgtQuat); + break; + case 0xA: + parameterWrapper->set(inertialModeControllerParameters.refRotRate); + break; + case 0xC: + parameterWrapper->set(inertialModeControllerParameters.quatRef); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xC): // StrParameters switch (parameterId) { case 0x0: parameterWrapper->set(strParameters.exclusionAngle); @@ -390,7 +501,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xB): // GpsParameters + case (0xD): // GpsParameters switch (parameterId) { case 0x0: parameterWrapper->set(gpsParameters.timeDiffVelocityMax); @@ -399,22 +510,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xC): // PtgTargetParameters - switch (parameterId) { - case 0x0: - parameterWrapper->set(ptgTargetParameters.latitudeTgt); - break; - case 0x1: - parameterWrapper->set(ptgTargetParameters.longitudeTgt); - break; - case 0x2: - parameterWrapper->set(ptgTargetParameters.altitudeTgt); - break; - default: - return INVALID_IDENTIFIER_ID; - } - break; - case (0xD): // SunModelParameters + case (0xE): // SunModelParameters switch (parameterId) { case 0x0: parameterWrapper->set(sunModelParameters.domega); @@ -444,7 +540,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xE): // KalmanFilterParameters + case (0xF): // KalmanFilterParameters switch (parameterId) { case 0x0: parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); @@ -468,7 +564,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0xF): // MagnetorquesParameter + case (0x10): // MagnetorquesParameter switch (parameterId) { case 0x0: parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); @@ -492,7 +588,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, return INVALID_IDENTIFIER_ID; } break; - case (0x10): // DetumbleParameter + case (0x11): // DetumbleParameter switch (parameterId) { case 0x0: parameterWrapper->set(detumbleParameter.detumblecounter); From 36f20d235c50c49b6fb189b80ae1ddc12e28de39 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 13:58:41 +0100 Subject: [PATCH 083/119] this was fun --- CHANGELOG.md | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c5eceb87..18aa0462 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,36 @@ change warranting a new major release: # [unreleased] +## Changed +- added calculation of satellite velocity vector from GPS position data +- added detumble mode using GYR values +- added inertial pointing mode +- added nadir pointing mode +- added ground station target mode +- added antistiction for RWs +- added `sunTargetSafe` differentiation for LEOP +- added check for existance of `SD_0_SKEWED_PTG_FILE` and `SD_1_SKEWED_PTG_FILE` to determine + which `sunTargetSafe` to use +- added `gpsVelocity` and `gpsPosition` to `gpsProcessed` +- removed deprecated `OutputValues` +- added `HasParametersIF` to `AcsParameters` +- added `ReceivesParameterMessagesIF` and `ParameterHelper` to `AcsController` +- updated `AcsParameters` with actual values and changed structure +- sun vector model and magnetic field vector model calculations are always executed now +- `domainId` is now used as identifier for parameter structs +- changed onboard GYR value handling from deg/s to rad/s + +## Fixed +- fixed calculation of model sun vector +- fixed calculation of model magnetic field vector +- fixed MEKF algorithm +- fixed several variable initializations +- fixed several variable types +- fixed use of `sunMagAngleMin` for safe mode +- fixed MEKF not using correct `sampleTime` +- fixed assignment of `SUS0` and `SUS6` calibration matrices due to wiring being mixed up +- various smaller bugfixes + # [v1.25.0] 2023-02-06 eive-tmtc version: v2.12.0 From ab0a3bfd45165f804b0a437cbdc3347ac262e183 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 14:02:46 +0100 Subject: [PATCH 084/119] some fixes and tweaks --- linux/devices/GpsHyperionLinuxController.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index 7554b805..aa55b696 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -102,6 +102,7 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) { if (not callAgainImmediately) { handleQueue(); poolManager.performHkOperation(); + TaskFactory::delayTask(200); } } // Should never be reached. @@ -155,7 +156,7 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { }; if (readMode == ReadModes::SOCKET) { // Perform other necessary handling if not data seen for 0.2 seconds. - if (gps_waiting(&gps, 200000)) { + if (gps_waiting(&gps, 0)) { if (-1 == gps_read(&gps)) { readError(); return false; @@ -258,7 +259,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { gpsSet.speed.setValid(false); } - if (TIME_SET != (TIME_SET & gps.set)) { + if (TIME_SET == (TIME_SET & gps.set)) { timeval time = {}; #if LIBGPS_VERSION_MINOR <= 17 gpsSet.unixSeconds.value = std::floor(gps.fix.time); From 36ed787db1fc8ad4157ad5a9f57b0b522bf3d6fe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 14:16:43 +0100 Subject: [PATCH 085/119] improve SW switch handling - also add CANT_GET_FIX event --- linux/devices/GpsHyperionLinuxController.cpp | 40 +++++++++---------- linux/devices/GpsHyperionLinuxController.h | 15 +++++-- .../devicedefinitions/GPSDefinitions.h | 3 ++ 3 files changed, 33 insertions(+), 25 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index aa55b696..ee5f2321 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -34,7 +34,6 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ uint32_t *msToReachTheMode) { if (not modeCommanded) { if (mode == MODE_ON or mode == MODE_OFF) { - gpsNotOpenSwitch = true; // 5h time to reach fix *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX; maxTimeToReachFix.resetTimer(); @@ -46,6 +45,7 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ if (mode == MODE_OFF) { PoolReadGuard pg(&gpsSet); gpsSet.setValidity(false, true); + oneShotSwitches.reset(); } return returnvalue::OK; } @@ -102,7 +102,6 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) { if (not callAgainImmediately) { handleQueue(); poolManager.performHkOperation(); - TaskFactory::delayTask(200); } } // Should never be reached. @@ -115,25 +114,24 @@ ReturnValue_t GpsHyperionLinuxController::initialize() { return result; } auto openError = [&](const char *type, int error) { - if (gpsNotOpenSwitch) { - // Opening failed + // Opening failed #if FSFW_VERBOSE_LEVEL >= 1 - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type - << " failed | Error " << error << " | " << gps_errstr(error) << std::endl; + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type + << " failed | Error " << error << " | " << gps_errstr(error) << std::endl; #endif - gpsNotOpenSwitch = false; - } }; if (readMode == ReadModes::SOCKET) { int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps); if (retval != 0) { openError("Socket", retval); + return ObjectManager::CHILD_INIT_FAILED; } gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr); } else if (readMode == ReadModes::SHM) { int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps); if (retval != 0) { openError("SHM", retval); + return ObjectManager::CHILD_INIT_FAILED; } } return result; @@ -147,8 +145,8 @@ void GpsHyperionLinuxController::performControlOperation() {} bool GpsHyperionLinuxController::readGpsDataFromGpsd() { auto readError = [&]() { - if (gpsReadFailedSwitch) { - gpsReadFailedSwitch = false; + if (oneShotSwitches.gpsReadFailedSwitch) { + oneShotSwitches.gpsReadFailedSwitch = false; sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | " "Error " << errno << " | " << gps_errstr(errno) << std::endl; @@ -156,23 +154,21 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { }; if (readMode == ReadModes::SOCKET) { // Perform other necessary handling if not data seen for 0.2 seconds. - if (gps_waiting(&gps, 0)) { + if (gps_waiting(&gps, 200000)) { if (-1 == gps_read(&gps)) { readError(); return false; } + oneShotSwitches.gpsReadFailedSwitch = true; if (MODE_SET != (MODE_SET & gps.set)) { - if (mode == MODE_ON) { - if (noModeSetCntr >= 0) { - noModeSetCntr++; - } - if (noModeSetCntr == 10) { - // TODO: Trigger event here - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be " - "read for 10 consecutive reads" - << std::endl; - noModeSetCntr = -1; - } + if (mode != MODE_OFF and maxTimeToReachFix.hasTimedOut() and + oneShotSwitches.cantGetFixSwitch) { + // TODO: Trigger event here + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be " + "read for 10 consecutive reads" + << std::endl; + triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout); + oneShotSwitches.cantGetFixSwitch = false; // did not event get mode, nothing to see. return false; } diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h index 5b0a84cd..5d4a35ff 100644 --- a/linux/devices/GpsHyperionLinuxController.h +++ b/linux/devices/GpsHyperionLinuxController.h @@ -58,10 +58,19 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { const char* currentClientBuf = nullptr; ReadModes readMode = ReadModes::SOCKET; Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); - bool modeCommanded = true; + bool modeCommanded = false; bool timeInit = false; - bool gpsNotOpenSwitch = true; - bool gpsReadFailedSwitch = true; + + struct OneShotSwitches { + void reset() { + gpsReadFailedSwitch = true; + cantGetFixSwitch = true; + } + bool gpsReadFailedSwitch = true; + bool cantGetFixSwitch = true; + + } oneShotSwitches; + bool debugHyperionGps = false; int32_t noModeSetCntr = 0; Countdown timeUpdateCd = Countdown(60); diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index 0b88cfa5..2ac7781d 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -13,6 +13,9 @@ static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER; //! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix //! 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO); +//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. P1: Maximum allowed time +//! to get a fix after the GPS was switched on. +static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW); static constexpr DeviceCommandId_t GPS_REPLY = 0; static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5; From ecc61d184baf438ded0aa6c4f64a9e57869b9d89 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 14:22:03 +0100 Subject: [PATCH 086/119] remove TODO --- linux/devices/GpsHyperionLinuxController.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index ee5f2321..8d868512 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -163,10 +163,8 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { if (MODE_SET != (MODE_SET & gps.set)) { if (mode != MODE_OFF and maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) { - // TODO: Trigger event here - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be " - "read for 10 consecutive reads" - << std::endl; + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed " + << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl; triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout); oneShotSwitches.cantGetFixSwitch = false; // did not event get mode, nothing to see. From 540bd63f4cc058defcfab66cae1fd5c83e584114 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 14:22:18 +0100 Subject: [PATCH 087/119] afmt --- linux/devices/GpsHyperionLinuxController.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index 8d868512..e4bd8c7f 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -163,7 +163,8 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { if (MODE_SET != (MODE_SET & gps.set)) { if (mode != MODE_OFF and maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) { - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed " + sif::warning + << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed " << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl; triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout); oneShotSwitches.cantGetFixSwitch = false; From b2de4ee08c668a73b98ba6361b6c78c0a6a71eb4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 16:57:24 +0100 Subject: [PATCH 088/119] use full polling for gps_wating, manual delay --- linux/devices/GpsHyperionLinuxController.cpp | 12 ++++++++++-- tmtc | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index e4bd8c7f..7350aebc 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -45,7 +45,10 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ if (mode == MODE_OFF) { PoolReadGuard pg(&gpsSet); gpsSet.setValidity(false, true); + // There can't be a fix with a device that is off. + triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0); oneShotSwitches.reset(); + modeCommanded = false; } return returnvalue::OK; } @@ -102,6 +105,7 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) { if (not callAgainImmediately) { handleQueue(); poolManager.performHkOperation(); + TaskFactory::delayTask(250); } } // Should never be reached. @@ -152,9 +156,13 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { << errno << " | " << gps_errstr(errno) << std::endl; } }; + // GPS is off, no point in reading data from GPSD. + if(mode == MODE_OFF) { + return false; + } if (readMode == ReadModes::SOCKET) { // Perform other necessary handling if not data seen for 0.2 seconds. - if (gps_waiting(&gps, 200000)) { + if (gps_waiting(&gps, 0)) { if (-1 == gps_read(&gps)) { readError(); return false; @@ -212,7 +220,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { modeCommanded = false; } gpsSet.setValidity(false, true); - } else if (gps.satellites_used > 0 && validFix) { + } else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) { gpsSet.setValidity(true, true); } diff --git a/tmtc b/tmtc index d6445d38..8a96e21d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d6445d38a8eb644a5e1bd27f0fc56d29e93c030d +Subproject commit 8a96e21d1a1ca790cd1053594495dd74beb12133 From f5ba8595607592350a51c2792e9ace567207d281 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 17:01:07 +0100 Subject: [PATCH 089/119] comment fix --- linux/devices/GpsHyperionLinuxController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index 7350aebc..ec9cb61a 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -161,7 +161,7 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { return false; } if (readMode == ReadModes::SOCKET) { - // Perform other necessary handling if not data seen for 0.2 seconds. + // Poll the GPS. if (gps_waiting(&gps, 0)) { if (-1 == gps_read(&gps)) { readError(); From bc6cecc14a51ce39e6102c6154dffb9dd87223e0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 17:02:22 +0100 Subject: [PATCH 090/119] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index d6445d38..8a96e21d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d6445d38a8eb644a5e1bd27f0fc56d29e93c030d +Subproject commit 8a96e21d1a1ca790cd1053594495dd74beb12133 From e8120fcdefc0d2a73bf3ee69d4538618691ab604 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 17:18:13 +0100 Subject: [PATCH 091/119] allow passsing TM dest names --- fsfw | 2 +- linux/ObjectFactory.cpp | 4 ++-- mission/core/GenericFactory.cpp | 8 ++++---- mission/tmtc/CfdpTmFunnel.cpp | 7 ++++--- mission/tmtc/PusTmFunnel.cpp | 7 ++++--- mission/tmtc/TmFunnelBase.cpp | 5 +++-- mission/tmtc/TmFunnelBase.h | 15 +++++++++++++-- 7 files changed, 31 insertions(+), 17 deletions(-) diff --git a/fsfw b/fsfw index 38789e05..69c94645 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 38789e053b65cfa14604fc625e7bcc8ca03a3f17 +Subproject commit 69c94645df81c2fc9d538a0f19d2c6552e1c73a7 diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index 469187a6..2f04b294 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -363,6 +363,6 @@ void ObjectFactory::gpioChecker(ReturnValue_t result, std::string output) { void ObjectFactory::addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel) { - cfdpFunnel.addDestination(ipCoreHandler, config::LIVE_TM); - pusFunnel.addDestination(ipCoreHandler, config::LIVE_TM); + cfdpFunnel.addDestination("PTME IP Core", ipCoreHandler, config::LIVE_TM); + pusFunnel.addDestination("PTME IP Core", ipCoreHandler, config::LIVE_TM); } diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp index a5acd734..d486ed59 100644 --- a/mission/core/GenericFactory.cpp +++ b/mission/core/GenericFactory.cpp @@ -127,12 +127,12 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun config::MAX_PUS_FUNNEL_QUEUE_DEPTH); #if OBSW_ADD_TCPIP_SERVERS == 1 #if OBSW_ADD_TMTC_UDP_SERVER == 1 - (*cfdpFunnel)->addDestination(*udpBridge, 0); - (*pusFunnel)->addDestination(*udpBridge, 0); + (*cfdpFunnel)->addDestination("UDP Server", *udpBridge, 0); + (*pusFunnel)->addDestination("UDP Server", *udpBridge, 0); #endif #if OBSW_ADD_TMTC_TCP_SERVER == 1 - (*cfdpFunnel)->addDestination(*tcpBridge, 0); - (*pusFunnel)->addDestination(*tcpBridge, 0); + (*cfdpFunnel)->addDestination("TCP Server", *tcpBridge, 0); + (*pusFunnel)->addDestination("TCP Server", *tcpBridge, 0); #endif #endif // Every TM packet goes through this funnel diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp index aff7c95d..f19d5360 100644 --- a/mission/tmtc/CfdpTmFunnel.cpp +++ b/mission/tmtc/CfdpTmFunnel.cpp @@ -68,7 +68,7 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) { msg.setStorageId(newStoreId); store_address_t origStoreId = newStoreId; for (unsigned int idx = 0; idx < destinations.size(); idx++) { - const auto& destVcidPair = destinations[idx]; + const auto& dest = destinations[idx]; if (destinations.size() > 1) { if (idx < destinations.size() - 1) { // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need @@ -87,10 +87,11 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) { msg.setStorageId(origStoreId); } } - result = tmQueue->sendMessage(destVcidPair.first, &msg); + result = tmQueue->sendMessage(dest.queueId, &msg); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; + sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler " << dest.name + << " failed" << std::endl; #endif tmStore.deleteData(msg.getStorageId()); } diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp index 5d6bbb4d..dda854a6 100644 --- a/mission/tmtc/PusTmFunnel.cpp +++ b/mission/tmtc/PusTmFunnel.cpp @@ -49,7 +49,7 @@ ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) { packet.updateErrorControl(); for (unsigned int idx = 0; idx < destinations.size(); idx++) { - const auto &destVcidPair = destinations[idx]; + const auto &dest = destinations[idx]; if (destinations.size() > 1) { if (idx < destinations.size() - 1) { // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need @@ -68,10 +68,11 @@ ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) { message.setStorageId(origStoreId); } } - result = tmQueue->sendMessage(destVcidPair.first, &message); + result = tmQueue->sendMessage(dest.queueId, &message); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; + sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler " << dest.name + << " failed" << std::endl; #endif tmStore.deleteData(message.getStorageId()); } diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp index fa0062e6..51bf93a1 100644 --- a/mission/tmtc/TmFunnelBase.cpp +++ b/mission/tmtc/TmFunnelBase.cpp @@ -13,7 +13,8 @@ MessageQueueId_t TmFunnelBase::getReportReceptionQueue(uint8_t virtualChannel) c return tmQueue->getId(); } -void TmFunnelBase::addDestination(const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) { +void TmFunnelBase::addDestination(const char *name, const AcceptsTelemetryIF &downlinkDestination, + uint8_t vcid) { auto queueId = downlinkDestination.getReportReceptionQueue(vcid); - destinations.emplace_back(queueId, vcid); + destinations.emplace_back(name, queueId, vcid); } diff --git a/mission/tmtc/TmFunnelBase.h b/mission/tmtc/TmFunnelBase.h index c630fefd..408244dd 100644 --- a/mission/tmtc/TmFunnelBase.h +++ b/mission/tmtc/TmFunnelBase.h @@ -10,14 +10,25 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject { public: TmFunnelBase(object_id_t objectId, StorageManagerIF& tmStore, uint32_t tmMsgDepth); - void addDestination(const AcceptsTelemetryIF& downlinkDestination, uint8_t vcid = 0); + void addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination, + uint8_t vcid = 0); [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; virtual ~TmFunnelBase(); protected: StorageManagerIF& tmStore; - std::vector> destinations; + struct Destination { + Destination(const char* name, MessageQueueId_t queueId, uint8_t virtualChannel) + : name(name), queueId(queueId), virtualChannel(virtualChannel) {} + + const char* name; + MessageQueueId_t queueId; + uint8_t virtualChannel = 0; + }; + + std::vector destinations; + MessageQueueIF* tmQueue = nullptr; }; From 8bc71b268ebef87801a7e5a9e2b6120a5fa01ab6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 17:28:59 +0100 Subject: [PATCH 092/119] some tests with subsystem --- bsp_q7s/core/scheduling.cpp | 2 +- mission/acsDefs.h | 3 +- mission/controller/AcsController.cpp | 2 +- mission/system/objects/AcsSubsystem.cpp | 4 +- mission/system/tree/acsModeTree.cpp | 88 ++++++++++++------------- 5 files changed, 50 insertions(+), 49 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 150efa97..e8f83f0e 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -414,7 +414,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction ReturnValue_t result = returnvalue::OK; FixedTimeslotTaskIF* acsPst = factory.createFixedTimeslotTask( - "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 1.0, missedDeadlineFunc); result = pst::pstAcs(acsPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 8869d6ff..ca4ab77c 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -6,7 +6,8 @@ namespace acs { -enum CtrlSubmode { +// These modes are the submodes of the ACS controller and the modes of the ACS subsystem. +enum AcsModes { OFF = HasModesIF::MODE_OFF, SAFE = 10, DETUMBLE = 11, diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7544c471..7d9cc015 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -426,7 +426,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode > 6) || (submode < 2)) { + if (submode < acs::AcsModes::SAFE or submode > acs::AcsModes::PTG_TARGET_INERTIAL) { return INVALID_SUBMODE; } else { return returnvalue::OK; diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index f5b8a0ce..25b601ac 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -58,7 +58,7 @@ void AcsSubsystem::handleEventMessages() { case EventMessage::EVENT_MESSAGE: if (event.getEvent() == acs::SAFE_RATE_VIOLATION) { CommandMessage msg; - ModeMessage::setCmdModeModeMessage(msg, acs::CtrlSubmode::DETUMBLE, 0); + ModeMessage::setCmdModeModeMessage(msg, acs::AcsModes::DETUMBLE, 0); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); if (result != returnvalue::OK) { sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl; @@ -66,7 +66,7 @@ void AcsSubsystem::handleEventMessages() { } if (event.getEvent() == acs::SAFE_RATE_RECOVERY) { CommandMessage msg; - ModeMessage::setCmdModeModeMessage(msg, acs::CtrlSubmode::SAFE, 0); + ModeMessage::setCmdModeModeMessage(msg, acs::AcsModes::SAFE, 0); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); if (result != returnvalue::OK) { sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl; diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index 849959f9..db7d7a81 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -30,69 +30,69 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper); static const auto OFF = HasModesIF::MODE_OFF; static const auto NML = DeviceHandlerIF::MODE_NORMAL; -auto ACS_SEQUENCE_OFF = std::make_pair(acs::CtrlSubmode::OFF, FixedArrayList()); +auto ACS_SEQUENCE_OFF = std::make_pair(acs::AcsModes::OFF, FixedArrayList()); auto ACS_TABLE_OFF_TGT = - std::make_pair((acs::CtrlSubmode::OFF << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsModes::OFF << 24) | 1, FixedArrayList()); auto ACS_TABLE_OFF_TRANS_0 = - std::make_pair((acs::CtrlSubmode::OFF << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsModes::OFF << 24) | 2, FixedArrayList()); auto ACS_TABLE_OFF_TRANS_1 = - std::make_pair((acs::CtrlSubmode::OFF << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsModes::OFF << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_DETUMBLE = - std::make_pair(acs::CtrlSubmode::DETUMBLE, FixedArrayList()); + std::make_pair(acs::AcsModes::DETUMBLE, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TGT = - std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsModes::DETUMBLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TRANS_0 = - std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsModes::DETUMBLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TRANS_1 = - std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsModes::DETUMBLE << 24) | 3, FixedArrayList()); -auto ACS_SEQUENCE_SAFE = std::make_pair(acs::CtrlSubmode::SAFE, FixedArrayList()); +auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsModes::SAFE, FixedArrayList()); auto ACS_TABLE_SAFE_TGT = - std::make_pair((acs::CtrlSubmode::SAFE << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsModes::SAFE << 24) | 1, FixedArrayList()); auto ACS_TABLE_SAFE_TRANS_0 = - std::make_pair((acs::CtrlSubmode::SAFE << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsModes::SAFE << 24) | 2, FixedArrayList()); auto ACS_TABLE_SAFE_TRANS_1 = - std::make_pair((acs::CtrlSubmode::SAFE << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsModes::SAFE << 24) | 3, FixedArrayList()); -auto ACS_SEQUENCE_IDLE = std::make_pair(acs::CtrlSubmode::IDLE, FixedArrayList()); +auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsModes::IDLE, FixedArrayList()); auto ACS_TABLE_IDLE_TGT = - std::make_pair((acs::CtrlSubmode::IDLE << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsModes::IDLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_0 = - std::make_pair((acs::CtrlSubmode::IDLE << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsModes::IDLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_1 = - std::make_pair((acs::CtrlSubmode::IDLE << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsModes::IDLE << 24) | 3, FixedArrayList()); auto ACS_TABLE_PTG_TRANS_0 = - std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsModes::PTG_TARGET << 24) | 2, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET = - std::make_pair(acs::CtrlSubmode::PTG_TARGET, FixedArrayList()); + std::make_pair(acs::AcsModes::PTG_TARGET, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_TGT = - std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsModes::PTG_TARGET << 24) | 1, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_TRANS_1 = - std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsModes::PTG_TARGET << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_GS = - std::make_pair(acs::CtrlSubmode::PTG_TARGET_GS, FixedArrayList()); + std::make_pair(acs::AcsModes::PTG_TARGET_GS, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_GS_TGT = - std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsModes::PTG_TARGET_GS << 24) | 1, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 = - std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsModes::PTG_TARGET_GS << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_NADIR = - std::make_pair(acs::CtrlSubmode::PTG_TARGET_NADIR, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 1, + std::make_pair(acs::AcsModes::PTG_TARGET_NADIR, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::AcsModes::PTG_TARGET_NADIR << 24) | 1, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair( - (acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList()); + (acs::AcsModes::PTG_TARGET_NADIR << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_INERTIAL = - std::make_pair(acs::CtrlSubmode::PTG_TARGET_INERTIAL, FixedArrayList()); + std::make_pair(acs::AcsModes::PTG_TARGET_INERTIAL, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair( - (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList()); + (acs::AcsModes::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair( - (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList()); + (acs::AcsModes::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList()); void satsystem::acs::init() { ModeListEntry entry; @@ -122,7 +122,7 @@ void satsystem::acs::init() { buildTargetPtGsSequence(ACS_SUBSYSTEM, entry); buildTargetPtNadirSequence(ACS_SUBSYSTEM, entry); buildTargetPtInertialSequence(ACS_SUBSYSTEM, entry); - ACS_SUBSYSTEM.setInitialMode(::acs::CtrlSubmode::SAFE); + ACS_SUBSYSTEM.setInitialMode(::acs::AcsModes::SAFE); } namespace { @@ -190,7 +190,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build SAFE target - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::SAFE, ACS_TABLE_SAFE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::SAFE, ACS_TABLE_SAFE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); @@ -206,7 +206,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build SAFE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::SAFE, ACS_TABLE_SAFE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::SAFE, ACS_TABLE_SAFE_TRANS_1.second); check(ss.addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true), ctxc); @@ -239,7 +239,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build DETUMBLE target - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); @@ -257,7 +257,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build DETUMBLE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second); check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false, true), ctxc); @@ -291,7 +291,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build IDLE target - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); @@ -307,7 +307,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); // Build IDLE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::IDLE, ACS_TABLE_IDLE_TRANS_1.second); ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true); // Build IDLE sequence @@ -339,7 +339,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); @@ -350,7 +350,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second); check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false, true), @@ -386,7 +386,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); @@ -399,7 +399,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_NADIR, + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_NADIR, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), @@ -436,7 +436,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_GS, + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_GS, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); @@ -449,7 +449,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_GS, + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_GS, ACS_TABLE_PTG_TARGET_GS_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_GS_TRANS_1.first, &ACS_TABLE_PTG_TARGET_GS_TRANS_1.second)), @@ -485,7 +485,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL, + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); @@ -498,7 +498,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL, + iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)), From 6829c16ced541ab772b41c158d4baf84a4454ef7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 17:50:44 +0100 Subject: [PATCH 093/119] that seems to do the trick --- CMakeLists.txt | 6 +++--- bsp_q7s/OBSWConfig.h.in | 2 ++ cmake/BuildType.cmake | 3 +++ 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 97b1b2a0..69c3d8f8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -301,6 +301,9 @@ else() endif() endif() +include(BuildType) +set_build_type() + # Configuration files configure_file(${COMMON_CONFIG_PATH}/commonConfig.h.in commonConfig.h) configure_file(${FSFW_CONFIG_PATH}/FSFWConfig.h.in FSFWConfig.h) @@ -559,6 +562,3 @@ add_custom_command( POST_BUILD COMMAND ${CMAKE_SIZE} ${OBSW_BIN_NAME}${FILE_SUFFIX} COMMENT ${POST_BUILD_COMMENT}) - -include(BuildType) -set_build_type() diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 5dff001c..be5c7dc9 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -10,6 +10,8 @@ #include "q7sConfig.h" #include "OBSWVersion.h" +#cmakedefine RELEASE_BUILD + /*******************************************************************/ /** All of the following flags should be enabled for mission code */ /*******************************************************************/ diff --git a/cmake/BuildType.cmake b/cmake/BuildType.cmake index e078e5c7..ee027d13 100644 --- a/cmake/BuildType.cmake +++ b/cmake/BuildType.cmake @@ -21,10 +21,13 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) ) endif() +set(RELEASE_BUILD 1 PARENT_SCOPE) + if(${CMAKE_BUILD_TYPE} MATCHES "Debug") message(STATUS "Building Debug application with flags: ${CMAKE_C_FLAGS_DEBUG}" ) + set(RELEASE_BUILD 0 PARENT_SCOPE) elseif(${CMAKE_BUILD_TYPE} MATCHES "RelWithDebInfo") message(STATUS "Building Release (Debug) application with " From 835d2277d8b530741e177d145b90a4c0635c51be Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 17:54:56 +0100 Subject: [PATCH 094/119] impl variable freq depending on build type --- bsp_q7s/core/scheduling.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index e8f83f0e..42a11875 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -413,8 +413,13 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction std::vector& taskVec) { ReturnValue_t result = returnvalue::OK; +#ifdef RELEASE_BUILD + static constexpr float acsPstPeriod = 0.4; +#else + static constexpr float acsPstPeriod = 0.6; +#endif FixedTimeslotTaskIF* acsPst = factory.createFixedTimeslotTask( - "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 1.0, missedDeadlineFunc); + "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc); result = pst::pstAcs(acsPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { From 4f2853e1b81e742258465676b8196bf8e92f7b89 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 18:07:19 +0100 Subject: [PATCH 095/119] adapted new acs submodes for checkModeCommand --- mission/controller/AcsController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8a58eb90..2e53d17c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -426,7 +426,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode > 6) || (submode < 2)) { + if ((submode > acs::PTG_TARGET_INERTIAL) || (submode < acs::SAFE)) { return INVALID_SUBMODE; } else { return returnvalue::OK; From 6cda616236389c8a7719f828f8afc621dcd71220 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 18:10:48 +0100 Subject: [PATCH 096/119] update changelog --- CHANGELOG.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 787c5d25..e6237272 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -53,7 +53,8 @@ eive-tmtc version: v2.12.0 ## Changed - Update ACS scheduling to represent the actual ACS design. There is one ACS PST now for all - timing sensitive ACS operations. + timing sensitive ACS operations. In the debug builds, the new ACS polling sequence table + will have a period of 0.6 seconds, but will remain 0.4 seconds for the release build. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/365 - `ACS::SensorValues` is now an ACS controller member to reduce the risk of stack overflow. - ACS Subsystem Sequence Mode IDs updated. From 2580b7cb20b92598a652dcd620a1a2d7f3593dbf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 19:37:23 +0100 Subject: [PATCH 097/119] proper handling for git sha hash --- CMakeLists.txt | 5 ++++- cmake/EiveHelpers.cmake | 7 +++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 97b1b2a0..25e63e69 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -171,7 +171,10 @@ if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/.git) list(GET GIT_INFO 1 OBSW_VERSION_MAJOR) list(GET GIT_INFO 2 OBSW_VERSION_MINOR) list(GET GIT_INFO 3 OBSW_VERSION_REVISION) - list(GET GIT_INFO 4 OBSW_VERSION_CST_GIT_SHA1) + list(LENGTH GIT_INFO LIST_LEN) + if(LIST_LEN GREATER 4) + list(GET GIT_INFO 4 OBSW_VERSION_CST_GIT_SHA1) + endif() if(NOT OBSW_VERSION_MAJOR) set(OBSW_VERSION_MAJOR ${OBSW_VERSION_MAJOR_IF_GIT_FAILS}) endif() diff --git a/cmake/EiveHelpers.cmake b/cmake/EiveHelpers.cmake index 1a717552..6410bf28 100644 --- a/cmake/EiveHelpers.cmake +++ b/cmake/EiveHelpers.cmake @@ -4,7 +4,7 @@ # 2. Major version # 3. Minor version # 4. Revision -# 5. git SHA hash and commits since tag +# 5. (Optional) git SHA hash and commits since tag when applicable function(determine_version_with_git) include(GetGitRevisionDescription) git_describe(VERSION ${ARGN}) @@ -22,7 +22,10 @@ function(determine_version_with_git) list(APPEND GIT_INFO ${_VERSION_MAJOR}) list(APPEND GIT_INFO ${_VERSION_MINOR}) list(APPEND GIT_INFO ${_VERSION_PATCH}) - list(APPEND GIT_INFO ${VERSION_SHA1}) + if(NOT VERSION_SHA1 STREQUAL VERSION) + list(APPEND GIT_INFO ${VERSION_SHA1}) + endif() set(GIT_INFO ${GIT_INFO} PARENT_SCOPE) + message(STATUS "GIT INFO: ${GIT_INFO}") message(STATUS "eive | Set git version info into GIT_INFO from the git tag ${VERSION}") endfunction() From 7514071d799e31e4348d69ed7f697697c1b16a37 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 19:39:29 +0100 Subject: [PATCH 098/119] remove stray printout --- cmake/EiveHelpers.cmake | 1 - 1 file changed, 1 deletion(-) diff --git a/cmake/EiveHelpers.cmake b/cmake/EiveHelpers.cmake index 6410bf28..b210739e 100644 --- a/cmake/EiveHelpers.cmake +++ b/cmake/EiveHelpers.cmake @@ -26,6 +26,5 @@ function(determine_version_with_git) list(APPEND GIT_INFO ${VERSION_SHA1}) endif() set(GIT_INFO ${GIT_INFO} PARENT_SCOPE) - message(STATUS "GIT INFO: ${GIT_INFO}") message(STATUS "eive | Set git version info into GIT_INFO from the git tag ${VERSION}") endfunction() From 9373a244131b8b9dcbf8738ff34c521ace99cb45 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 19:40:32 +0100 Subject: [PATCH 099/119] bump changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c5eceb87..e6fee72b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,12 @@ change warranting a new major release: # [unreleased] +## Fixed + +- Build system: Fixed small bug, where the version itself was + stored as the git SHA hash in `commonConfig.h`. This will be + an empty string now for regular versions. + # [v1.25.0] 2023-02-06 eive-tmtc version: v2.12.0 From 6a24e1b9357ec0a53da11ffcbbdaace7bf493c52 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 20:11:16 +0100 Subject: [PATCH 100/119] updated MGM calibration values --- mission/controller/acs/AcsParameters.h | 32 +++++++++++++------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index a7b2d631..c4653590 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -41,23 +41,23 @@ class AcsParameters /*: public HasParametersIF*/ { float mgm1hardIronOffset[3] = {-1.077152, 2.080583, 1.974483}; float mgm2hardIronOffset[3] = {-19.285857, 5.401821, -16.096297}; float mgm3hardIronOffset[3] = {-0.634033, 2.787695, 0.092036}; - float mgm4hardIronOffset[3] = {1.722133, 4.511870, 0.379980}; + float mgm4hardIronOffset[3] = {2.702743, 5.236043, 0.726229}; - float mgm0softIronInverse[3][3] = {{1.163335, 0.137584, 0.203908}, - {0.137584, 0.625606, 0.045537}, - {0.203908, 0.045537, 1.095923}}; - float mgm1softIronInverse[3][3] = {{0.972071, 0.131166, 0.042151}, - {0.131166, 0.803260, 0.034779}, - {0.042151, 0.034779, 0.923417}}; - float mgm2softIronInverse[3][3] = {{1.234863, -0.157745, 0.070813}, - {-0.157745, 0.746707, -0.062080}, - {0.070813, -0.062080, 1.122514}}; - float mgm3softIronInverse[3][3] = {{0.956890, -0.134841, 0.054017}, - {-0.134841, 0.811361, -0.015383}, - {0.054017, -0.015383, 0.939153}}; - float mgm4softIronInverse[3][3] = {{0.913163, 0.011058, 0.023273}, - {0.011058, 0.792295, 0.002921}, - {0.023273, 0.002921, 0.739496}}; + float mgm0softIronInverse[3][3] = {{0.910192, -0.188413, -0.161522}, + {-0.188413, 1.642303, -0.033184}, + {-0.161522, -0.033184, 0.943904}}; + float mgm1softIronInverse[3][3] = {{1.053508, -0.170225, -0.041678}, + {-0.170225, 1.274465, -0.040231}, + {-0.041678, -0.040231, 1.086352}}; + float mgm2softIronInverse[3][3] = {{0.931086, 0.172675, -0.043084}, + {0.172675, 1.541296, 0.065489}, + {-0.043084, 0.065489, 1.001238}}; + float mgm3softIronInverse[3][3] = {{1.073353, 0.177266, -0.058832}, + {0.177266, 1.262156, 0.010478}, + {-0.058832, 0.010478, 1.068345}}; + float mgm4softIronInverse[3][3] = {{1.114887, -0.007534, -0.037072}, + {-0.007534, 1.253879, 0.006812}, + {-0.037072, 0.006812, 1.313158}}; float mgm02variance[3] = {1, 1, 1}; float mgm13variance[3] = {1, 1, 1}; From d6a40487934e17c42b38bce1e7a85b6f04385e2c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 01:32:18 +0100 Subject: [PATCH 101/119] bump FSFW, TMTC and changelog --- CHANGELOG.md | 1 + fsfw | 2 +- tmtc | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e6fee72b..ddeee97b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,7 @@ change warranting a new major release: - Build system: Fixed small bug, where the version itself was stored as the git SHA hash in `commonConfig.h`. This will be an empty string now for regular versions. +- Bump FSFW for important fix in PUS mode service. # [v1.25.0] 2023-02-06 diff --git a/fsfw b/fsfw index 38789e05..84dc7ac0 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 38789e053b65cfa14604fc625e7bcc8ca03a3f17 +Subproject commit 84dc7ac0ce4f16d93a6a5a5fcd453b2fde206dba diff --git a/tmtc b/tmtc index d6445d38..f16e27b7 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d6445d38a8eb644a5e1bd27f0fc56d29e93c030d +Subproject commit f16e27b79b9996f5a6c246cea560b783eb566d7e From 489c813bcc637bd519b0b14eb40cb625b4988224 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 8 Feb 2023 11:05:35 +0100 Subject: [PATCH 102/119] better fit for acs::OFF with SUBMODE_NONE --- mission/acsDefs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 8869d6ff..63824131 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -7,7 +7,7 @@ namespace acs { enum CtrlSubmode { - OFF = HasModesIF::MODE_OFF, + OFF = HasModesIF::SUBMODE_NONE, SAFE = 10, DETUMBLE = 11, IDLE = 12, From c170bff7202dca5e0fd958d68db0a52ebfb08ef9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 11:08:38 +0100 Subject: [PATCH 103/119] add release checklist --- CHANGELOG.md | 2 ++ CMakeLists.txt | 14 +++++++------- bsp_hosted/OBSWConfig.h.in | 1 - bsp_q7s/OBSWConfig.h.in | 1 - bsp_q7s/core/CoreController.cpp | 13 +++++++++---- bsp_q7s/core/CoreController.h | 8 ++++++++ common/config/OBSWVersion.h | 10 ---------- fsfw | 2 +- mission/system/objects/AcsSubsystem.cpp | 4 ++-- release_checklist.md | 12 ++++++++++++ unittest/rebootLogic/src/CoreController.h | 2 +- 11 files changed, 42 insertions(+), 27 deletions(-) delete mode 100644 common/config/OBSWVersion.h create mode 100644 release_checklist.md diff --git a/CHANGELOG.md b/CHANGELOG.md index ddeee97b..336fce6c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,8 @@ change warranting a new major release: ## Fixed +- Single sourcing the version information into `CMakeLists.txt`. The `git describe` functionality + is only used to retrieve the git SHA hash now. Also removed `OBSWVersion.h` accordingly. - Build system: Fixed small bug, where the version itself was stored as the git SHA hash in `commonConfig.h`. This will be an empty string now for regular versions. diff --git a/CMakeLists.txt b/CMakeLists.txt index 25e63e69..f670b6f9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,9 +9,9 @@ # ############################################################################## cmake_minimum_required(VERSION 3.13) -set(OBSW_VERSION_MAJOR_IF_GIT_FAILS 1) -set(OBSW_VERSION_MINOR_IF_GIT_FAILS 25) -set(OBSW_VERSION_REVISION_IF_GIT_FAILS 0) +set(OBSW_VERSION_MAJOR 1) +set(OBSW_VERSION_MINOR 25) +set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) @@ -168,12 +168,12 @@ if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/.git) set(GIT_INFO ${GIT_INFO} CACHE STRING "Version information retrieved with git describe") - list(GET GIT_INFO 1 OBSW_VERSION_MAJOR) - list(GET GIT_INFO 2 OBSW_VERSION_MINOR) - list(GET GIT_INFO 3 OBSW_VERSION_REVISION) + # CMakeLists.txt is now single source of information. list(GET GIT_INFO 1 + # OBSW_VERSION_MAJOR) list(GET GIT_INFO 2 OBSW_VERSION_MINOR) list(GET + # GIT_INFO 3 OBSW_VERSION_REVISION) list(LENGTH GIT_INFO LIST_LEN) if(LIST_LEN GREATER 4) - list(GET GIT_INFO 4 OBSW_VERSION_CST_GIT_SHA1) + list(GET GIT_INFO 4 OBSW_VERSION_CST_GIT_SHA1) endif() if(NOT OBSW_VERSION_MAJOR) set(OBSW_VERSION_MAJOR ${OBSW_VERSION_MAJOR_IF_GIT_FAILS}) diff --git a/bsp_hosted/OBSWConfig.h.in b/bsp_hosted/OBSWConfig.h.in index e58e534d..5e6fcc2c 100644 --- a/bsp_hosted/OBSWConfig.h.in +++ b/bsp_hosted/OBSWConfig.h.in @@ -7,7 +7,6 @@ #define FSFWCONFIG_OBSWCONFIG_H_ #include "commonConfig.h" -#include "OBSWVersion.h" /*******************************************************************/ /** All of the following flags should be enabled for mission code */ diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 5dff001c..9273e4e9 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -8,7 +8,6 @@ #include "commonConfig.h" #include "q7sConfig.h" -#include "OBSWVersion.h" /*******************************************************************/ /** All of the following flags should be enabled for mission code */ diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 23c6cc4f..50c662d7 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -5,7 +5,7 @@ #include #include -#include "OBSWVersion.h" +#include "commonConfig.h" #include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/timemanager/Stopwatch.h" #include "fsfw/version.h" @@ -179,6 +179,11 @@ ReturnValue_t CoreController::initializeAfterTaskCreation() { ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t *data, size_t size) { switch (actionId) { + case (ANNOUNCE_VERSION): { + // uint32_t p1= + triggerEvent(VERSION_INFO); + return HasActionsIF::EXECUTION_FINISHED; + } case (LIST_DIRECTORY_INTO_FILE): { return actionListDirectoryIntoFile(actionId, commandedBy, data, size); } @@ -673,9 +678,9 @@ ReturnValue_t CoreController::initVersionFile() { sif::warning << "CoreController::versionFileInit: Retrieving uname line failed" << std::endl; } - std::string fullObswVersionString = "OBSW: v" + std::to_string(SW_VERSION) + "." + - std::to_string(SW_SUBVERSION) + "." + - std::to_string(SW_REVISION); + std::string fullObswVersionString = "OBSW: v" + std::to_string(common::OBSW_VERSION_MAJOR) + "." + + std::to_string(common::OBSW_VERSION_MINOR) + "." + + std::to_string(common::OBSW_VERSION_REVISION); char versionString[16] = {}; fsfw::FSFW_VERSION.getVersion(versionString, sizeof(versionString)); std::string fullFsfwVersionString = "FSFW: v" + std::string(versionString); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 4d9d5ee8..9cbc6340 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -74,6 +74,8 @@ class CoreController : public ExtendedControllerBase { static constexpr dur_millis_t DEFAULT_SD_CARD_CHECK_TIMEOUT = 60000; static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0; + static constexpr ActionId_t ANNOUNCE_VERSION = 1; + static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2; static constexpr ActionId_t SWITCH_REBOOT_FILE_HANDLING = 5; static constexpr ActionId_t RESET_REBOOT_COUNTERS = 6; static constexpr ActionId_t SWITCH_IMG_LOCK = 7; @@ -109,6 +111,12 @@ class CoreController : public ExtendedControllerBase { //! [EXPORT] : [COMMENT] No SD card was active. Core controller will attempt to re-initialize //! a SD card. static constexpr Event NO_SD_CARD_ACTIVE = event::makeEvent(SUBSYSTEM_ID, 4, severity::HIGH); + //! [EXPORT] : [COMMENT] + //! P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash + //! P2: First four letters of Git SHA is the last byte of P1 is set. + static constexpr Event VERSION_INFO = event::makeEvent(SUBSYSTEM_ID, 5, severity::INFO); + //! [EXPORT] : [COMMENT] P1: Current Chip, P2: Current Copy + static constexpr Event CURRENT_IMAGE_INFO = event::makeEvent(SUBSYSTEM_ID, 6, severity::INFO); CoreController(object_id_t objectId); virtual ~CoreController(); diff --git a/common/config/OBSWVersion.h b/common/config/OBSWVersion.h deleted file mode 100644 index 36233c3b..00000000 --- a/common/config/OBSWVersion.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef COMMON_CONFIG_OBSWVERSION_H_ -#define COMMON_CONFIG_OBSWVERSION_H_ - -const char* const SW_NAME = "eive"; - -#define SW_VERSION 1 -#define SW_SUBVERSION 12 -#define SW_REVISION 1 - -#endif /* COMMON_CONFIG_OBSWVERSION_H_ */ diff --git a/fsfw b/fsfw index 84dc7ac0..84bbef01 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 84dc7ac0ce4f16d93a6a5a5fcd453b2fde206dba +Subproject commit 84bbef016712096147e8cf3f2cec87f317d9e7e7 diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index f5b8a0ce..6f7bbf7c 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -58,7 +58,7 @@ void AcsSubsystem::handleEventMessages() { case EventMessage::EVENT_MESSAGE: if (event.getEvent() == acs::SAFE_RATE_VIOLATION) { CommandMessage msg; - ModeMessage::setCmdModeModeMessage(msg, acs::CtrlSubmode::DETUMBLE, 0); + ModeMessage::setCmdModeMessage(msg, acs::CtrlSubmode::DETUMBLE, 0); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); if (result != returnvalue::OK) { sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl; @@ -66,7 +66,7 @@ void AcsSubsystem::handleEventMessages() { } if (event.getEvent() == acs::SAFE_RATE_RECOVERY) { CommandMessage msg; - ModeMessage::setCmdModeModeMessage(msg, acs::CtrlSubmode::SAFE, 0); + ModeMessage::setCmdModeMessage(msg, acs::CtrlSubmode::SAFE, 0); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); if (result != returnvalue::OK) { sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl; diff --git a/release_checklist.md b/release_checklist.md new file mode 100644 index 00000000..b4f08165 --- /dev/null +++ b/release_checklist.md @@ -0,0 +1,12 @@ +OBSW Release Checklist +========= + +# Pre-Release + +1. Update version in `CMakeLists.txt` +2. Verify that the Q7S, Q7S EM and Host build are working +3. Wait for CI/CD results + +# Post-Release + +1. Create a new release with tag on `EGit` diff --git a/unittest/rebootLogic/src/CoreController.h b/unittest/rebootLogic/src/CoreController.h index 1846c27f..0d89b215 100644 --- a/unittest/rebootLogic/src/CoreController.h +++ b/unittest/rebootLogic/src/CoreController.h @@ -76,4 +76,4 @@ class CoreController { SdCardManager* sdcMan = nullptr; RebootFile rebootFile = {}; bool doPerformRebootFileHandling = true; -}; \ No newline at end of file +}; From d0c8c2a418104cff03084743b4b611a255023c0f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 11:12:12 +0100 Subject: [PATCH 104/119] revert some change --- unittest/rebootLogic/src/CoreController.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittest/rebootLogic/src/CoreController.h b/unittest/rebootLogic/src/CoreController.h index 0d89b215..1846c27f 100644 --- a/unittest/rebootLogic/src/CoreController.h +++ b/unittest/rebootLogic/src/CoreController.h @@ -76,4 +76,4 @@ class CoreController { SdCardManager* sdcMan = nullptr; RebootFile rebootFile = {}; bool doPerformRebootFileHandling = true; -}; +}; \ No newline at end of file From 37988fbfd454f9df9967ad86cee8c54a9bec7f52 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 11:17:08 +0100 Subject: [PATCH 105/119] complete impl --- bsp_q7s/core/CoreController.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 50c662d7..d892bc74 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -180,8 +180,20 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ const uint8_t *data, size_t size) { switch (actionId) { case (ANNOUNCE_VERSION): { - // uint32_t p1= - triggerEvent(VERSION_INFO); + uint32_t p1 = (common::OBSW_VERSION_MAJOR << 24) | (common::OBSW_VERSION_MINOR << 16) | + (common::OBSW_VERSION_REVISION << 8); + uint32_t p2 = 0; + if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) { + p1 |= 1; + // Only copy first 4 letters of git hash + memcpy(&p2, common::OBSW_VERSION_CST_GIT_SHA1 + 3, 4); + } + + triggerEvent(VERSION_INFO, p1, p2); + return HasActionsIF::EXECUTION_FINISHED; + } + case (ANNOUNCE_CURRENT_IMAGE): { + triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY); return HasActionsIF::EXECUTION_FINISHED; } case (LIST_DIRECTORY_INTO_FILE): { From b7ec4e3b2043feebba3bb5f403443ff5727690df Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 11:21:39 +0100 Subject: [PATCH 106/119] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index f16e27b7..ef42b1a1 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f16e27b79b9996f5a6c246cea560b783eb566d7e +Subproject commit ef42b1a179c8cede6092ba328cc7baa92916c26f From de95a1ba61148e3cb51925ad9419423cdc170708 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 11:22:57 +0100 Subject: [PATCH 107/119] add new events --- generators/bsp_q7s_events.csv | 2 ++ generators/events/translateEvents.cpp | 10 ++++++++-- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 10 ++++++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 6 files changed, 21 insertions(+), 7 deletions(-) diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 2e39b641..b6e2cc88 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -244,3 +244,5 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14002;0x36b2;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h 14003;0x36b3;REBOOT_HW;MEDIUM;;bsp_q7s/core/CoreController.h 14004;0x36b4;NO_SD_CARD_ACTIVE;HIGH;No SD card was active. Core controller will attempt to re-initialize a SD card.;bsp_q7s/core/CoreController.h +14005;0x36b5;VERSION_INFO;INFO;P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;bsp_q7s/core/CoreController.h +14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index e810f4d1..44dc3f24 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 245 translations. + * @brief Auto-generated event translation file. Contains 247 translations. * @details - * Generated on: 2023-02-03 10:52:53 + * Generated on: 2023-02-08 11:22:40 */ #include "translateEvents.h" @@ -245,6 +245,8 @@ const char *REBOOT_SW_STRING = "REBOOT_SW"; const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED"; const char *REBOOT_HW_STRING = "REBOOT_HW"; const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE"; +const char *VERSION_INFO_STRING = "VERSION_INFO"; +const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO"; const char *translateEvents(Event event) { switch ((event & 0xFFFF)) { @@ -728,6 +730,10 @@ const char *translateEvents(Event event) { return REBOOT_HW_STRING; case (14004): return NO_SD_CARD_ACTIVE_STRING; + case (14005): + return VERSION_INFO_STRING; + case (14006): + return CURRENT_IMAGE_INFO_STRING; default: return "UNKNOWN_EVENT"; } diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 9707189d..873bc547 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 152 translations. - * Generated on: 2023-02-03 10:52:53 + * Generated on: 2023-02-08 11:22:40 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index e810f4d1..44dc3f24 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 245 translations. + * @brief Auto-generated event translation file. Contains 247 translations. * @details - * Generated on: 2023-02-03 10:52:53 + * Generated on: 2023-02-08 11:22:40 */ #include "translateEvents.h" @@ -245,6 +245,8 @@ const char *REBOOT_SW_STRING = "REBOOT_SW"; const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED"; const char *REBOOT_HW_STRING = "REBOOT_HW"; const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE"; +const char *VERSION_INFO_STRING = "VERSION_INFO"; +const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO"; const char *translateEvents(Event event) { switch ((event & 0xFFFF)) { @@ -728,6 +730,10 @@ const char *translateEvents(Event event) { return REBOOT_HW_STRING; case (14004): return NO_SD_CARD_ACTIVE_STRING; + case (14005): + return VERSION_INFO_STRING; + case (14006): + return CURRENT_IMAGE_INFO_STRING; default: return "UNKNOWN_EVENT"; } diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 9707189d..873bc547 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 152 translations. - * Generated on: 2023-02-03 10:52:53 + * Generated on: 2023-02-08 11:22:40 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index ef42b1a1..dedfe1c9 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit ef42b1a179c8cede6092ba328cc7baa92916c26f +Subproject commit dedfe1c977fbe47a2d6a50e56e01eed9fe48b869 From 02f03495259c09e1ab70e402bfe852c9852643f7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 11:53:20 +0100 Subject: [PATCH 108/119] done --- bsp_q7s/core/CoreController.cpp | 5 ++++- tmtc | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index d892bc74..116b4fc9 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -185,8 +185,11 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_ uint32_t p2 = 0; if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) { p1 |= 1; + auto shaAsStr = std::string(common::OBSW_VERSION_CST_GIT_SHA1); + size_t posDash = shaAsStr.find("-"); + auto gitHash = shaAsStr.substr(posDash + 2, 4); // Only copy first 4 letters of git hash - memcpy(&p2, common::OBSW_VERSION_CST_GIT_SHA1 + 3, 4); + memcpy(&p2, gitHash.c_str(), 4); } triggerEvent(VERSION_INFO, p1, p2); diff --git a/tmtc b/tmtc index dedfe1c9..4086e794 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit dedfe1c977fbe47a2d6a50e56e01eed9fe48b869 +Subproject commit 4086e7947b4458bcbbd47c78e67c23d3a47885c8 From e25eed76877b14afe9ddb43dc7b051c300d3f2fa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 13:17:47 +0100 Subject: [PATCH 109/119] now it compiles --- mission/system/objects/AcsSubsystem.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index 6f7bbf7c..800c4344 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -58,7 +58,7 @@ void AcsSubsystem::handleEventMessages() { case EventMessage::EVENT_MESSAGE: if (event.getEvent() == acs::SAFE_RATE_VIOLATION) { CommandMessage msg; - ModeMessage::setCmdModeMessage(msg, acs::CtrlSubmode::DETUMBLE, 0); + ModeMessage::setCmdModeMessage(msg, acs::AcsModes::DETUMBLE, 0); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); if (result != returnvalue::OK) { sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl; @@ -66,7 +66,7 @@ void AcsSubsystem::handleEventMessages() { } if (event.getEvent() == acs::SAFE_RATE_RECOVERY) { CommandMessage msg; - ModeMessage::setCmdModeMessage(msg, acs::CtrlSubmode::SAFE, 0); + ModeMessage::setCmdModeMessage(msg, acs::AcsModes::SAFE, 0); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); if (result != returnvalue::OK) { sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl; From bfa61632c2043478b8273fb14e9149428fd9352f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 13:18:45 +0100 Subject: [PATCH 110/119] use singular enum name --- mission/acsDefs.h | 2 +- mission/controller/AcsController.cpp | 2 +- mission/system/objects/AcsSubsystem.cpp | 4 +- mission/system/tree/acsModeTree.cpp | 88 ++++++++++++------------- 4 files changed, 48 insertions(+), 48 deletions(-) diff --git a/mission/acsDefs.h b/mission/acsDefs.h index ca4ab77c..170305bb 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -7,7 +7,7 @@ namespace acs { // These modes are the submodes of the ACS controller and the modes of the ACS subsystem. -enum AcsModes { +enum AcsMode { OFF = HasModesIF::MODE_OFF, SAFE = 10, DETUMBLE = 11, diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 50f22483..14069b0a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -426,7 +426,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode < acs::AcsModes::SAFE) or (submode > acs::AcsModes::PTG_TARGET_INERTIAL)) { + if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_TARGET_INERTIAL)) { return INVALID_SUBMODE; } else { return returnvalue::OK; diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp index 800c4344..dcf4e67b 100644 --- a/mission/system/objects/AcsSubsystem.cpp +++ b/mission/system/objects/AcsSubsystem.cpp @@ -58,7 +58,7 @@ void AcsSubsystem::handleEventMessages() { case EventMessage::EVENT_MESSAGE: if (event.getEvent() == acs::SAFE_RATE_VIOLATION) { CommandMessage msg; - ModeMessage::setCmdModeMessage(msg, acs::AcsModes::DETUMBLE, 0); + ModeMessage::setCmdModeMessage(msg, acs::AcsMode::DETUMBLE, 0); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); if (result != returnvalue::OK) { sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl; @@ -66,7 +66,7 @@ void AcsSubsystem::handleEventMessages() { } if (event.getEvent() == acs::SAFE_RATE_RECOVERY) { CommandMessage msg; - ModeMessage::setCmdModeMessage(msg, acs::AcsModes::SAFE, 0); + ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0); ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg); if (result != returnvalue::OK) { sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl; diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index db7d7a81..fe87dff5 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -30,69 +30,69 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper); static const auto OFF = HasModesIF::MODE_OFF; static const auto NML = DeviceHandlerIF::MODE_NORMAL; -auto ACS_SEQUENCE_OFF = std::make_pair(acs::AcsModes::OFF, FixedArrayList()); +auto ACS_SEQUENCE_OFF = std::make_pair(acs::AcsMode::OFF, FixedArrayList()); auto ACS_TABLE_OFF_TGT = - std::make_pair((acs::AcsModes::OFF << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsMode::OFF << 24) | 1, FixedArrayList()); auto ACS_TABLE_OFF_TRANS_0 = - std::make_pair((acs::AcsModes::OFF << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsMode::OFF << 24) | 2, FixedArrayList()); auto ACS_TABLE_OFF_TRANS_1 = - std::make_pair((acs::AcsModes::OFF << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsMode::OFF << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_DETUMBLE = - std::make_pair(acs::AcsModes::DETUMBLE, FixedArrayList()); + std::make_pair(acs::AcsMode::DETUMBLE, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TGT = - std::make_pair((acs::AcsModes::DETUMBLE << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsMode::DETUMBLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TRANS_0 = - std::make_pair((acs::AcsModes::DETUMBLE << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsMode::DETUMBLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_DETUMBLE_TRANS_1 = - std::make_pair((acs::AcsModes::DETUMBLE << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsMode::DETUMBLE << 24) | 3, FixedArrayList()); -auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsModes::SAFE, FixedArrayList()); +auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsMode::SAFE, FixedArrayList()); auto ACS_TABLE_SAFE_TGT = - std::make_pair((acs::AcsModes::SAFE << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsMode::SAFE << 24) | 1, FixedArrayList()); auto ACS_TABLE_SAFE_TRANS_0 = - std::make_pair((acs::AcsModes::SAFE << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsMode::SAFE << 24) | 2, FixedArrayList()); auto ACS_TABLE_SAFE_TRANS_1 = - std::make_pair((acs::AcsModes::SAFE << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList()); -auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsModes::IDLE, FixedArrayList()); +auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::IDLE, FixedArrayList()); auto ACS_TABLE_IDLE_TGT = - std::make_pair((acs::AcsModes::IDLE << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsMode::IDLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_0 = - std::make_pair((acs::AcsModes::IDLE << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsMode::IDLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_1 = - std::make_pair((acs::AcsModes::IDLE << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsMode::IDLE << 24) | 3, FixedArrayList()); auto ACS_TABLE_PTG_TRANS_0 = - std::make_pair((acs::AcsModes::PTG_TARGET << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 2, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET = - std::make_pair(acs::AcsModes::PTG_TARGET, FixedArrayList()); + std::make_pair(acs::AcsMode::PTG_TARGET, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_TGT = - std::make_pair((acs::AcsModes::PTG_TARGET << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 1, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_TRANS_1 = - std::make_pair((acs::AcsModes::PTG_TARGET << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_GS = - std::make_pair(acs::AcsModes::PTG_TARGET_GS, FixedArrayList()); + std::make_pair(acs::AcsMode::PTG_TARGET_GS, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_GS_TGT = - std::make_pair((acs::AcsModes::PTG_TARGET_GS << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 1, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 = - std::make_pair((acs::AcsModes::PTG_TARGET_GS << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_NADIR = - std::make_pair(acs::AcsModes::PTG_TARGET_NADIR, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::AcsModes::PTG_TARGET_NADIR << 24) | 1, + std::make_pair(acs::AcsMode::PTG_TARGET_NADIR, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::AcsMode::PTG_TARGET_NADIR << 24) | 1, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair( - (acs::AcsModes::PTG_TARGET_NADIR << 24) | 3, FixedArrayList()); + (acs::AcsMode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_INERTIAL = - std::make_pair(acs::AcsModes::PTG_TARGET_INERTIAL, FixedArrayList()); + std::make_pair(acs::AcsMode::PTG_TARGET_INERTIAL, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair( - (acs::AcsModes::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList()); + (acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList()); auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair( - (acs::AcsModes::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList()); + (acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList()); void satsystem::acs::init() { ModeListEntry entry; @@ -122,7 +122,7 @@ void satsystem::acs::init() { buildTargetPtGsSequence(ACS_SUBSYSTEM, entry); buildTargetPtNadirSequence(ACS_SUBSYSTEM, entry); buildTargetPtInertialSequence(ACS_SUBSYSTEM, entry); - ACS_SUBSYSTEM.setInitialMode(::acs::AcsModes::SAFE); + ACS_SUBSYSTEM.setInitialMode(::acs::AcsMode::SAFE); } namespace { @@ -190,7 +190,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build SAFE target - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::SAFE, ACS_TABLE_SAFE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second); @@ -206,7 +206,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build SAFE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::SAFE, ACS_TABLE_SAFE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TRANS_1.second); check(ss.addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true), ctxc); @@ -239,7 +239,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build DETUMBLE target - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second); @@ -257,7 +257,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) { ctxc); // Build DETUMBLE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second); check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false, true), ctxc); @@ -291,7 +291,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build IDLE target - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::IDLE, ACS_TABLE_IDLE_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); @@ -307,7 +307,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); // Build IDLE transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::IDLE, ACS_TABLE_IDLE_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::IDLE, ACS_TABLE_IDLE_TRANS_1.second); ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true); // Build IDLE sequence @@ -339,7 +339,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second); @@ -350,7 +350,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second); check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false, true), @@ -386,7 +386,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second); @@ -399,7 +399,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_NADIR, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_NADIR, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), @@ -436,7 +436,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_GS, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second); @@ -449,7 +449,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_GS, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS, ACS_TABLE_PTG_TARGET_GS_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_GS_TRANS_1.first, &ACS_TABLE_PTG_TARGET_GS_TRANS_1.second)), @@ -485,7 +485,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_INERTIAL, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); @@ -498,7 +498,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsModes::PTG_TARGET_INERTIAL, + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)), From 6ae0ee5b26812f5aab4e42e5c86d18be5d8b20ea Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 13:23:05 +0100 Subject: [PATCH 111/119] small update --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 35491ccf..9ccce344 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,7 +19,7 @@ change warranting a new major release: ## Changed -- Readded calibration matrices for MGM calibration +- Readded calibration matrices for MGM calibration. ## Fixed @@ -29,7 +29,7 @@ change warranting a new major release: stored as the git SHA hash in `commonConfig.h`. This will be an empty string now for regular versions. - Bump FSFW for important fix in PUS mode service. -- Bugfixes in 'SensofProcessing' where previously MGM values would be calibrated before being +- Bugfixes in 'SensorProcessing' where previously MGM values would be calibrated before being transformed in body RF. However, the calibration values are in the body RF. Also fixed the validity flag of 'mgmVecTotDerivative'. From da166c7bc31b0688d80ff8eea8adeaaa97302aff Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 13:40:03 +0100 Subject: [PATCH 112/119] resolve merge conflicts --- linux/devices/GpsHyperionLinuxController.cpp | 2 +- mission/controller/AcsController.cpp | 8 +-- mission/system/tree/acsModeTree.cpp | 57 +++++++------------- 3 files changed, 23 insertions(+), 44 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index ec9cb61a..cb0523d9 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -157,7 +157,7 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { } }; // GPS is off, no point in reading data from GPSD. - if(mode == MODE_OFF) { + if (mode == MODE_OFF) { return false; } if (readMode == ReadModes::SOCKET) { diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index bcf8ba07..b590786b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -27,7 +27,7 @@ AcsController::AcsController(object_id_t objectId) ctrlValData(this), actuatorCmdData(this) {} -ReturnValue_t AcsController::handleCommandMessage(CommandMessage* message) { +ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t result = actionHelper.handleActionMessage(message); if (result == returnvalue::OK) { return result; @@ -42,8 +42,8 @@ ReturnValue_t AcsController::handleCommandMessage(CommandMessage* message) { MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); } ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId, - ParameterWrapper* parameterWrapper, - const ParameterWrapper* newValues, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex) { return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues, startAtIndex); @@ -563,7 +563,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_TARGET_INERTIAL)) { + if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_INERTIAL)) { return INVALID_SUBMODE; } else { return returnvalue::OK; diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index 704b39e1..0474d0a1 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -55,13 +55,13 @@ auto ACS_TABLE_SAFE_TRANS_0 = auto ACS_TABLE_SAFE_TRANS_1 = std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList()); -auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::IDLE, FixedArrayList()); +auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::PTG_IDLE, FixedArrayList()); auto ACS_TABLE_IDLE_TGT = - std::make_pair((acs::AcsMode::IDLE << 24) | 1, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 1, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_0 = - std::make_pair((acs::AcsMode::IDLE << 24) | 2, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 2, FixedArrayList()); auto ACS_TABLE_IDLE_TRANS_1 = - std::make_pair((acs::AcsMode::IDLE << 24) | 3, FixedArrayList()); + std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 3, FixedArrayList()); auto ACS_TABLE_PTG_TRANS_0 = std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 2, FixedArrayList()); @@ -82,17 +82,17 @@ auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 = auto ACS_SEQUENCE_PTG_TARGET_NADIR = std::make_pair(acs::AcsMode::PTG_NADIR, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::AcsMode::PTG_TARGET_NADIR << 24) | 1, - FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair( - (acs::AcsMode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TGT = + std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 1, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = + std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 3, FixedArrayList()); auto ACS_SEQUENCE_PTG_TARGET_INERTIAL = std::make_pair(acs::AcsMode::PTG_INERTIAL, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair( - (acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList()); -auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair( - (acs::AcsMode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = + std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 1, FixedArrayList()); +auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = + std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 3, FixedArrayList()); void satsystem::acs::init() { ModeListEntry entry; @@ -291,11 +291,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { check(sequence.insert(eh), ctxc); }; // Build IDLE target -<<<<<<< HEAD - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second); -======= - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::IDLE, ACS_TABLE_IDLE_TGT.second); ->>>>>>> origin/develop + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second); @@ -311,11 +307,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) { ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true); // Build IDLE transition 1 -<<<<<<< HEAD - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second); -======= - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::IDLE, ACS_TABLE_IDLE_TRANS_1.second); ->>>>>>> origin/develop + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second); ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true); // Build IDLE sequence @@ -358,8 +350,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, - ACS_TABLE_PTG_TARGET_TRANS_1.second); + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second); check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false, true), ctxc); @@ -407,11 +398,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 -<<<<<<< HEAD - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_NADIR, -======= - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_NADIR, ->>>>>>> origin/develop + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_NADIR, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)), @@ -497,11 +484,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { }; // Build TARGET PT table -<<<<<<< HEAD - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_INERTIAL, -======= - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_INERTIAL, ->>>>>>> origin/develop + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second); @@ -514,11 +497,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) { // Transition 0 already built // Build TARGET PT transition 1 -<<<<<<< HEAD - iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_INERTIAL, -======= - iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_INERTIAL, ->>>>>>> origin/develop + iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL, ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second); check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)), From 349ebca08d462043ad32d7071442fa9f64b79a43 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 14:11:11 +0100 Subject: [PATCH 113/119] re-run generators all associated changes and fixes --- common/config/eive/resultClassIds.h | 5 +++ generators/bsp_q7s_events.csv | 1 + generators/bsp_q7s_returnvalues.csv | 31 ++++++++++--------- generators/events/event_parser.py | 5 +-- generators/events/translateEvents.cpp | 7 +++-- generators/objects/translateObjects.cpp | 2 +- .../returnvalues/returnvalues_parser.py | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 7 +++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- .../acs/MultiplicativeKalmanFilter.h | 6 ++-- mission/controller/acs/SensorProcessing.h | 4 +-- mission/controller/acs/config/classIds.h | 18 ----------- mission/controller/acs/control/Detumble.h | 11 ++----- mission/controller/acs/control/PtgCtrl.h | 4 +-- mission/controller/acs/control/SafeCtrl.h | 11 ++----- .../devicedefinitions/GPSDefinitions.h | 2 +- tmtc | 2 +- 17 files changed, 52 insertions(+), 68 deletions(-) delete mode 100644 mission/controller/acs/config/classIds.h diff --git a/common/config/eive/resultClassIds.h b/common/config/eive/resultClassIds.h index aeb05815..e4dfb927 100644 --- a/common/config/eive/resultClassIds.h +++ b/common/config/eive/resultClassIds.h @@ -35,6 +35,11 @@ enum commonClassIds : uint8_t { SA_DEPL_HANDLER, // SADPL MPSOC_RETURN_VALUES_IF, // MPSOCRTVIF SUPV_RETURN_VALUES_IF, // SPVRTVIF + ACS_KALMAN, // ACSKAL + ACS_SAFE, // ACSSAF + ACS_PTG, // ACSPTG + ACS_DETUMBLE, // ACSDTB + ACS_MEKF, // ACSMEK COMMON_CLASS_ID_END // [EXPORT] : [END] }; diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index b6e2cc88..9f477ca2 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -198,6 +198,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12903;0x3267;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/objects/SusAssembly.h 13000;0x32c8;CHILDREN_LOST_MODE;MEDIUM;;mission/system/objects/TcsBoardAssembly.h 13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: Old fix. P2: New fix 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;mission/devices/devicedefinitions/GPSDefinitions.h +13101;0x332d;CANT_GET_FIX;LOW;Could not get fix in maximum allowed time. P1: Maximum allowed time to get a fix after the GPS was switched on.;mission/devices/devicedefinitions/GPSDefinitions.h 13200;0x3390;P60_BOOT_COUNT;INFO;P60 boot count is broadcasted once at SW startup. P1: Boot count;mission/devices/P60DockHandler.h 13201;0x3391;BATT_MODE;INFO;Battery mode is broadcasted at startup. P1: Mode;mission/devices/P60DockHandler.h 13202;0x3392;BATT_MODE_CHANGED;MEDIUM;Battery mode has changed. P1: Old mode. P2: New mode;mission/devices/P60DockHandler.h diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 82f44173..0581c352 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -51,9 +51,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x4fa4;HEATER_MainSwitchSetTimeout;;164;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa5;HEATER_CommandAlreadyWaiting;;165;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h -0x6001;CCSDS_KalmanNoGyrMeas;;1;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6002;CCSDS_KalmanNoModel;;2;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6003;CCSDS_KalmanInversionFailed;;3;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6a01;ACSSAF_SafectrlMekfInputInvalid;;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h +0x6b01;ACSPTG_PtgctrlMekfInputInvalid;;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h +0x6c01;ACSDTB_DetumbleNoSensordata;;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h +0x6901;ACSKAL_KalmanNoGyrMeas;;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6902;ACSKAL_KalmanNoModel;;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h +0x6903;ACSKAL_KalmanInversionFailed;;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h 0x4500;HSPI_OpeningFileFailed;;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4501;HSPI_FullDuplexTransferFailed;;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4502;HSPI_HalfDuplexTransferFailed;;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h @@ -468,16 +471,16 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x1d03;ATC_IllegalApplicationData;;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h 0x1d04;ATC_SendTmFailed;;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h 0x1d05;ATC_Timeout;;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x6b00;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h +0x7000;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h 0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h -0x6a00;SDMA_OpOngoing;;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a01;SDMA_AlreadyOn;;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a02;SDMA_AlreadyMounted;;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a03;SDMA_AlreadyOff;;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a0a;SDMA_StatusFileNexists;;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a0b;SDMA_StatusFileFormatInvalid;;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a0c;SDMA_MountError;;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a0d;SDMA_UnmountError;;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a0e;SDMA_SystemCallError;;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6a0f;SDMA_PopenCallError;;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f00;SDMA_OpOngoing;;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f01;SDMA_AlreadyOn;;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f02;SDMA_AlreadyMounted;;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f03;SDMA_AlreadyOff;;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f0a;SDMA_StatusFileNexists;;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f0b;SDMA_StatusFileFormatInvalid;;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f0c;SDMA_MountError;;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f0d;SDMA_UnmountError;;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f0e;SDMA_SystemCallError;;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6f0f;SDMA_PopenCallError;;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h diff --git a/generators/events/event_parser.py b/generators/events/event_parser.py index 8c5ab35b..8dd4952b 100644 --- a/generators/events/event_parser.py +++ b/generators/events/event_parser.py @@ -29,6 +29,7 @@ GENERATE_CSV = True COPY_CPP_FILE = True COPY_CPP_H_FILE = True MOVE_CSV_FILE = True +PRINT_EVENTS = False PARSE_HOST_BSP = True @@ -80,13 +81,13 @@ LOGGER = get_console_logger() def parse_events( - generate_csv: bool = True, generate_cpp: bool = True, print_events: bool = True + generate_csv: bool = True, generate_cpp: bool = True ): LOGGER.info("EventParser: Parsing events: ") # Small delay for clean printout time.sleep(0.01) event_list = generate_event_list() - if print_events: + if PRINT_EVENTS: PrettyPrinter.pprint(event_list) # Delay for clean printout time.sleep(0.1) diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 44dc3f24..a13005e1 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 247 translations. + * @brief Auto-generated event translation file. Contains 248 translations. * @details - * Generated on: 2023-02-08 11:22:40 + * Generated on: 2023-02-08 14:09:40 */ #include "translateEvents.h" @@ -200,6 +200,7 @@ const char *POWER_STATE_MACHINE_TIMEOUT_STRING = "POWER_STATE_MACHINE_TIMEOUT"; const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED"; const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE"; const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE"; +const char *CANT_GET_FIX_STRING = "CANT_GET_FIX"; const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT"; const char *BATT_MODE_STRING = "BATT_MODE"; const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED"; @@ -640,6 +641,8 @@ const char *translateEvents(Event event) { return CHILDREN_LOST_MODE_STRING; case (13100): return GPS_FIX_CHANGE_STRING; + case (13101): + return CANT_GET_FIX_STRING; case (13200): return P60_BOOT_COUNT_STRING; case (13201): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 873bc547..0e1c12dc 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 152 translations. - * Generated on: 2023-02-08 11:22:40 + * Generated on: 2023-02-08 14:09:40 */ #include "translateObjects.h" diff --git a/generators/returnvalues/returnvalues_parser.py b/generators/returnvalues/returnvalues_parser.py index 4f6f30e1..6ad893d1 100644 --- a/generators/returnvalues/returnvalues_parser.py +++ b/generators/returnvalues/returnvalues_parser.py @@ -22,7 +22,7 @@ LOGGER = get_console_logger() EXPORT_TO_FILE = True COPY_CSV_FILE = True EXPORT_TO_SQL = True -PRINT_TABLES = True +PRINT_TABLES = False FILE_SEPARATOR = ";" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 44dc3f24..a13005e1 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 247 translations. + * @brief Auto-generated event translation file. Contains 248 translations. * @details - * Generated on: 2023-02-08 11:22:40 + * Generated on: 2023-02-08 14:09:40 */ #include "translateEvents.h" @@ -200,6 +200,7 @@ const char *POWER_STATE_MACHINE_TIMEOUT_STRING = "POWER_STATE_MACHINE_TIMEOUT"; const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED"; const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE"; const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE"; +const char *CANT_GET_FIX_STRING = "CANT_GET_FIX"; const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT"; const char *BATT_MODE_STRING = "BATT_MODE"; const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED"; @@ -640,6 +641,8 @@ const char *translateEvents(Event event) { return CHILDREN_LOST_MODE_STRING; case (13100): return GPS_FIX_CHANGE_STRING; + case (13101): + return CANT_GET_FIX_STRING; case (13200): return P60_BOOT_COUNT_STRING; case (13201): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 873bc547..0e1c12dc 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 152 translations. - * Generated on: 2023-02-08 11:22:40 + * Generated on: 2023-02-08 14:09:40 */ #include "translateObjects.h" diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 79d39510..77bb2d9d 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -20,7 +20,7 @@ #include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" -#include "config/classIds.h" +#include "eive/resultClassIds.h" class MultiplicativeKalmanFilter { public: @@ -64,11 +64,11 @@ class MultiplicativeKalmanFilter { const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); // Declaration of Events (like init) and memberships - // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND + static const uint8_t INTERFACE_ID = CLASS_ID::ACS_MEKF; // CLASS IDS ND // (/config/returnvalues/classIDs.h) static const Event RESET = // MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be // resetting Mekf - static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN; + static const uint8_t INTERFACE_ID = CLASS_ID::ACS_KALMAN; static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01); static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03); diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 49c659b0..6fa1ab8c 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -1,5 +1,5 @@ /******************************* - * EIVE Flight Software Framework (FSFW) + * EIVE Flight Software * (c) 2022 IRS, Uni Stuttgart *******************************/ #ifndef SENSORPROCESSING_H_ @@ -13,7 +13,7 @@ #include "AcsParameters.h" #include "SensorValues.h" #include "SusConverter.h" -#include "config/classIds.h" +#include "eive/resultClassIds.h" class SensorProcessing { public: diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h deleted file mode 100644 index ccf6b616..00000000 --- a/mission/controller/acs/config/classIds.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef ACS_CONFIG_CLASSIDS_H_ -#define ACS_CONFIG_CLASSIDS_H_ - -#include -#include - -namespace CLASS_ID { -enum eiveclassIds : uint8_t { - EIVE_CLASS_ID_START = COMMON_CLASS_ID_END, - KALMAN, - SAFE, - PTG, - DETUMBLE, - EIVE_CLASS_ID_END // [EXPORT] : [END] -}; -} - -#endif /* ACS_CONFIG_CLASSIDS_H_ */ diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 6a3e6eaa..65e5ec28 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -1,10 +1,3 @@ -/* - * Detumble.h - * - * Created on: 17 Aug 2022 - * Author: Robin Marquardt - */ - #ifndef ACS_CONTROL_DETUMBLE_H_ #define ACS_CONTROL_DETUMBLE_H_ @@ -15,14 +8,14 @@ #include "../AcsParameters.h" #include "../SensorValues.h" -#include "../config/classIds.h" +#include "eive/resultClassIds.h" class Detumble { public: Detumble(AcsParameters *acsParameters_); virtual ~Detumble(); - static const uint8_t INTERFACE_ID = CLASS_ID::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 diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 559216f7..5e9a704c 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -20,7 +20,7 @@ #include "../AcsParameters.h" #include "../SensorValues.h" -#include "../config/classIds.h" +#include "eive/resultClassIds.h" class PtgCtrl { public: @@ -30,7 +30,7 @@ class PtgCtrl { PtgCtrl(AcsParameters *acsParameters_); virtual ~PtgCtrl(); - static const uint8_t INTERFACE_ID = CLASS_ID::PTG; + 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 diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 72e45f08..1784f9ca 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -1,10 +1,3 @@ -/* - * safeCtrl.h - * - * Created on: 19 Apr 2022 - * Author: rooob - */ - #ifndef SAFECTRL_H_ #define SAFECTRL_H_ @@ -14,14 +7,14 @@ #include "../AcsParameters.h" #include "../SensorValues.h" -#include "../config/classIds.h" +#include "eive/resultClassIds.h" class SafeCtrl { public: SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - static const uint8_t INTERFACE_ID = CLASS_ID::SAFE; + 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_); diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index 2ac7781d..8acb77e9 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -15,7 +15,7 @@ static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER; static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO); //! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. P1: Maximum allowed time //! to get a fix after the GPS was switched on. -static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW); +static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW); static constexpr DeviceCommandId_t GPS_REPLY = 0; static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5; diff --git a/tmtc b/tmtc index 4086e794..9955295d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 4086e7947b4458bcbbd47c78e67c23d3a47885c8 +Subproject commit 9955295dfe16a9e0da7dee273c0abd502307eb64 From 7e0b30e8457525e7d16ebe929394f486844a7c46 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 14:11:49 +0100 Subject: [PATCH 114/119] ACS section separate --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 46c8c554..57920507 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,8 @@ change warranting a new major release: ## Changed +### ACS + - Readded calibration matrices for MGM calibration. - Added calculation of satellite velocity vector from GPS position data - Added detumble mode using GYR values @@ -46,6 +48,9 @@ change warranting a new major release: stored as the git SHA hash in `commonConfig.h`. This will be an empty string now for regular versions. - Bump FSFW for important fix in PUS mode service. + +### ACS + - Bugfixes in 'SensorProcessing' where previously MGM values would be calibrated before being transformed in body RF. However, the calibration values are in the body RF. Also fixed the validity flag of 'mgmVecTotDerivative'. From 50d8d0b278473870e8fa184097dba455e17f8382 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 14:22:56 +0100 Subject: [PATCH 115/119] remove old unneeded IF ID --- mission/controller/acs/MultiplicativeKalmanFilter.h | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 77bb2d9d..a2f81272 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -63,15 +63,11 @@ class MultiplicativeKalmanFilter { const double *sunDirJ, const bool validSSModel, const double *magFieldJ, const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData); - // Declaration of Events (like init) and memberships - static const uint8_t INTERFACE_ID = CLASS_ID::ACS_MEKF; // CLASS IDS ND - // (/config/returnvalues/classIDs.h) static const Event RESET = - // MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be // resetting Mekf - static const uint8_t INTERFACE_ID = CLASS_ID::ACS_KALMAN; - static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01); - static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); - static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03); + static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN; + static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1); + static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2); + static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3); private: /*Parameters*/ From b1da4bb81b18ed680037abc0dcf2dbabfe77fd99 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 14:33:48 +0100 Subject: [PATCH 116/119] remove doubly scheduled ACS CTRL --- bsp_q7s/core/scheduling.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index 42a11875..79390b92 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -184,15 +184,8 @@ void scheduling::initTasks() { #endif /* OBSW_ADD_GPS_CTRL */ PeriodicTaskIF* acsSysTask = factory->createPeriodicTask( - "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); + "SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); static_cast(acsSysTask); - // To be removed soon because it will be part of the ACS PST. -#if OBSW_ADD_ACS_CTRL == 1 - acsSysTask->addComponent(objects::ACS_CONTROLLER); - if (result != returnvalue::OK) { - scheduling::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); - } -#endif #if OBSW_ADD_ACS_BOARD == 1 result = acsSysTask->addComponent(objects::ACS_BOARD_ASS); if (result != returnvalue::OK) { From 881f9ea7ea7a03957ebb6fd1ebc06b1274f1d9a6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 14:52:11 +0100 Subject: [PATCH 117/119] prep v1.26.0 --- CHANGELOG.md | 4 ++++ CMakeLists.txt | 2 +- tmtc | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 57920507..dd207914 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,10 @@ change warranting a new major release: # [unreleased] +# [v1.26.0] 2023-02-08 + +eive-tmtc v2.12.1 + ## Changed ### ACS diff --git a/CMakeLists.txt b/CMakeLists.txt index f5220063..8672db5e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 1) -set(OBSW_VERSION_MINOR 25) +set(OBSW_VERSION_MINOR 26) set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) diff --git a/tmtc b/tmtc index 9955295d..d0c8e20d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 9955295dfe16a9e0da7dee273c0abd502307eb64 +Subproject commit d0c8e20d4f9a6f5aee3ccfa05c4f7ab7151400b5 From 4528e61fcdbe0423052039059c9498f6de7ae5c9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 15:02:56 +0100 Subject: [PATCH 118/119] initialize ACS controller --- CHANGELOG.md | 4 ++++ mission/controller/AcsController.cpp | 8 ++++++++ mission/controller/AcsController.h | 1 + 3 files changed, 13 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dd207914..c545da36 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,10 @@ change warranting a new major release: # [unreleased] +# [v1.26.1] 2023-02-08 + +- Initialize parameter helper in ACS controller. + # [v1.26.0] 2023-02-08 eive-tmtc v2.12.1 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b590786b..83088b41 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -721,6 +721,14 @@ void AcsController::copySusData() { } } +ReturnValue_t AcsController::initialize() { + ReturnValue_t result = parameterHelper.initialize(); + if (result != returnvalue::OK) { + return result; + } + return ExtendedControllerBase::initialize(); +} + void AcsController::copyGyrData() { ACS::SensorValues sensorValues; { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 6a18e988..e1e31a2a 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -55,6 +55,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes InternalState internalState = InternalState::STARTUP; + ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; From e1e6ce5d3442c1a30b0b1f4e2912f0ccce565175 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 15:03:22 +0100 Subject: [PATCH 119/119] bump patch version, prep v1.26.1 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8672db5e..9820c3ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 1) set(OBSW_VERSION_MINOR 26) -set(OBSW_VERSION_REVISION 0) +set(OBSW_VERSION_REVISION 1) # set(CMAKE_VERBOSE TRUE)