From ffc7a5576332b008015887291fb51eed72494eda Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 21 Oct 2022 14:23:31 +0200 Subject: [PATCH 01/66] 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; }; -- 2.43.0 From e87221a8a3efb90e0b840f2fabcf81d622e121cb Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 21 Oct 2022 16:46:09 +0200 Subject: [PATCH 02/66] 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; -- 2.43.0 From ba541300caf65b9abe29ffd7b12153f7698f7169 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 28 Oct 2022 18:18:28 +0200 Subject: [PATCH 03/66] 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) -- 2.43.0 From 75ab11fc35d44c394dd1dc4d93555ace44338f66 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 4 Nov 2022 17:21:17 +0100 Subject: [PATCH 04/66] 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 -- 2.43.0 From 20936faec6f7e2465b833dbc6225d32cf5940093 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Tue, 8 Nov 2022 13:48:50 +0100 Subject: [PATCH 05/66] 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_ */ -- 2.43.0 From 55dec574c5bd23825fbbd1f12dbb6d01c62c86c5 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Mon, 14 Nov 2022 17:16:47 +0100 Subject: [PATCH 06/66] 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: -- 2.43.0 From 609d429161129e18581a62575327efc92b8dd482 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Tue, 22 Nov 2022 21:10:05 +0100 Subject: [PATCH 07/66] 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_ */ -- 2.43.0 From 8d1cbd9f8b64f00c9e40571efffb5eda3753dc72 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 24 Nov 2022 13:40:55 +0100 Subject: [PATCH 08/66] 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 -- 2.43.0 From 2b445369fd0a66fa5f37ac7b236630817d4b9015 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Mon, 5 Dec 2022 21:31:52 +0100 Subject: [PATCH 09/66] 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}}; -- 2.43.0 From 72b503bb8f5352dcc2065e0ca04787708c332135 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 6 Dec 2022 09:21:43 +0100 Subject: [PATCH 10/66] 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) + -- 2.43.0 From fac2fc4971b3dfbea6d9bdc2724ce255597e73dc Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 8 Dec 2022 09:52:42 +0100 Subject: [PATCH 11/66] - 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]; -- 2.43.0 From e9dd0f8f6d943f477f6624b0fd9b32cc1ce06705 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 11:51:56 +0100 Subject: [PATCH 12/66] 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]; -- 2.43.0 From f35c42aeadab0b5119f5fe03e25a7b8f6dc1b382 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 11:52:46 +0100 Subject: [PATCH 13/66] 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 -- 2.43.0 From 5fe3ac09a5b312f8619f768b510749e38c6b86f6 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 14:30:42 +0100 Subject: [PATCH 14/66] 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]; -- 2.43.0 From 46945a8674d3c270b61f2d6cf6dcf12302058a1c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 14:45:20 +0100 Subject: [PATCH 15/66] 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}}, -- 2.43.0 From c6e8c40b2c2f4d5176baff1909ea358b04042f0f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 12 Dec 2022 14:50:27 +0100 Subject: [PATCH 16/66] 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); -- 2.43.0 From b49d37e15a2ed22808ec8b8b8672bc2e9a1c11a3 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 13 Dec 2022 11:51:03 +0100 Subject: [PATCH 17/66] 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 18/66] 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 -- 2.43.0 From d5677c20f7a7c437af84932afbaf4c2ba3f589b7 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 10:04:37 +0100 Subject: [PATCH 19/66] 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; -- 2.43.0 From 12a367f65f61ea1bc2374a93e9b699ac9e89061f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 10:13:28 +0100 Subject: [PATCH 20/66] 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_); -- 2.43.0 From b6c5796121b61881dc473ac90ad70bd492860189 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 10:31:20 +0100 Subject: [PATCH 21/66] 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; -- 2.43.0 From c97d319b2a2619361dbb1c4c071d0a755b87af57 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 10:39:41 +0100 Subject: [PATCH 22/66] 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: }; -- 2.43.0 From 8b415883e9e0c35d3fa9d4ce8b43366096a1526e Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 11:46:58 +0100 Subject: [PATCH 23/66] 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]); -- 2.43.0 From 3f2910c3a72632e939056587843ff447b2f1111d Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 14 Dec 2022 11:50:29 +0100 Subject: [PATCH 24/66] 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; -- 2.43.0 From b3a2cc43670757662861e6af9b9e33d084b386c6 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 16 Dec 2022 13:59:49 +0100 Subject: [PATCH 25/66] 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); -- 2.43.0 From e4936b1bedb3c0a08ceed13911cb0e8daa57c55c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Dec 2022 14:23:42 +0100 Subject: [PATCH 26/66] 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}; -- 2.43.0 From c338c4fb58c494596cfd4c2ff546906541a39125 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Dec 2022 14:24:03 +0100 Subject: [PATCH 27/66] 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; } -- 2.43.0 From d9d75590972c777ad5c711c1d29d8628613cd26a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Dec 2022 14:34:20 +0100 Subject: [PATCH 28/66] 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( -- 2.43.0 From bd52da8afdefa59724759f8293bf2c51dde91a43 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Dec 2022 09:59:42 +0100 Subject: [PATCH 29/66] 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; -- 2.43.0 From 341966e70920b57a7f24accd153eae3451450ac4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 9 Jan 2023 09:36:00 +0100 Subject: [PATCH 30/66] 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 = -- 2.43.0 From 10f1898b98b3118b04abb8b223774a7063fde5c9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 9 Jan 2023 09:36:30 +0100 Subject: [PATCH 31/66] 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_ */ -- 2.43.0 From 4e74dc7beee935e6e21ab717e90dd2d3d8a09b35 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 09:05:27 +0100 Subject: [PATCH 32/66] 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, -- 2.43.0 From 887d193526c5ef7c46cf8996dfae8bd44a9d6ebf Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 09:06:09 +0100 Subject: [PATCH 33/66] 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); -- 2.43.0 From 0854b1c77874844b831b451d738565335ac57fe8 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 09:16:13 +0100 Subject: [PATCH 34/66] 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() { -- 2.43.0 From 93ec49bf8deaaaa073dea28661cc1d49d95399c1 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 11:20:28 +0100 Subject: [PATCH 35/66] 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); -- 2.43.0 From c79e17514c3f695750cd1d21d50d80b08905c1d3 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 13:51:55 +0100 Subject: [PATCH 36/66] 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; -- 2.43.0 From ac83e660169e7939536312cadf3ba6eff650d351 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 10 Jan 2023 13:52:26 +0100 Subject: [PATCH 37/66] 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; -- 2.43.0 From a2626afebb1a5870cd25342811bd2777c889eba2 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 11 Jan 2023 13:42:48 +0100 Subject: [PATCH 38/66] 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: -- 2.43.0 From 25867e76b196fa4cddf3a00773df34323e6175e9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 11 Jan 2023 16:00:30 +0100 Subject: [PATCH 39/66] 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); -- 2.43.0 From 66e2d782b21505450954bcd07326bdf4722c7d85 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 12 Jan 2023 11:47:56 +0100 Subject: [PATCH 40/66] 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; -- 2.43.0 From 27d6e95062c23dc5c09c59b9dadcce6c5b57a1e4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 12 Jan 2023 11:48:57 +0100 Subject: [PATCH 41/66] 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 -- 2.43.0 From 4090941c3ea723cdc8949e3f2bc12881507712bb Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 12 Jan 2023 13:33:06 +0100 Subject: [PATCH 42/66] 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; } // -- 2.43.0 From 5e557d2d46b3ffdc37c788916cbc2f77ec3c25b4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 12 Jan 2023 14:49:37 +0100 Subject: [PATCH 43/66] 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_ */ -- 2.43.0 From d8c0ba19fd051380286cde94140c2c23a9b91097 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 12 Jan 2023 15:19:21 +0100 Subject: [PATCH 44/66] 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; -- 2.43.0 From 2f0eace82279d6b73166ea37aa7fc41d39554dda Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 12 Jan 2023 17:10:40 +0100 Subject: [PATCH 45/66] 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], -- 2.43.0 From ce3841a23d9c0011a92ef7e5552b261b15149fa2 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Thu, 12 Jan 2023 17:19:35 +0100 Subject: [PATCH 46/66] 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; } -- 2.43.0 From db763e394dd90e42a53ffb28a281b538981f96ec Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 09:54:03 +0100 Subject: [PATCH 47/66] 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); -- 2.43.0 From 153c62337fca414e1075ae1d297534ccfaf31a88 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 10:07:14 +0100 Subject: [PATCH 48/66] 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]}}; -- 2.43.0 From 9791c11c993bd663c437b97ec274b0689dcfca93 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 10:25:55 +0100 Subject: [PATCH 49/66] 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(), -- 2.43.0 From 8fa1d69db3b313235f4a660876cd3fbb05acbf5a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 15:23:20 +0100 Subject: [PATCH 50/66] 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}; -- 2.43.0 From 673c24a34d9b1e075f0b5a65934828aeb55f6615 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 16 Jan 2023 15:43:40 +0100 Subject: [PATCH 51/66] 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()); + } + } +} -- 2.43.0 From 9c4b2872e336c2ee91c65f81b03f212d7d1957db Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 17 Jan 2023 11:50:52 +0100 Subject: [PATCH 52/66] 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; -- 2.43.0 From 44b384cd17d20be0d1168d26905e5a0ffc9a761c Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Tue, 17 Jan 2023 20:11:45 +0100 Subject: [PATCH 53/66] 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; -- 2.43.0 From 19c8703e15d745f1d910d3d1c78e8235ed2cff17 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 18 Jan 2023 11:38:09 +0100 Subject: [PATCH 54/66] 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" -- 2.43.0 From 638573757b8f691960fafac9981d6db1fc3966dd Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 19 Jan 2023 11:39:59 +0100 Subject: [PATCH 55/66] 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); -- 2.43.0 From e5b297a513c4238b87bced71eaa17d113d4c412a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 23 Jan 2023 15:58:57 +0100 Subject: [PATCH 56/66] 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}; -- 2.43.0 From adef468c0bc902923f396a92bda4d68a99e7c880 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Mon, 23 Jan 2023 16:34:52 +0100 Subject: [PATCH 57/66] 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 -- 2.43.0 From 9a056e6ad5c156c83f0c77b3722a0ab1466467d5 Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Wed, 1 Feb 2023 15:37:10 +0100 Subject: [PATCH 58/66] 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; } -- 2.43.0 From b9ed485d601ca1937312646ff4a5454c28eb0a94 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 3 Feb 2023 15:44:50 +0100 Subject: [PATCH 59/66] 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, -- 2.43.0 From 5d0a1858cce1a4cbe0b72893db8fd54baac2b07b Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 10:20:36 +0100 Subject: [PATCH 60/66] 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]; -- 2.43.0 From 5f9a445aa22f87a9f4ffd80d8df5f3e7b6a189cd Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 10:29:28 +0100 Subject: [PATCH 61/66] 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]; -- 2.43.0 From e3cc45d7e3cb8540a8a755d5e3a1b61eab3fff96 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 13:09:42 +0100 Subject: [PATCH 62/66] 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)), -- 2.43.0 From afbaec8e2d7b92020c60590939b4e7423150175c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 13:10:57 +0100 Subject: [PATCH 63/66] 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 -- 2.43.0 From 454d56935c09274b35c22be8bc31f3ce483abed0 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 13:11:57 +0100 Subject: [PATCH 64/66] 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); -- 2.43.0 From 36f20d235c50c49b6fb189b80ae1ddc12e28de39 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 7 Feb 2023 13:58:41 +0100 Subject: [PATCH 65/66] 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 -- 2.43.0 From da166c7bc31b0688d80ff8eea8adeaaa97302aff Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 13:40:03 +0100 Subject: [PATCH 66/66] 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)), -- 2.43.0