From d9427c61125ad770a2a60ab9776a2a626adffe85 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 16 Sep 2022 14:13:41 +0200 Subject: [PATCH 001/244] added AcsParameters --- mission/controller/acs/AcsParameters.cpp | 128 +++++++++++++++ mission/controller/acs/AcsParameters.h | 201 +++++++++++++++++++++++ 2 files changed, 329 insertions(+) create mode 100644 mission/controller/acs/AcsParameters.cpp create mode 100644 mission/controller/acs/AcsParameters.h diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp new file mode 100644 index 00000000..91090028 --- /dev/null +++ b/mission/controller/acs/AcsParameters.cpp @@ -0,0 +1,128 @@ +#include +#include "AcsParameters.h" + +#include +#include + +AcsParameters::AcsParameters() { + + onBoardParams.sampleTime = 0.1; + + inertiaEIVE.inertiaMatrix = {{ 1, 0, 0}, + { 0, 1, 0}, + { 0,0.5, 1}}; + + mgmHandlingParameters.mgm0orientationMatrix = {{ 0, 0,-1}, + { 0, 1, 0}, + { 1, 0, 0}}; + /*mgmHandlingParameters.mgm1orientationMatrix = {{ 0, 0, 1}, + { 0,-1, 0}, + { 1, 0, 0}}; + mgmHandlingParameters.mgm2orientationMatrix = {{ 0, 0,-1}, + { 0, 1, 0}, + { 1 ,0, 0}}; + mgmHandlingParameters.mgm3orientationMatrix = {{ 0, 0, 1}, + { 0,-1, 0}, + { 1, 0, 0}}; + mgmHandlingParameters.mgm4orientationMatrix = {{ 0, 0,-1}, + {-1, 0, 0}, + { 0, 1, 0}};*/ + + rwHandlingParameters.inertiaWheel = 0.000028198; + rwHandlingParameters.maxTrq = 0.0032; //3.2 [mNm] + + //Geometry frame + rwMatrices.alignmentMatrix = {{ 0.9205, 0,-0.9205, 0}, + { 0,-0.9205, 0, 0.9205}, + { 0.3907, 0.3907, 0.3907, 0.3907}}; + + rwMatrices.pseudoInverse = {{ 0.4434,-0.2845, 0.3597}, + { 0.2136,-0.3317, 1.0123}, + {-0.8672,-0.1406, 0.1778}, + { 0.6426, 0.4794, 1.3603}}; + + rwMatrices.nullspace = {-0.7358, 0.5469,-0.3637,-0.1649}; + + kalmanFilterParameters.sensorNoiseSS = 8 * Math::PI / 180; + kalmanFilterParameters.sensorNoiseMAG = 4 * Math::PI / 180; + kalmanFilterParameters.sensorNoiseSTR = 0.1 * Math::PI / 180; + + sunModelParameters.domega = 36000.771; + sunModelParameters.omega_0 = 282.94 * Math::PI / 180.; + sunModelParameters.dm = 35999.049; + sunModelParameters.m_0 = 357.5256; + sunModelParameters.e = 23.4392911 * Math::PI / 180.; + sunModelParameters.e = 0.74508 * Math::PI / 180.; + sunModelParameters.p1 = 6892. / 3600. * Math::PI / 180.; + sunModelParameters.p2 = 72. / 3600. * Math::PI / 180.; + + safeModeControllerParameters.k_rate_mekf = 0.00059437; + safeModeControllerParameters.k_align_mekf = 0.000056875; + + safeModeControllerParameters.sunTargetDir = {1,0,0}; + + detumbleCtrlParameters.gainD = pow(10.0,-3.3); + + // Stuttgart + groundStationParameters.latitudeGs = 48.7495 * Math::PI / 180.; + groundStationParameters.longitudeGs = 9.10384 * Math::PI / 180.; + groundStationParameters.altitudeGs = 500; + + groundStationParameters.earthRadiusEquat = 6378137; + groundStationParameters.earthRadiusPolar = 6356752.314; + +// Geometry frame + targetModeControllerParameters.refDirection = {1,0,0}; + targetModeControllerParameters.refRotRate = {0,0,0}; + targetModeControllerParameters.quatRef = {0,0,0,1}; + + targetModeControllerParameters.avoidBlindStr = true; + targetModeControllerParameters.blindAvoidStart = 1.5; + targetModeControllerParameters.blindAvoidStop = 2.5; + targetModeControllerParameters.blindRotRate = 1 * Math::PI /180; + + targetModeControllerParameters.zeta = 0.3; + targetModeControllerParameters.om = 0.3; + targetModeControllerParameters.qiMin = 0.1; + targetModeControllerParameters.omMax = 1 * Math::PI / 180; + targetModeControllerParameters.gainNullspace = 0.01; + + targetModeControllerParameters.desatMomentumRef = {0,0,0}; + targetModeControllerParameters.deSatGainFactor = 1000; + targetModeControllerParameters.desatOn = true; + + targetModeControllerParameters.omegaEarth = 0.000072921158553; + + strParameters.boresightAxis = { 0.7593, 0,-0.6508}; + strParameters.exclusionAngle = 20 * Math::PI /180; + + magnetorquesParameter.mtq0orientationMatrix = {{ 1, 0, 0}, + { 0, 0, 1}, + { 0,-1, 0}}; + magnetorquesParameter.mtq1orientationMatrix = {{ 1, 0, 0}, + { 0, 1, 0}, + { 0, 0, 1}}; + magnetorquesParameter.mtq2orientationMatrix = {{ 0, 0, 1}, + { 0, 1, 0}, + {-1, 0, 0}}; + magnetorquesParameter.alignmentMatrixMtq = {{ 0, 0,-1}, + {-1, 0, 0}, + { 0, 1, 0}}; + magnetorquesParameter.inverseAlignment = {{ 0,-1, 0}, + { 0, 0, 1}, + {-1, 0, 0}}; + magnetorquesParameter.DipolMax = 0.2; + +} + +AcsParameters::~AcsParameters() { +} + +/* +ReturnValue_t AcsParameters::getParameter(uint8_t domainId, + uint16_t parameterId, ParameterWrapper* parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex) { + + return 0; + +}*/ diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h new file mode 100644 index 00000000..b4e32d65 --- /dev/null +++ b/mission/controller/acs/AcsParameters.h @@ -0,0 +1,201 @@ +/******************************* + * EIVE Flight Software Framework (FSFW) + * (c) 2022 IRS, Uni Stuttgart + *******************************/ + +#ifndef ACSPARAMETERS_H_ +#define ACSPARAMETERS_H_ + +#include + +typedef unsigned char uint8_t; + +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); + + struct OnBoardParams { + double sampleTime; // [s] + } onBoardParams; + + struct InertiaEIVE { + double inertiaMatrix[3][3]; + double inertiaMatrixInverse[3][3]; + } inertiaEIVE; + + struct MgmHandlingParameters { + float mgm0orientationMatrix[3][3]; + float mgm1orientationMatrix[3][3]; + float mgm2orientationMatrix[3][3]; + float mgm3orientationMatrix[3][3]; + float mgm4orientationMatrix[3][3]; + } mgmHandlingParameters; + + struct SusHandlingParameters { + float sus0orientationMatrix[3][3]; + float sus1orientationMatrix[3][3]; + float sus2orientationMatrix[3][3]; + float sus3orientationMatrix[3][3]; + float sus4orientationMatrix[3][3]; + float sus5orientationMatrix[3][3]; + float sus6orientationMatrix[3][3]; + float sus7orientationMatrix[3][3]; + float sus8orientationMatrix[3][3]; + float sus9orientationMatrix[3][3]; + float sus10orientationMatrix[3][3]; + float sus11orientationMatrix[3][3]; + + float filterAlpha; + float sunThresh; + } susHandlingParameters; + + struct RmuHandlingParameters { + double rmu0orientationMatrix[3][3]; + double rmu1orientationMatrix[3][3]; + double rmu2orientationMatrix[3][3]; + } rmuHandlingParameters; + + struct RwHandlingParameters { + double rw0orientationMatrix[3][3]; + double rw1orientationMatrix[3][3]; + double rw2orientationMatrix[3][3]; + double rw3orientationMatrix[3][3]; + double inertiaWheel; + double maxTrq; + } rwHandlingParameters; + + struct SafeModeControllerParameters { + double k_rate_mekf; + double k_align_mekf; + + double k_rate_no_mekf; + double k_align_no_mekf; + double sunMagAngleMin; + + double sunTargetDir[3]; //Body frame + double satRateRef[3]; //Body frame + + } safeModeControllerParameters; + + struct DetumbleCtrlParameters { + + double gainD; + + } detumbleCtrlParameters; + + + struct PointingModeControllerParameters { + double updtFlag; + double A_rw[3][12]; + + double refDirection[3]; + double refRotRate[3]; + double quatRef[4]; + bool avoidBlindStr; + double blindAvoidStart; + double blindAvoidStop; + double blindRotRate; + + double zeta; + double zetaLow; + double om; + double omLow; + double qiMin; + double omMax; + double gainNullspace; + + double desatMomentumRef[3]; + double deSatGainFactor; + bool desatOn; + + double omegaEarth; + + + } inertialModeControllerParameters, nadirModeControllerParameters, + targetModeControllerParameters; + + + struct RWMatrices { + double alignmentMatrix[3][4]; + double pseudoInverse[4][3]; + double without0[4][3]; + double without1[4][3]; + double without2[4][3]; + double without3[4][3]; + double nullspace[4]; + } rwMatrices; + + struct StrParameters { + double exclusionAngle; +// double strOrientationMatrix[3][3]; + double boresightAxis[3]; //in body/geometry frame + } strParameters; + + struct GpsParameters { + } gpsParameters; + + struct GroundStationParameters { + double latitudeGs; // [rad] Latitude + double longitudeGs; // [rad] Longitude + double altitudeGs; // [m] Altitude + double earthRadiusEquat; // [m] + double earthRadiusPolar; // [m] + } groundStationParameters; + + struct SunModelParameters { + enum UseSunModel { + NO = 0, YES = 3 + }; + uint8_t useSunModel; + float domega; + float omega_0; //Rektaszension des Aufsteigenden Knotens plus Argument des Perigäums + float m_0; //coefficients for mean anomaly + float dm; //coefficients for mean anomaly + float e; //angle of earth's rotation axis + float e1; + float p1; //some parameter + float p2; //some parameter + } sunModelParameters; + + struct KalmanFilterParameters { + uint8_t activateKalmanFilter; + uint8_t requestResetFlag; + double maxToleratedTimeBetweenKalmanFilterExecutionSteps; + double processNoiseOmega[3]; + double processNoiseQuaternion[4]; + + double sensorNoiseSTR; + double sensorNoiseSS; + double sensorNoiseMAG; + double sensorNoiseRMU[3]; + + double sensorNoiseArwRmu; //Angular Random Walk + double sensorNoiseBsRMU; // Bias Stability + } kalmanFilterParameters; + + struct MagnetorquesParameter { + + double mtq0orientationMatrix[3][3]; + double mtq1orientationMatrix[3][3]; + double mtq2orientationMatrix[3][3]; + double alignmentMatrixMtq[3][3]; + double inverseAlignment[3][3]; + double DipolMax; // [Am^2] + + } magnetorquesParameter; + + struct DetumbleParameter { + + uint8_t detumblecounter; + double omegaDetumbleStart; + double omegaDetumbleEnd; + } detumbleParameter; +}; + +#endif /* ACSPARAMETERS_H_ */ From 4583f0cb86fab232fd06d240eb6ad0fd36bb6fd1 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 10:46:21 +0200 Subject: [PATCH 002/244] added Igrf13Model --- mission/controller/acs/Igrf13Model.cpp | 126 +++++++++++++++++++++++++ mission/controller/acs/Igrf13Model.h | 117 +++++++++++++++++++++++ 2 files changed, 243 insertions(+) create mode 100644 mission/controller/acs/Igrf13Model.cpp create mode 100644 mission/controller/acs/Igrf13Model.h diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp new file mode 100644 index 00000000..de4659d8 --- /dev/null +++ b/mission/controller/acs/Igrf13Model.cpp @@ -0,0 +1,126 @@ +/* + * Igrf13Model.cpp + * + * Created on: 10 Mar 2022 + * Author: Robin Marquardt + */ + +#include "Igrf13Model.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Math; + +Igrf13Model::Igrf13Model(){ +} +Igrf13Model::~Igrf13Model(){ +} + +void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, + const double altitude, timeval timeOfMagMeasurement, double* magFieldModelInertial) { + + double phi = longitude, theta = gcLatitude; //geocentric + /* Here is the co-latitude needed*/ + theta -= 90*PI/180; + theta *= (-1); + + double rE = 6371200.0; // radius earth [m] + /* Predefine recursive associated Legendre polynomials */ + double P11 = 1; + double P10 = P11; //P10 = P(n-1,m-0) + double dP11 = 0; //derivative + double dP10 = dP11; //derivative + + double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0; + + for (int m = 0; m <= igrfOrder; m++) { + for (int n = 1; n <= igrfOrder; n++) { + if (m <= n) { + /* Calculation of Legendre Polynoms (normalised) */ + if (n == m) { + P2 = sin(theta) * P11; + dP2 = sin(theta) * dP11 - cos(theta) * P11; + P11 = P2; + P10 = P11; + P20 = 0; + dP11 = dP2; + dP10 = dP11; + dP20 = 0; + } else if (n == 1) { + P2 = cos(theta) * P10; + dP2 = cos(theta) * dP10 - sin(theta) * P10; + P20 = P10; + P10 = P2; + dP20 = dP10; + dP10 = dP2; + } else { + K = (pow((n - 1), 2) - pow(m, 2)) + / ((2 * n - 1) * (2 * n - 3)); + P2 = cos(theta) * P10 - K * P20; + dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20; + P20 = P10; + P10 = P2; + dP20 = dP10; + dP10 = dP2; + } + /* gradient of scalar potential towards radius */ + 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 */ + 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 */ + magFieldModel[2]+=pow(rE/(altitude+rE),(n+2))* + ((-updatedG[m][n-1]*sin(m*phi) + updatedH[m][n-1]*cos(m*phi))*P2*m); + } + } + } + + magFieldModel[1] *= -1; + magFieldModel[2] *= (-1 / sin(theta)); + + /* Next step: transform into inertial KOS (IJK)*/ + //Julean Centuries + double JD2000Floor = 0; + double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + JD2000Floor = floor(JD2000); + double JC2000 = JD2000Floor / 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 lst = gst + longitude; //local sidereal time [rad] + + + + 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); + + double normVecMagFieldInert[3] = {0,0,0}; + VectorOperations::normalize(magFieldModelInertial, normVecMagFieldInert, 3); +} + +void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement){ + + double JD2000Igrf = (2458850.0-2451545); //Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000 + double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + double days = ceil(JD2000-JD2000Igrf); + for(int i = 0;i <= igrfOrder; i++){ + for(int j = 0;j <= (igrfOrder-1); j++){ + updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days/365); + updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days/365); + } + } +} diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h new file mode 100644 index 00000000..dadb9c4e --- /dev/null +++ b/mission/controller/acs/Igrf13Model.h @@ -0,0 +1,117 @@ +/* + * Igrf13Model.h + * + * Created on: 10 Mar 2022 + * Author: Robin Marquardt + * Description: Calculates the magnetic field vector of earth with the IGRF Model. + * Sources: https://www.ngdc.noaa.gov/IAGA/vmod/igrf.html + * https://doi.org/10.1186/s40623-020-01288-x + * J. Davis, Mathematical Modeling of Earth's Magnetic Field, TN, 2004 + * + * [Conversion of ENU (geocentric) to IJK: Skript Bahnmechanik für Raumfahrzeuge, + * Prof. Dr.-Ing. Stefanos Fasoulas / Dr.-Ing. Frank Zimmermann] + * + */ + +#ifndef IGRF13MODEL_H_ +#define IGRF13MODEL_H_ + +#include +#include +#include +#include + + +// Output should be transformed to [T] instead of [nT] +// Updating Coefficients has to be implemented yet. Question, updating everyday ? +class Igrf13Model { + +public: + Igrf13Model(); + virtual ~Igrf13Model(); + + // Main Function + void magFieldComp(const double longitude, const double gcLatitude, const double altitude, timeval timeOfMagMeasurement,double* magFieldModelInertial); + // Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is oriented from the satellite to earth COM + // Difference up to 25 km, which is 5 % of the total flight altitude + /* Inputs: + * - longitude: geocentric longitude [rad] + * - latitude: geocentric latitude [rad] + * - altitude: [m] + * - timeOfMagMeasurement: time of actual measurement [s] + * + * Outputs: + * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ + + + // Coefficient wary over year, could be updated sometimes. + void updateCoeffGH(timeval timeOfMagMeasurement); //Secular variation (SV) + double magFieldModel[3]; + +private: + const 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, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5}, + {0, 0, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3 ,0.7}, + {0 ,0 ,0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3}, + {0, 0, 0, 0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8}, + {0, 0, 0, 0, 0,-64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0}, + {0, 0, 0, 0, 0, 0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8}, + {0, 0, 0, 0, 0, 0, 0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, -11.9, -2.4, -0.6, -0.5, 0.4}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, -3.8, 0.2, 0.1, 0.1}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3.1, -1.1, 0.5}, + {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.4}}; // [m][n] in nT + + const double coeffH[14][13] = {{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, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6}, + {0, 0, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4}, + {0, 0, 0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4}, + {0, 0, 0, 0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3}, + {0, 0, 0, 0, 0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1}, + {0, 0, 0, 0, 0, 0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3}, + {0, 0, 0, 0, 0, 0, 0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1}, + {0, 0, 0, 0, 0, 0, 0, 0, 9.6, -0.1, -3.0, 0.2, 0.5}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, -8.8, -2.0, -0.9, 0.5}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2.6, 0.0, -0.4}, + {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.6}}; // [m][n] in nT + + const double svG[14][13] = {{5.7, -11, 2.2, -1.2, -0.3, -0.5, -0.1, 0, 0, 0, 0, 0 ,0}, + {7.4, -7, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0, 0, 0, 0,0}, + {0, -2.1, 3.1, -5.9, -0.6, 0.4, 0, -0.1, 0, 0, 0, 0, 0}, + {0, 0, -12, 5.2, 0.2, 1.3, 0.7, 0.4, 0, 0, 0, 0, 0}, + {0 ,0 ,0, -5.1, 1.3, -1.4, 0.1, -0.1, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0.9, 0, -0.5, 0.4, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0.9, -0.8, 0.3, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0.8, -0.1, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 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, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 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] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {-25.9, -30.2, 6, -0.1, 0, 0, 0.6, -0.2, 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.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0, 0, 0, 0, 0}, + {0 ,0 ,0, -5, 3, 0.8, -0.2, 0.5, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0.3, 0, -1.1, -0.3, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 1, 0.1, -0.4, 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, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 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 updatedG[14][13]; + double updatedH[14][13]; + static const int igrfOrder = 13; // degree of truncation +}; + +#endif /* IGRF13MODEL_H_ */ From b42986dd8a9c006f6430ea0428e6a765666011de Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 11:10:28 +0200 Subject: [PATCH 003/244] added MathOperations to util folder --- mission/controller/acs/Igrf13Model.cpp | 2 +- mission/controller/acs/util/MathOperations.h | 297 +++++++++++++++++++ 2 files changed, 298 insertions(+), 1 deletion(-) create mode 100644 mission/controller/acs/util/MathOperations.h diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index de4659d8..fbdb481e 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include using namespace Math; diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h new file mode 100644 index 00000000..57d9b7b4 --- /dev/null +++ b/mission/controller/acs/util/MathOperations.h @@ -0,0 +1,297 @@ +/* + * MathOperations.h + * + * Created on: 3 Mar 2022 + * Author: rooob + */ + +#ifndef MATH_MATHOPERATIONS_H_ +#define MATH_MATHOPERATIONS_H_ + +#include +#include +#include +#include +#include + +using namespace Math; + +template +class MathOperations { +public: + static void skewMatrix(const T1 vector[], T2 *result) { + // Input Dimension [3], Output [3][3] + result[0] = 0; + result[1] = -vector[2]; + result[2] = vector[1]; + result[3] = vector[2]; + result[4] = 0; + result[5] = -vector[0]; + result[6] = -vector[1]; + result[7] = vector[0]; + result[8] = 0; + } + static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], + T2 *result, uint8_t size = 3) { + // Looks like MatrixOpertions::multiply is able to do the same thing + for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) { + for (uint8_t resultRow = 0; resultRow < size; resultRow++) { + result[resultColumn + size * resultRow] = vector1[resultRow] + * transposeVector2[resultColumn]; + + } + } + /*matrixSun[i][j] = sunEstB[i] * sunEstB[j]; + matrixMag[i][j] = magEstB[i] * magEstB[j]; + matrixSunMag[i][j] = sunEstB[i] * magEstB[j]; + matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/ + } + + static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, + uint8_t colSize) { + int min_idx; + T1 temp; + memcpy(result, matrix, rowSize * colSize * sizeof(*result)); + // One by one move boundary of unsorted subarray + for (int k = 0; k < rowSize; k++) { + for (int i = 0; i < colSize - 1; i++) { + // Find the minimum element in unsorted array + min_idx = i; + for (int j = i + 1; j < colSize; j++) { + if (result[j + k * colSize] + < result[min_idx + k * colSize]) { + min_idx = j; + } + } + // Swap the found minimum element with the first element + temp = result[i + k * colSize]; + result[i + k * colSize] = result[min_idx + k * colSize]; + result[min_idx + k * colSize] = temp; + } + } + } + + 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}} + T1 julianDate2000; + julianDate2000 = (time.tv_sec/86400.0)+2440587.5-2451545; + return julianDate2000; + } + + static void dcmFromQuat(const T1 vector[], T1 *outputDcm){ + // convention q = [qx,qy,qz, qw] + outputDcm[0] = pow(vector[0],2) - pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2); + outputDcm[1] = 2*(vector[0]*vector[1] + vector[2]*vector[3]); + outputDcm[2] = 2*(vector[0]*vector[2] - vector[1]*vector[3]); + + outputDcm[3] = 2*(vector[1]*vector[0] - vector[2]*vector[3]); + outputDcm[4] = -pow(vector[0],2) + pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2); + outputDcm[5] = 2*(vector[1]*vector[2] + vector[0]*vector[3]); + + outputDcm[6] = 2*(vector[2]*vector[0] + vector[1]*vector[3]); + outputDcm[7] = 2*(vector[2]*vector[1] - vector[0]*vector[3]); + 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){ + + double radiusPolar = 6378137; + double radiusEqua = 6356752.314; + + 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); + + } + + /* @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){ + + 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; + double gmst = 0; //greenwich mean sidereal time + gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000,2) - + 0.0000062 * pow(JC2000,3) + 1.002737909350795 * sec; + double rest = gmst / 86400; + double FloorRest = floor(rest); + double secOfDay = rest-FloorRest; + secOfDay *= 86400; + gmst = secOfDay / 240 * PI / 180; + + outputDcmEJ[0] = cos(gmst); + outputDcmEJ[1] = sin(gmst); + outputDcmEJ[2] = 0; + outputDcmEJ[3] = -sin(gmst); + outputDcmEJ[4] = cos(gmst); + outputDcmEJ[5] = 0; + outputDcmEJ[6] = 0; + outputDcmEJ[7] = 0; + outputDcmEJ[8] = 1; + + } + + /* @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) + + double JD2000UTC1 = convertUnixToJD2000(unixTime); + +// 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; + +//------------------------------------------------------------------------------------- +// 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; + + 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 Precession P +//------------------------------------------------------------------------------------- + double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + + 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); + + zeta *= arcsecFactor; + theta2 *= arcsecFactor; + ze *= arcsecFactor; + + 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); + +//------------------------------------------------------------------------------------- +// 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); + +// delta eps approx + double de = 9.203 * arcsecFactor *cos(Om); + +// % true obliquity of the ecliptic eps p.71 (simplified) + double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;; + + 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 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); + +//------------------------------------------------------------------------------------- +// 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: Mon, 19 Sep 2022 13:24:20 +0200 Subject: [PATCH 004/244] small fixes --- mission/controller/acs/AcsParameters.cpp | 4 +- mission/controller/acs/Igrf13Model.cpp | 7 +- mission/controller/acs/Igrf13Model.h | 119 ++++++++++--------- mission/controller/acs/util/MathOperations.h | 2 +- 4 files changed, 66 insertions(+), 66 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 91090028..a3c0211e 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -1,8 +1,8 @@ #include #include "AcsParameters.h" -#include -#include +//#include +#include AcsParameters::AcsParameters() { diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index fbdb481e..91bfad15 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -6,17 +6,16 @@ */ #include "Igrf13Model.h" -#include +#include #include #include -#include +//#include #include #include #include #include #include -using namespace Math; Igrf13Model::Igrf13Model(){ } @@ -28,7 +27,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, double phi = longitude, theta = gcLatitude; //geocentric /* Here is the co-latitude needed*/ - theta -= 90*PI/180; + theta -= 90*Math::PI/180; theta *= (-1); double rE = 6371200.0; // radius earth [m] diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index dadb9c4e..7e8d6e3f 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -16,15 +16,16 @@ #ifndef IGRF13MODEL_H_ #define IGRF13MODEL_H_ -#include +#include +//#include #include #include -#include +#include // Output should be transformed to [T] instead of [nT] // Updating Coefficients has to be implemented yet. Question, updating everyday ? -class Igrf13Model { +class Igrf13Model:public HasParametersIF{ public: Igrf13Model(); @@ -49,65 +50,65 @@ public: double magFieldModel[3]; private: - const 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, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5}, - {0, 0, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3 ,0.7}, - {0 ,0 ,0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3}, - {0, 0, 0, 0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8}, - {0, 0, 0, 0, 0,-64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0}, - {0, 0, 0, 0, 0, 0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8}, - {0, 0, 0, 0, 0, 0, 0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0}, - {0, 0, 0, 0, 0, 0, 0, 0, -11.9, -2.4, -0.6, -0.5, 0.4}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, -3.8, 0.2, 0.1, 0.1}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3.1, -1.1, 0.5}, - {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.4}}; // [m][n] in nT + const 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 , 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2,-17.6, 2.9,-0.1,-2.5, 0.5, 0.5}, + { 0 , 0 , 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7}, + { 0 , 0 , 0 , 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3}, + { 0 , 0 , 0 , 0 , 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8}, + { 0 , 0 , 0 , 0 , 0 , -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0}, + { 0 , 0 , 0 , 0 , 0 , 0 , 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , -0.3, -9.3, 1.4, 1.4,-0.3, 0.0}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-11.9,-2.4,-0.6,-0.5, 0.4}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-3.8, 0.2, 0.1, 0.1}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3.1,-1.1, 0.5}, + { 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.4}}; // [m][n] in nT - const double coeffH[14][13] = {{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, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6}, - {0, 0, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4}, - {0, 0, 0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4}, - {0, 0, 0, 0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3}, - {0, 0, 0, 0, 0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1}, - {0, 0, 0, 0, 0, 0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3}, - {0, 0, 0, 0, 0, 0, 0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1}, - {0, 0, 0, 0, 0, 0, 0, 0, 9.6, -0.1, -3.0, 0.2, 0.5}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, -8.8, -2.0, -0.9, 0.5}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2.6, 0.0, -0.4}, - {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.6}}; // [m][n] in nT + const double coeffH[14][13] = {{ 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 , -734.6, 241.9,-158.4, 208.3, 25.1,-16.9,-15.3, 11.0,-0.2, 2.5, 0.5, 0.6}, + { 0 , 0 ,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4}, + { 0 , 0 , 0 ,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4}, + { 0 , 0 , 0 , 0 , 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3}, + { 0 , 0 , 0 , 0 , 0 , 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1}, + { 0 , 0 , 0 , 0 , 0 , 0 , -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 2.8, -1.4,-3.4,-1.6, 0.6,-0.1}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 9.6,-0.1,-3.0, 0.2, 0.5}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-8.8,-2.0,-0.9, 0.5}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-2.6, 0.0,-0.4}, + { 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.6}}; // [m][n] in nT - const double svG[14][13] = {{5.7, -11, 2.2, -1.2, -0.3, -0.5, -0.1, 0, 0, 0, 0, 0 ,0}, - {7.4, -7, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0, 0, 0, 0,0}, - {0, -2.1, 3.1, -5.9, -0.6, 0.4, 0, -0.1, 0, 0, 0, 0, 0}, - {0, 0, -12, 5.2, 0.2, 1.3, 0.7, 0.4, 0, 0, 0, 0, 0}, - {0 ,0 ,0, -5.1, 1.3, -1.4, 0.1, -0.1, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0.9, 0, -0.5, 0.4, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0.9, -0.8, 0.3, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0.8, -0.1, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 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, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 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 svG[14][13] = {{5.7,-11.0, 2.2,-1.2,-0.3,-0.5,-0.1, 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 , -2.1, 3.1,-5.9,-0.6, 0.4, 0.0,-0.1, 0, 0, 0, 0, 0}, + {0 , 0 ,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0, 0, 0, 0, 0}, + {0 , 0 , 0 ,-5.1, 1.3,-1.4, 0.1,-0.1, 0, 0, 0, 0, 0}, + {0 , 0 , 0 , 0 , 0.9, 0.0,-0.5, 0.4, 0, 0, 0, 0, 0}, + {0 , 0 , 0 , 0 , 0 , 0.9,-0.8, 0.3, 0, 0, 0, 0, 0}, + {0 , 0 , 0 , 0 , 0 , 0 , 0.8,-0.1, 0, 0, 0, 0, 0}, + {0 , 0 , 0 , 0 , 0 , 0 , 0 , 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 , 0 , 0, 0, 0, 0, 0}, + {0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0}, + {0 , 0 , 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] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {-25.9, -30.2, 6, -0.1, 0, 0, 0.6, -0.2, 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.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0, 0, 0, 0, 0}, - {0 ,0 ,0, -5, 3, 0.8, -0.2, 0.5, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0.3, 0, -1.1, -0.3, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 1, 0.1, -0.4, 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, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 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] = {{ 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 ,-22.4,-1.1, 6.5, 2.5,-1.6, 0.6, 0.6, 0, 0, 0, 0, 0}, + { 0 , 0 , 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0, 0, 0, 0, 0}, + { 0 , 0 , 0 ,-5.0, 3.0, 0.8,-0.2, 0.5, 0, 0, 0, 0, 0}, + { 0 , 0 , 0 , 0 , 0.3, 0.0,-1.1,-0.3, 0, 0, 0, 0, 0}, + { 0 , 0 , 0 , 0 , 0 , 1.0, 0.1,-0.4, 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 , 0 , 0, 0, 0, 0, 0}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0}, + { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0}, + { 0 , 0 , 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 updatedG[14][13]; double updatedH[14][13]; diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 57d9b7b4..82d19b19 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -8,7 +8,7 @@ #ifndef MATH_MATHOPERATIONS_H_ #define MATH_MATHOPERATIONS_H_ -#include +#include #include #include #include From 8b6d3c98358d896c15d207e9a95f2b717116544b Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 14:09:14 +0200 Subject: [PATCH 005/244] small formating-fixes --- mission/controller/acs/AcsParameters.cpp | 22 ++--- mission/controller/acs/Igrf13Model.h | 106 +++++++++++------------ 2 files changed, 64 insertions(+), 64 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index a3c0211e..63d8951d 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -8,9 +8,9 @@ AcsParameters::AcsParameters() { onBoardParams.sampleTime = 0.1; - inertiaEIVE.inertiaMatrix = {{ 1, 0, 0}, - { 0, 1, 0}, - { 0,0.5, 1}}; + inertiaEIVE.inertiaMatrix = {{ 1.0, 0.0, 0.0}, + { 0.0, 1.0, 0.0}, + { 0.0, 0.5, 1.0}}; mgmHandlingParameters.mgm0orientationMatrix = {{ 0, 0,-1}, { 0, 1, 0}, @@ -32,16 +32,16 @@ AcsParameters::AcsParameters() { rwHandlingParameters.maxTrq = 0.0032; //3.2 [mNm] //Geometry frame - rwMatrices.alignmentMatrix = {{ 0.9205, 0,-0.9205, 0}, - { 0,-0.9205, 0, 0.9205}, - { 0.3907, 0.3907, 0.3907, 0.3907}}; + rwMatrices.alignmentMatrix = {{ 0.9205, 0.0000, -0.9205, 0.0000}, + { 0.0000, -0.9205, 0.0000, 0.9205}, + { 0.3907, 0.3907, 0.3907, 0.3907}}; - rwMatrices.pseudoInverse = {{ 0.4434,-0.2845, 0.3597}, - { 0.2136,-0.3317, 1.0123}, - {-0.8672,-0.1406, 0.1778}, - { 0.6426, 0.4794, 1.3603}}; + rwMatrices.pseudoInverse = {{ 0.4434, -0.2845, 0.3597}, + { 0.2136, -0.3317, 1.0123}, + { -0.8672, -0.1406, 0.1778}, + { 0.6426, 0.4794, 1.3603}}; - rwMatrices.nullspace = {-0.7358, 0.5469,-0.3637,-0.1649}; + rwMatrices.nullspace = { -0.7358, 0.5469, -0.3637, -0.1649}; kalmanFilterParameters.sensorNoiseSS = 8 * Math::PI / 180; kalmanFilterParameters.sensorNoiseMAG = 4 * Math::PI / 180; diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index 7e8d6e3f..6e46a90e 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -52,63 +52,63 @@ public: private: const 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 , 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2,-17.6, 2.9,-0.1,-2.5, 0.5, 0.5}, - { 0 , 0 , 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7}, - { 0 , 0 , 0 , 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3}, - { 0 , 0 , 0 , 0 , 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8}, - { 0 , 0 , 0 , 0 , 0 , -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0}, - { 0 , 0 , 0 , 0 , 0 , 0 , 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , -0.3, -9.3, 1.4, 1.4,-0.3, 0.0}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-11.9,-2.4,-0.6,-0.5, 0.4}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-3.8, 0.2, 0.1, 0.1}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3.1,-1.1, 0.5}, - { 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.4}}; // [m][n] in nT + { 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}, + { 0.0, 0.0, 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7}, + { 0.0, 0.0, 0.0, 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3}, + { 0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8}, + { 0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4,-0.3, 0.0}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-11.9,-2.4,-0.6,-0.5, 0.4}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-3.8, 0.2, 0.1, 0.1}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1,-1.1, 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.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] = {{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 }, + const 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 , -734.6, 241.9,-158.4, 208.3, 25.1,-16.9,-15.3, 11.0,-0.2, 2.5, 0.5, 0.6}, - { 0 , 0 ,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4}, - { 0 , 0 , 0 ,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4}, - { 0 , 0 , 0 , 0 , 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3}, - { 0 , 0 , 0 , 0 , 0 , 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1}, - { 0 , 0 , 0 , 0 , 0 , 0 , -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 2.8, -1.4,-3.4,-1.6, 0.6,-0.1}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 9.6,-0.1,-3.0, 0.2, 0.5}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-8.8,-2.0,-0.9, 0.5}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-2.6, 0.0,-0.4}, - { 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.6}}; // [m][n] in nT + { 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}, + { 0.0, 0.0,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4}, + { 0.0, 0.0, 0.0,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4}, + { 0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4,-3.4,-1.6, 0.6,-0.1}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6,-0.1,-3.0, 0.2, 0.5}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-8.8,-2.0,-0.9, 0.5}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-2.6, 0.0,-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.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] = {{5.7,-11.0, 2.2,-1.2,-0.3,-0.5,-0.1, 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 , -2.1, 3.1,-5.9,-0.6, 0.4, 0.0,-0.1, 0, 0, 0, 0, 0}, - {0 , 0 ,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0, 0, 0, 0, 0}, - {0 , 0 , 0 ,-5.1, 1.3,-1.4, 0.1,-0.1, 0, 0, 0, 0, 0}, - {0 , 0 , 0 , 0 , 0.9, 0.0,-0.5, 0.4, 0, 0, 0, 0, 0}, - {0 , 0 , 0 , 0 , 0 , 0.9,-0.8, 0.3, 0, 0, 0, 0, 0}, - {0 , 0 , 0 , 0 , 0 , 0 , 0.8,-0.1, 0, 0, 0, 0, 0}, - {0 , 0 , 0 , 0 , 0 , 0 , 0 , 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 , 0 , 0, 0, 0, 0, 0}, - {0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0}, - {0 , 0 , 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 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}, + {0.0, 0.0,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0,-5.1, 1.3,-1.4, 0.1,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.9, 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.9,-0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8,-0.1, 0.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, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 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] = {{ 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 ,-22.4,-1.1, 6.5, 2.5,-1.6, 0.6, 0.6, 0, 0, 0, 0, 0}, - { 0 , 0 , 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0, 0, 0, 0, 0}, - { 0 , 0 , 0 ,-5.0, 3.0, 0.8,-0.2, 0.5, 0, 0, 0, 0, 0}, - { 0 , 0 , 0 , 0 , 0.3, 0.0,-1.1,-0.3, 0, 0, 0, 0, 0}, - { 0 , 0 , 0 , 0 , 0 , 1.0, 0.1,-0.4, 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 , 0 , 0, 0, 0, 0, 0}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0}, - { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0}, - { 0 , 0 , 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] = {{ 0.0, 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}, + { 0.0, 0.0, 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, + { 0.0, 0.0, 0.0,-5.0, 3.0, 0.8,-0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0}, + { 0.0, 0.0, 0.0, 0.0, 0.3, 0.0,-1.1,-0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1,-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.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.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + { 0.0, 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 updatedG[14][13]; double updatedH[14][13]; From d497dd53c5b2c4dddd3df2017298e884a3dbe43d Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 14:28:54 +0200 Subject: [PATCH 006/244] added MKF --- .../acs/MultiplicativeKalmanFilter.cpp | 1205 +++++++++++++++++ .../acs/MultiplicativeKalmanFilter.h | 107 ++ 2 files changed, 1312 insertions(+) create mode 100644 mission/controller/acs/MultiplicativeKalmanFilter.cpp create mode 100644 mission/controller/acs/MultiplicativeKalmanFilter.h diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp new file mode 100644 index 00000000..5d895566 --- /dev/null +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -0,0 +1,1205 @@ +/* + * MultiplicativeKalmanFilter.cpp + * + * Created on: 4 Feb 2022 + * Author: rooob + */ + +#include +#include +#include +#include +#include +#include +#include "MultiplicativeKalmanFilter.h" +#include +#include + +/*Initialisation of values for parameters in constructor*/ +MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) : + initialQuaternion { 0.5, 0.5, 0.5, 0.5 },initialCovarianceMatrix + {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}, + {0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}}{ + loadAcsParameters(acsParameters_); + +} + +MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() { + +} + +void MultiplicativeKalmanFilter::loadAcsParameters( + AcsParameters *acsParameters_) { + inertiaEIVE = &(acsParameters_->inertiaEIVE); + kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ +} + +void MultiplicativeKalmanFilter::reset() { + +} + +void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *validMagField_, + const double *sunDir_, const bool *validSS, + const double *sunDirJ, const bool *validSSModel, + const double *magFieldJ,const bool *validMagModel) { // valids for "model measurements"? + // 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 + + double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0}, + normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0}; + VectorOperations::normalize(magneticField_, normMagB, 3); // b2 + VectorOperations::normalize(sunDir_, normSunB, 3); //b1 + VectorOperations::normalize(magFieldJ, normMagJ, 3); //r2 + VectorOperations::normalize(sunDirJ, normSunJ, 3); //r1 + + double thirdAxis_B[3] = {0,0,0}, thirdAxis_J[3] = {0,0,0}; + VectorOperations::cross(normSunJ, normMagJ, thirdAxis_J); + VectorOperations::cross(normSunB, normMagB, thirdAxis_B); + VectorOperations::normalize(thirdAxis_J, thirdAxis_J, 3); //rx + VectorOperations::normalize(thirdAxis_B, thirdAxis_B, 3); //bx + + double weigthSun= 1 / (sigmaSun * sigmaSun); //a1 + double weigthMag= 1 / (sigmaMag * sigmaMag); //a2 + + double a[3] = {0,0,0}, b[3] = {0,0,0}; + VectorOperations::mulScalar(normSunB, weigthSun, a, 3); //a + VectorOperations::mulScalar(normMagB, weigthMag, b, 3); //b + + double thirdAxisCross[3] = {0,0,0}, weightACross[3] = {0,0,0}, + weightBCross[3] = {0,0,0},weigthCrossSum[3] = {0,0,0}; + VectorOperations::cross(thirdAxis_B, thirdAxis_J, + thirdAxisCross); //cross(bx,rx) + VectorOperations::cross(a, normSunJ, weightACross); + VectorOperations::cross(b, normMagJ, weightBCross); + VectorOperations::add(weightACross, weightBCross, + weigthCrossSum); + + double thirdAxisSum[3] = {0,0,0}, sum2[3] = {0,0,0}; + VectorOperations::add(thirdAxis_B, thirdAxis_J, thirdAxisSum); + VectorOperations::add(weightACross, weightBCross, sum2); + + double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0; + alpha = (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)) + * (VectorOperations::dot(a, normSunJ) + + VectorOperations::dot(b, normMagJ)) + + VectorOperations::dot(thirdAxisCross, weigthCrossSum); + beta = VectorOperations::dot(thirdAxisSum, sum2); + gamma = sqrt(alpha * alpha + beta * beta); + constPlus = 1 + / (2 + * sqrt( + gamma * (gamma + alpha) + * (1 + + VectorOperations::dot( + thirdAxis_B, + thirdAxis_J)))); + constMin = 1 + / (2 + * sqrt( + gamma * (gamma - alpha) + * (1 + + VectorOperations::dot( + thirdAxis_B, + thirdAxis_J)))); + + /*Quaternion Computation*/ + double quat[3] = {0,0,0}, var1[3] = {0,0,0}, var2[3] = {0,0,0}; + if (alpha >= 0) { + VectorOperations::mulScalar(thirdAxisCross, gamma + alpha, + var1, 3); + VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); + VectorOperations::mulScalar(var2, beta, var2, 3); + VectorOperations::add(var1, var2, quat); + + for (int i = 0; i < 3; i++) { + initialQuaternion[i] = quat[i]; + } + initialQuaternion[3] = (gamma + alpha) + * (1 + + VectorOperations::dot(thirdAxis_B, + thirdAxis_J)); + VectorOperations::mulScalar(initialQuaternion, + constPlus, initialQuaternion, 4); + } else { + double diffGammaAlpha = gamma - alpha; + VectorOperations::mulScalar(thirdAxisCross, beta, var1, 3); + VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); + VectorOperations::mulScalar(var2, diffGammaAlpha, var2, 3); + VectorOperations::add(var1, var2, quat); + for (int i = 0; i < 3; i++) { + initialQuaternion[i] = quat[i]; + } + initialQuaternion[3] = beta + * (1 + + VectorOperations::dot(thirdAxis_B, + thirdAxis_J)); + VectorOperations::mulScalar(initialQuaternion, + constMin, initialQuaternion, 4); + + } + propagatedQuaternion[0] = initialQuaternion[0]; + propagatedQuaternion[1] = initialQuaternion[1]; + propagatedQuaternion[2] = initialQuaternion[2]; + propagatedQuaternion[3] = initialQuaternion[3]; + + /*Initial Covariance Matrix------------------------------------ */ + double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, + sunEstB[3] = {0,0,0}, magEstB[3] = {0,0,0}; + QuaternionOperations::toDcm(initialQuaternion, dcmBJ); + MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, + 1); + + double matrixSun[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, + 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); + MatrixOperations::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3); + + double partA[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, partB = 0; + MatrixOperations::multiplyScalar(*matrixSun, + weigthSun * weigthSun, *matrixSun, 3, 3); + MatrixOperations::multiplyScalar(*matrixMag, + weigthMag * weigthMag, *matrixMag, 3, 3); + MatrixOperations::add(*matrixSunMag, *matrixMagSun, *partA, 3, + 3); + double factor = weigthMag * weigthSun + * VectorOperations::dot(sunEstB, magEstB); + MatrixOperations::multiplyScalar(*partA, factor, *partA, 3, 3); + MatrixOperations::add(*partA, *matrixSun, *partA, 3, 3); + MatrixOperations::add(*partA, *matrixMag, *partA, 3, 3); + + double crossSunMag[3] = {0,0,0}; + double identityMatrix3[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; + VectorOperations::cross(sunEstB, magEstB, crossSunMag); + partB = + 1 + / (weigthMag * weigthSun + * pow( + VectorOperations::norm( + crossSunMag, 3), 2)); + MatrixOperations::multiplyScalar(*partA, partB, *partA, 3, 3); + MatrixOperations::add(*matrixSunMag, *identityMatrix3, *partA, 3, + 3); + factor = 1 / (weigthMag + weigthMag); + + double questCovMatrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + MatrixOperations::multiplyScalar(*partA, factor, + *questCovMatrix, 3, 3); + double initGyroCov[3][3] = {{pow(sigmaGyro,2),0,0},{0,pow(sigmaGyro,2),0}, + {0,0,pow(sigmaGyro,2)}}; + initialCovarianceMatrix[0][0] = questCovMatrix[0][0]; + initialCovarianceMatrix[0][1] = questCovMatrix[0][1]; + initialCovarianceMatrix[0][2] = questCovMatrix[0][2]; + initialCovarianceMatrix[0][3] = 0; + initialCovarianceMatrix[0][4] = 0; + initialCovarianceMatrix[0][5] = 0; + initialCovarianceMatrix[1][0] = questCovMatrix[1][0]; + initialCovarianceMatrix[1][1] = questCovMatrix[1][1]; + initialCovarianceMatrix[1][2] = questCovMatrix[1][2]; + initialCovarianceMatrix[1][3] = 0; + initialCovarianceMatrix[1][4] = 0; + initialCovarianceMatrix[1][5] = 0; + initialCovarianceMatrix[2][0] = questCovMatrix[2][0]; + initialCovarianceMatrix[2][1] = questCovMatrix[2][1]; + initialCovarianceMatrix[2][2] = questCovMatrix[2][2]; + initialCovarianceMatrix[2][3] = 0; + initialCovarianceMatrix[2][4] = 0; + initialCovarianceMatrix[2][5] = 0; + initialCovarianceMatrix[3][0] = 0; + initialCovarianceMatrix[3][1] = 0; + initialCovarianceMatrix[3][2] = 0; + initialCovarianceMatrix[3][3] = initGyroCov[0][0]; + initialCovarianceMatrix[3][4] = initGyroCov[0][1]; + initialCovarianceMatrix[3][5] = initGyroCov[0][2]; + initialCovarianceMatrix[4][0] = 0; + initialCovarianceMatrix[4][1] = 0; + initialCovarianceMatrix[4][2] = 0; + initialCovarianceMatrix[4][3] = initGyroCov[1][0]; + initialCovarianceMatrix[4][4] = initGyroCov[1][1]; + initialCovarianceMatrix[4][5] = initGyroCov[1][2]; + initialCovarianceMatrix[5][0] = 0; + initialCovarianceMatrix[5][1] = 0; + initialCovarianceMatrix[5][2] = 0; + 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; + } +} + +// --------------- MEKF DISCRETE TIME STEP ------------------------------- +ReturnValue_t MultiplicativeKalmanFilter::mekfEst( + const double *quaternionSTR, const bool *validSTR_, + const double *rateRMUs_, const bool *validRMUs_, + 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 *outputQuat, double *outputSatRate) { + + // Check for RMU Measurements + //AcsParameters mekfEstParams; + //loadAcsParameters(&mekfEstParams); + int MDF = 0; // Matrix Dimension Factor + if (!(*validRMUs_)){ + validMekf = false; + return KALMAN_NO_RMU_MEAS; + } + // Check for Model Calculations + else if (!(*validSSModel) || !(*validMagModel)){ + validMekf = false; + return KALMAN_NO_MODEL; + } + // Check Measurements available from SS, MAG, STR + if (*validSS && *validMagField_ && *validSTR_){ + sensorsAvail = 1; + MDF = 9; + } + else if (*validSS && *validMagField_ && !(*validSTR_)){ + sensorsAvail = 2; + MDF = 6; + } + else if (*validSS && !(*validMagField_) && *validSTR_){ + sensorsAvail = 3; + MDF = 6; + } + else if (!(*validSS) && *validMagField_ && *validSTR_){ + sensorsAvail = 4; + MDF = 6; + } + else if (*validSS && !(*validMagField_) && !(*validSTR_)){ + sensorsAvail = 5; + MDF = 3; + } + else if (!(*validSS) && *validMagField_ && !(*validSTR_)){ + sensorsAvail = 6; + MDF = 3; + } + else if (!(*validSS) && !(*validMagField_) && *validSTR_){ + sensorsAvail = 7; + MDF = 3; + } + else{ + sensorsAvail = 8; //no measurements + validMekf = false; + return RETURN_FAILED; + } + + // If we are here, MEKF will perform + double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; + sigmaSun = kalmanFilterParameters->sensorNoiseSS; + sigmaMag = kalmanFilterParameters->sensorNoiseMAG; + sigmaStr = kalmanFilterParameters->sensorNoiseSTR; + + double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0}, + normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0}; + VectorOperations::normalize(magneticField_, normMagB, 3); // b2 + VectorOperations::normalize(sunDir_, normSunB, 3); //b1 + VectorOperations::normalize(magFieldJ, normMagJ, 3); //r2 + VectorOperations::normalize(sunDirJ, normSunJ, 3); //r1 + + /*-------GAIN - UPDATE - STEP------*/ + double covMatrix11[3][3] = {{pow(sigmaSun,2),0,0},{0,pow(sigmaSun,2),0},{0,0,pow(sigmaSun,2)}}; + double covMatrix22[3][3] = {{pow(sigmaMag,2),0,0},{0,pow(sigmaMag,2),0},{0,0,pow(sigmaMag,2)}}; + double covMatrix33[3][3] = {{pow(sigmaStr,2),0,0},{0,pow(sigmaStr,2),0},{0,0,pow(sigmaStr,2)}}; + + double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, + sunEstB[3] = {0,0,0}, magEstB[3] = {0,0,0}; + QuaternionOperations::toDcm(propagatedQuaternion, dcmBJ); + MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, + 1); + double quatEstErr[3] = {0,0,0}; + + //Measurement Sensitivity Matrix + double measSensMatrix11[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; // ss + double measSensMatrix22[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; // mag + double measSensMatrix33[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // str + MathOperations::skewMatrix(sunEstB, *measSensMatrix11); + MathOperations::skewMatrix(magEstB, *measSensMatrix22); + + double measVecQuat[3] = {0,0,0}; + if (*validSTR_){ + double quatError[4] = {0,0,0,0}; + double invPropagatedQuat[4] = {0,0,0,0}; + QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat); + QuaternionOperations::multiply(quaternionSTR, invPropagatedQuat, quatError); + for (int i = 0 ; i < 3;i++){ + measVecQuat[i]=2*quatError[i]/quatError[3]; + } + } + // Adjust matrices based on valid measurements + double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF], + measEstVec[MDF]; + if (sensorsAvail==1){ //All measurements valid + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix22[0][0]; + measSensMatrix[3][1] = measSensMatrix22[0][1]; + measSensMatrix[3][2] = measSensMatrix22[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix22[1][0]; + measSensMatrix[4][1] = measSensMatrix22[1][1]; + measSensMatrix[4][2] = measSensMatrix22[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix22[2][0]; + measSensMatrix[5][1] = measSensMatrix22[2][1]; + measSensMatrix[5][2] = measSensMatrix22[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + measSensMatrix[6][0] = measSensMatrix33[0][0]; + measSensMatrix[6][1] = measSensMatrix33[0][1]; + measSensMatrix[6][2] = measSensMatrix33[0][2]; + measSensMatrix[6][3] = 0; + measSensMatrix[6][4] = 0; + measSensMatrix[6][5] = 0; + measSensMatrix[7][0] = measSensMatrix33[1][0]; + measSensMatrix[7][1] = measSensMatrix33[1][1]; + measSensMatrix[7][2] = measSensMatrix33[1][2]; + measSensMatrix[7][3] = 0; + measSensMatrix[7][4] = 0; + measSensMatrix[7][5] = 0; + measSensMatrix[8][0] = measSensMatrix33[2][0]; + measSensMatrix[8][1] = measSensMatrix33[2][1]; + measSensMatrix[8][2] = measSensMatrix33[2][2]; + measSensMatrix[8][3] = 0; + measSensMatrix[8][4] = 0; + measSensMatrix[8][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[0][6] = 0; + measCovMatrix[0][7] = 0; + measCovMatrix[0][8] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[1][6] = 0; + measCovMatrix[1][7] = 0; + measCovMatrix[1][8] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[2][6] = 0; + measCovMatrix[2][7] = 0; + measCovMatrix[2][8] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix22[0][0]; + measCovMatrix[3][4] = covMatrix22[0][1]; + measCovMatrix[3][5] = covMatrix22[0][2]; + measCovMatrix[3][6] = 0; + measCovMatrix[3][7] = 0; + measCovMatrix[3][8] = 0; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix22[1][0]; + measCovMatrix[4][4] = covMatrix22[1][1]; + measCovMatrix[4][5] = covMatrix22[1][2]; + measCovMatrix[4][6] = 0; + measCovMatrix[4][7] = 0; + measCovMatrix[4][8] = 0; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix22[2][0]; + measCovMatrix[5][4] = covMatrix22[2][1]; + measCovMatrix[5][5] = covMatrix22[2][2]; + measCovMatrix[5][6] = 0; + measCovMatrix[5][7] = 0; + measCovMatrix[5][8] = 0; + measCovMatrix[6][0] = 0; + measCovMatrix[6][1] = 0; + measCovMatrix[6][2] = 0; + measCovMatrix[6][3] = 0; + measCovMatrix[6][4] = 0; + measCovMatrix[6][5] = 0; + measCovMatrix[6][6] = covMatrix33[0][0]; + measCovMatrix[6][7] = covMatrix33[0][1]; + measCovMatrix[6][8] = covMatrix33[0][2]; + measCovMatrix[7][0] = 0; + measCovMatrix[7][1] = 0; + measCovMatrix[7][2] = 0; + measCovMatrix[7][3] = 0; + measCovMatrix[7][4] = 0; + measCovMatrix[7][5] = 0; + measCovMatrix[7][6] = covMatrix33[1][0]; + measCovMatrix[7][7] = covMatrix33[1][1]; + measCovMatrix[7][8] = covMatrix33[1][2]; + measCovMatrix[8][0] = 0; + measCovMatrix[8][1] = 0; + measCovMatrix[8][2] = 0; + measCovMatrix[8][3] = 0; + measCovMatrix[8][4] = 0; + measCovMatrix[8][5] = 0; + measCovMatrix[8][6] = covMatrix33[2][0]; + measCovMatrix[8][7] = covMatrix33[2][1]; + measCovMatrix[8][8] = covMatrix33[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = normMagB[0]; + measVec[4] = normMagB[1]; + measVec[5] = normMagB[2]; + measVec[6] = measVecQuat[0]; + measVec[7] = measVecQuat[1]; + measVec[8] = measVecQuat[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = magEstB[0]; + measEstVec[4] = magEstB[1]; + measEstVec[5] = magEstB[2]; + measEstVec[6] = quatEstErr[0]; + measEstVec[7] = quatEstErr[1]; + measEstVec[8] = quatEstErr[2]; + + } + else if(sensorsAvail==2){ // ss / mag valid + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix22[0][0]; + measSensMatrix[3][1] = measSensMatrix22[0][1]; + measSensMatrix[3][2] = measSensMatrix22[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix22[1][0]; + measSensMatrix[4][1] = measSensMatrix22[1][1]; + measSensMatrix[4][2] = measSensMatrix22[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix22[2][0]; + measSensMatrix[5][1] = measSensMatrix22[2][1]; + measSensMatrix[5][2] = measSensMatrix22[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix22[0][0]; + measCovMatrix[3][4] = covMatrix22[0][1]; + measCovMatrix[3][5] = covMatrix22[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix22[1][0]; + measCovMatrix[4][4] = covMatrix22[1][1]; + measCovMatrix[4][5] = covMatrix22[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix22[2][0]; + measCovMatrix[5][4] = covMatrix22[2][1]; + measCovMatrix[5][5] = covMatrix22[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = normMagB[0]; + measVec[4] = normMagB[1]; + measVec[5] = normMagB[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = magEstB[0]; + measEstVec[4] = magEstB[1]; + measEstVec[5] = magEstB[2]; + + } + else if(sensorsAvail==3){ // ss / str valid + + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix33[0][0]; + measSensMatrix[3][1] = measSensMatrix33[0][1]; + measSensMatrix[3][2] = measSensMatrix33[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix33[1][0]; + measSensMatrix[4][1] = measSensMatrix33[1][1]; + measSensMatrix[4][2] = measSensMatrix33[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix33[2][0]; + measSensMatrix[5][1] = measSensMatrix33[2][1]; + measSensMatrix[5][2] = measSensMatrix33[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix33[0][0]; + measCovMatrix[3][4] = covMatrix33[0][1]; + measCovMatrix[3][5] = covMatrix33[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix33[1][0]; + measCovMatrix[4][4] = covMatrix33[1][1]; + measCovMatrix[4][5] = covMatrix33[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix33[2][0]; + measCovMatrix[5][4] = covMatrix33[2][1]; + measCovMatrix[5][5] = covMatrix33[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = measVecQuat[0]; + measVec[4] = measVecQuat[1]; + measVec[5] = measVecQuat[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = quatEstErr[0]; + measEstVec[4] = quatEstErr[1]; + measEstVec[5] = quatEstErr[2]; + + } + else if (sensorsAvail == 4){ // mag / str avail + + measSensMatrix[0][0] = measSensMatrix22[0][0]; + measSensMatrix[0][1] = measSensMatrix22[0][1]; + measSensMatrix[0][2] = measSensMatrix22[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix22[1][0]; + measSensMatrix[1][1] = measSensMatrix22[1][1]; + measSensMatrix[1][2] = measSensMatrix22[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix22[2][0]; + measSensMatrix[2][1] = measSensMatrix22[2][1]; + measSensMatrix[2][2] = measSensMatrix22[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix33[0][0]; + measSensMatrix[3][1] = measSensMatrix33[0][1]; + measSensMatrix[3][2] = measSensMatrix33[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix33[1][0]; + measSensMatrix[4][1] = measSensMatrix33[1][1]; + measSensMatrix[4][2] = measSensMatrix33[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix33[2][0]; + measSensMatrix[5][1] = measSensMatrix33[2][1]; + measSensMatrix[5][2] = measSensMatrix33[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix22[0][0]; + measCovMatrix[0][1] = covMatrix22[0][1]; + measCovMatrix[0][2] = covMatrix22[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix22[1][0]; + measCovMatrix[1][1] = covMatrix22[1][1]; + measCovMatrix[1][2] = covMatrix22[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix22[2][0]; + measCovMatrix[2][1] = covMatrix22[2][1]; + measCovMatrix[2][2] = covMatrix22[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix33[0][0]; + measCovMatrix[3][4] = covMatrix33[0][1]; + measCovMatrix[3][5] = covMatrix33[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix33[1][0]; + measCovMatrix[4][4] = covMatrix33[1][1]; + measCovMatrix[4][5] = covMatrix33[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix33[2][0]; + measCovMatrix[5][4] = covMatrix33[2][1]; + measCovMatrix[5][5] = covMatrix33[2][2]; + + measVec[0] = normMagB[0]; + measVec[1] = normMagB[1]; + measVec[2] = normMagB[2]; + measVec[3] = measVecQuat[0]; + measVec[4] = measVecQuat[1]; + measVec[5] = measVecQuat[2]; + + measEstVec[0] = magEstB[0]; + measEstVec[1] = magEstB[1]; + measEstVec[2] = magEstB[2]; + measEstVec[3] = quatEstErr[0]; + measEstVec[4] = quatEstErr[1]; + measEstVec[5] = quatEstErr[2]; + + } + else if (sensorsAvail == 5){ // only ss + + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + + } + else if (sensorsAvail == 6){ // only mag + + measSensMatrix[0][0] = measSensMatrix22[0][0]; + measSensMatrix[0][1] = measSensMatrix22[0][1]; + measSensMatrix[0][2] = measSensMatrix22[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix22[1][0]; + measSensMatrix[1][1] = measSensMatrix22[1][1]; + measSensMatrix[1][2] = measSensMatrix22[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix22[2][0]; + measSensMatrix[2][1] = measSensMatrix22[2][1]; + measSensMatrix[2][2] = measSensMatrix22[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix22[0][0]; + measCovMatrix[0][1] = covMatrix22[0][1]; + measCovMatrix[0][2] = covMatrix22[0][2]; + measCovMatrix[1][0] = covMatrix22[1][0]; + measCovMatrix[1][1] = covMatrix22[1][1]; + measCovMatrix[1][2] = covMatrix22[1][2]; + measCovMatrix[2][0] = covMatrix22[2][0]; + measCovMatrix[2][1] = covMatrix22[2][1]; + measCovMatrix[2][2] = covMatrix22[2][2]; + + measVec[0] = normMagB[0]; + measVec[1] = normMagB[1]; + measVec[2] = normMagB[2]; + + measEstVec[0] = magEstB[0]; + measEstVec[1] = magEstB[1]; + measEstVec[2] = magEstB[2]; + + } + else if (sensorsAvail == 7){ // only str + + measSensMatrix[0][0] = measSensMatrix33[0][0]; + measSensMatrix[0][1] = measSensMatrix33[0][1]; + measSensMatrix[0][2] = measSensMatrix33[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix33[1][0]; + measSensMatrix[1][1] = measSensMatrix33[1][1]; + measSensMatrix[1][2] = measSensMatrix33[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix33[2][0]; + measSensMatrix[2][1] = measSensMatrix33[2][1]; + measSensMatrix[2][2] = measSensMatrix33[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix33[0][0]; + measCovMatrix[0][1] = covMatrix33[0][1]; + measCovMatrix[0][2] = covMatrix33[0][2]; + measCovMatrix[1][0] = covMatrix33[1][0]; + measCovMatrix[1][1] = covMatrix33[1][1]; + measCovMatrix[1][2] = covMatrix33[1][2]; + measCovMatrix[2][0] = covMatrix33[2][0]; + measCovMatrix[2][1] = covMatrix33[2][1]; + measCovMatrix[2][2] = covMatrix33[2][2]; + + measVec[0] = measVecQuat[0]; + measVec[1] = measVecQuat[1]; + measVec[2] = measVecQuat[2]; + + measEstVec[0] = quatEstErr[0]; + measEstVec[1] = quatEstErr[1]; + measEstVec[2] = quatEstErr[2]; + + } + // Kalman Gain: [K = P * H' / (H * P * H' + R)] + double kalmanGain[6][MDF] = {{0}}, kalmanGain1[6][MDF] = {{0}}; + + double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF], + residualCov1[6][MDF]; + // H * P * H' + MatrixOperations::transpose(*measSensMatrix,*measSensMatrixTrans,6,MDF); + MatrixOperations::multiply(*initialCovarianceMatrix,*measSensMatrixTrans,*residualCov1,6,6,MDF); + MatrixOperations::multiply(*measSensMatrix,*residualCov1,*residualCov,MDF,6,MDF); + // negative values, restrictions ? + // (H * P * H' + R) + MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); + // <> + double invResidualCov1[MDF] = {0}; + double invResidualCov[MDF][MDF] = {{0}}; + int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov,*invResidualCov,invResidualCov1,MDF); + if(inversionFailed) + { + validMekf = false; + return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed + } + + + // [K = P * H' / (H * P * H' + R)] + MatrixOperations::multiply(*measSensMatrixTrans,*invResidualCov,*kalmanGain1,6,MDF,MDF); + MatrixOperations::multiply(*initialCovarianceMatrix,*kalmanGain1,*kalmanGain,6,6,MDF); + + /* ------- UPDATE -STEP ---------*/ + + // Update Covariance Matrix: P_plus = (I-K*H)*P_min + double covMatPlus[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 identityMatrix[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 updateCov1[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}}; + MatrixOperations::multiply(*kalmanGain,*measSensMatrix,*updateCov1,6,MDF,MDF); + MatrixOperations::subtract(*identityMatrix,*updateCov1,*updateCov1,6,6); + MatrixOperations::multiply(*updateCov1,*initialCovarianceMatrix,*covMatPlus,6,6,6); + + // Error State Vector + double errStateVec[6] = {0,0,0,0,0,0}; + double errStateVec1[MDF] = {0}; + VectorOperations::subtract(measVec,measEstVec,errStateVec1,MDF); + MatrixOperations::multiply(*kalmanGain,errStateVec1,errStateVec,6,MDF,1); + + double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]}; + double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]}; + + // State Vector Elements + double xi1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, + xi2[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + MathOperations::skewMatrix(propagatedQuaternion, *xi2); + double identityMatrix3[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; + MatrixOperations::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); + MatrixOperations::add(*xi1, *xi2, *xi1, 3, 3); + double xi[4][3] = {{xi1[0][0],xi1[0][1],xi1[0][2]}, + {xi1[1][0],xi1[1][1],xi1[1][2]}, + {xi1[2][0],xi1[2][1],xi1[2][2]}, + {-propagatedQuaternion[0],-propagatedQuaternion[1],-propagatedQuaternion[2]}}; + + double errQuatTerm[4] = {0,0,0,0}; + MatrixOperations::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1); + VectorOperations::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4); + VectorOperations::add(propagatedQuaternion, errQuatTerm, quatBJ, 4); + VectorOperations::normalize(quatBJ, quatBJ, 4); + + outputQuat[0] = quatBJ[0]; + outputQuat[1] = quatBJ[1]; + outputQuat[2] = quatBJ[2]; + outputQuat[3] = quatBJ[3]; + + double updatedGyroBias[3] = {0,0,0}; + VectorOperations::add(biasRMU, errStateGyroBias, updatedGyroBias, 3); + // Bias RMU State + biasRMU[0] = updatedGyroBias[0]; + biasRMU[1] = updatedGyroBias[1]; + biasRMU[2] = updatedGyroBias[2]; + + + /* ----------- 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 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}; + VectorOperations::subtract(rateRMUs_, 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}}; + + // Corrected Sat Rate via Bias + outputSatRate[0] = rotRateEst[0]; + outputSatRate[1] = rotRateEst[1]; + outputSatRate[2] = rotRateEst[2]; + + // 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); + + 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]; + } + 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 covQ21[3][3], covQ22[3][3], covQ23[3][3]; + double fact4 = (0.5*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::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]; + } + + // 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}}, + 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 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); + + 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]; + + // Propagated Quaternion + double rotSin[3] = {0,0,0}, + omega1[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); + + 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]}, + {-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]; + 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); + validMekf = true; + + + // Discrete Time Step + + // Check for new data in measurement -> SensorProcessing ? + + return RETURN_OK; +} diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h new file mode 100644 index 00000000..d4370ad3 --- /dev/null +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -0,0 +1,107 @@ +/* + * MultiplicativeKalmanFilter.h + * + * Created on: 4 Feb 2022 + * Author: Robin Marquardt + * + * @brief: This class handles the calculation of an estimated quaternion and the gyro bias by + * means of the spacecraft attitude sensors + * + * @note: A description of the used algorithms can be found in the bachelor thesis of Robin Marquardt + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811 + */ + +#ifndef MULTIPLICATIVEKALMANFILTER_H_ +#define MULTIPLICATIVEKALMANFILTER_H_ + +#include //uint8_t +#include /*purpose, timeval ?*/ +#include "acs/config/classIds.h" +//#include <_timeval.h> + +#include "AcsParameters.h" + +class MultiplicativeKalmanFilter : public HasReturnvaluesIF { +public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + MultiplicativeKalmanFilter(AcsParameters *acsParameters_); + virtual ~MultiplicativeKalmanFilter(); + + void reset(); // NOT YET DEFINED - should only reset all mekf variables + + /* @brief: init() - This function initializes the Kalman Filter and will provide the first quaternion through + * the QUEST algorithm + * @param: magneticField_ magnetic field vector in the body frame + * sunDir_ sun direction vector in the body frame + * sunDirJ sun direction vector in the ECI frame + * magFieldJ magnetic field vector in the ECI frame + */ + void init(const double *magneticField_, const bool *validMagField_, + const double *sunDir_, const bool *validSS, + const double *sunDirJ, const bool *validSSModel, + const double *magFieldJ,const bool *validMagModel); + + /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter for the current step + * after the initalization + * @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame + * rateRMUs_ Estimated satellite rotation rate from the Rate Measurement Units [rad/s] + * magneticField_ magnetic field vector in the body frame + * sunDir_ sun direction vector in the body frame + * sunDirJ sun direction vector in the ECI frame + * magFieldJ magnetic field vector in the ECI frame + * outputQuat Stores the calculated quaternion + * outputSatRate Stores the adjusted satellite rate + * @return ReturnValue_t Feedback of this class, KALMAN_NO_RMU_MEAS if no satellite rate from the sensors was provided, + * KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model calculations, + * KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible, + * RETURN_OK else + */ + ReturnValue_t mekfEst( + const double *quaternionSTR, const bool *validSTR_, + const double *rateRMUs_, const bool *validRMUs_, + 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 *outputQuat, double *outputSatRate); + + + // Declaration of Events (like init) and memberships + //static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND (/config/returnvalues/classIDs.h) + //static const Event RESET = MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be + // resetting Mekf + static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN; + static const ReturnValue_t KALMAN_NO_RMU_MEAS = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03); + +private: +/*Parameters*/ + AcsParameters::InertiaEIVE* inertiaEIVE; + AcsParameters::KalmanFilterParameters* kalmanFilterParameters; + double quaternion_STR_SB[4]; + bool validInit; + double sampleTime = 0.1; + + +/*States*/ + double initialQuaternion[4]; /*after reset?QUEST*/ + double initialCovarianceMatrix[6][6];/*after reset?QUEST*/ + double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ + bool validMekf; + uint8_t sensorsAvail; + +/*Outputs*/ + double quatBJ[4]; /* Output Quaternion */ + double biasRMU[3]; /*Between measured and estimated sat Rate*/ +/*Parameter INIT*/ + //double alpha, gamma, beta; +/*Functions*/ + void loadAcsParameters(AcsParameters *acsParameters_); +}; + + + +#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ From a95f7ff8b013d48a3953d243fef0ef720604a7a4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 14:36:08 +0200 Subject: [PATCH 007/244] added CholeskyDecomposition --- .../acs/MultiplicativeKalmanFilter.cpp | 4 +- .../acs/util/CholeskyDecomposition.h | 102 ++++++++++++++++++ 2 files changed, 104 insertions(+), 2 deletions(-) create mode 100644 mission/controller/acs/util/CholeskyDecomposition.h diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5d895566..5a546409 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -12,8 +12,8 @@ #include #include #include "MultiplicativeKalmanFilter.h" -#include -#include +#include +#include /*Initialisation of values for parameters in constructor*/ MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) : diff --git a/mission/controller/acs/util/CholeskyDecomposition.h b/mission/controller/acs/util/CholeskyDecomposition.h new file mode 100644 index 00000000..38ce0bcc --- /dev/null +++ b/mission/controller/acs/util/CholeskyDecomposition.h @@ -0,0 +1,102 @@ +/* + * TinyEKF: Extended Kalman Filter for embedded processors + * + * Copyright (C) 2015 Simon D. Levy + * + * MIT License + */ +#ifndef CHOLESKYDECOMPOSITION_H_ +#define CHOLESKYDECOMPOSITION_H_ +#include +//typedef unsigned int uint8_t; + +template +class CholeskyDecomposition { +public: + static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) + { + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + return cholsl(matrix, result, tempMatrix, dimension); + } +private: + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t choldc1(double * a, double * p, uint8_t n) { + int8_t i,j,k; + double sum; + + for (i = 0; i < n; i++) { + for (j = i; j < n; j++) { + sum = a[i*n+j]; + for (k = i - 1; k >= 0; k--) { + sum -= a[i*n+k] * a[j*n+k]; + } + if (i == j) { + if (sum <= 0) { + return 1; /* error */ + } + p[i] = sqrt(sum); + } + else { + a[j*n+i] = sum / p[i]; + } + } + } + + return 0; /* success */ + } + + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t choldcsl(double * A, double * a, double * p, uint8_t n) + { + uint8_t i,j,k; double sum; + for (i = 0; i < n; i++) + for (j = 0; j < n; j++) + a[i*n+j] = A[i*n+j]; + if (choldc1(a, p, n)) return 1; + for (i = 0; i < n; i++) { + a[i*n+i] = 1 / p[i]; + for (j = i + 1; j < n; j++) { + sum = 0; + for (k = i; k < j; k++) { + sum -= a[j*n+k] * a[k*n+i]; + } + a[j*n+i] = sum / p[j]; + } + } + + return 0; /* success */ + } + + + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t cholsl(double * A,double * a,double * p, uint8_t n) + { + uint8_t i,j,k; + if (choldcsl(A,a,p,n)) return 1; + for (i = 0; i < n; i++) { + for (j = i + 1; j < n; j++) { + a[i*n+j] = 0.0; + } + } + for (i = 0; i < n; i++) { + a[i*n+i] *= a[i*n+i]; + for (k = i + 1; k < n; k++) { + a[i*n+i] += a[k*n+i] * a[k*n+i]; + } + for (j = i + 1; j < n; j++) { + for (k = j; k < n; k++) { + a[i*n+j] += a[k*n+i] * a[k*n+j]; + } + } + } + for (i = 0; i < n; i++) { + for (j = 0; j < i; j++) { + a[i*n+j] = a[j*n+i]; + } + } + + return 0; /* success */ + } +}; + +#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */ From 338c8299e2ea85ee3d30717d91c2cf2a702fc254 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 15:03:25 +0200 Subject: [PATCH 008/244] added Commanding as ActuatorCmd --- mission/controller/acs/ActuatorCmd.cpp | 83 ++++++++++++++++++++++++++ mission/controller/acs/ActuatorCmd.h | 48 +++++++++++++++ 2 files changed, 131 insertions(+) create mode 100644 mission/controller/acs/ActuatorCmd.cpp create mode 100644 mission/controller/acs/ActuatorCmd.h diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp new file mode 100644 index 00000000..25eeca9c --- /dev/null +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -0,0 +1,83 @@ +/* + * ActuatorCmd.cpp + * + * Created on: 4 Aug 2022 + * Author: Robin Marquardt + */ + + +#include "ActuatorCmd.h" +#include +#include +#include +#include +#include +#include +#include + +ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { + acsParameters = *acsParameters_; +} + +ActuatorCmd::~ActuatorCmd(){ + +} + +void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2, + const double *speedRw3, const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed){ + + using namespace Math; + // 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 (maxValue > maxTrq) { + + double scalingFactor = maxTrq / maxValue; + VectorOperations::mulScalar(torque, scalingFactor, torque, 4); + + } + + // Calculating the commanded speed in RPM for every reaction wheel + double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double deltaSpeed[4] = {0, 0, 0, 0}; + double commandTime = acsParameters.onBoardParams.sampleTime, + inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; + double radToRpm = 60 / (2 * PI); // factor for conversion to RPM + // W_RW = Torque_RW / I_RW * delta t [rad/s] + double factor = commandTime / inertiaWheel * radToRpm; + VectorOperations::mulScalar(torque, factor, deltaSpeed, 4); + VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); + + + +} + +void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) { + +// Convert to Unit frame + MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, dipolMoment, dipolMomentUnits, 3, 3, 1); +// Scaling along largest element if dipol exceeds maximum + double maxDipol = acsParameters.magnetorquesParameter.DipolMax; + double maxValue = 0; + for (int i = 0; i < 3; i++) { + if (abs(dipolMomentUnits[i]) > maxDipol) { + maxValue = abs(dipolMomentUnits[i]); + } + } + + if (maxValue > maxDipol) { + + double scalingFactor = maxDipol / maxValue; + VectorOperations::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3); + + } +} diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h new file mode 100644 index 00000000..9b721505 --- /dev/null +++ b/mission/controller/acs/ActuatorCmd.h @@ -0,0 +1,48 @@ +/* + * ActuatorCmd.h + * + * Created on: 4 Aug 2022 + * Author: Robin Marquardt + */ + +#ifndef ACTUATORCMD_H_ +#define ACTUATORCMD_H_ + + +#include "AcsParameters.h" +#include "SensorProcessing.h" +#include +#include +#include + +class ActuatorCmd{ +public: + ActuatorCmd(AcsParameters *acsParameters_); //Input mode ? + virtual ~ActuatorCmd(); + + /* + * @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 double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, + const double *rwTrqIn, const double *rwTrqNS, 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); + +protected: + +private: + AcsParameters acsParameters; +}; + +#endif /* ACTUATORCMD_H_ */ From 053c1a7fe8f7385370eedd894b1ef1d2f399d359 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 15:17:39 +0200 Subject: [PATCH 009/244] added Guidance --- mission/controller/acs/Guidance.cpp | 347 ++++++++++++++++++++++++++++ mission/controller/acs/Guidance.h | 42 ++++ 2 files changed, 389 insertions(+) create mode 100644 mission/controller/acs/Guidance.cpp create mode 100644 mission/controller/acs/Guidance.h diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp new file mode 100644 index 00000000..d861769d --- /dev/null +++ b/mission/controller/acs/Guidance.cpp @@ -0,0 +1,347 @@ +/* + * Guidance.cpp + * + * Created on: 6 Jun 2022 + * Author: Robin Marquardt + */ + + +#include "Guidance.h" +#include "string.h" +#include +#include +#include +#include +#include +#include + +Guidance::Guidance(AcsParameters *acsParameters_) { + acsParameters = *acsParameters_; + +} + +Guidance::~Guidance() { + +} + +void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { + + for (int i = 0; i < 3; i++) { + + sunTargetSafe[i] = + acsParameters.safeModeControllerParameters.sunTargetDir[i]; + satRateSafe[i] = + acsParameters.safeModeControllerParameters.satRateRef[i]; + + } + + +// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); + +} + +void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]){ +//------------------------------------------------------------------------------------- +// Calculation of target quaternion to groundstation +//------------------------------------------------------------------------------------- +// Transform longitude, latitude and altitude of groundstation 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}; + MathOperations::cartesianFromLatLongAlt(sensorValues->gps0latitude, sensorValues->gps0longitude, + sensorValues->gps0altitude, posSatE); + +// Target direction in the ECEF frame + 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}}; + 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}}; + 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 + 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.refDirection[0]; + refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; + double noramlizedTargetDirB[3] = {0, 0, 0}; + VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); + VectorOperations::normalize(refDir, refDir, 3); + 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 +//------------------------------------------------------------------------------------- + double velSatE[3] = {0, 0, 0}; + velSatE[0] = sensorValues->gps0Velocity[0]; + velSatE[1] = sensorValues->gps0Velocity[1]; + velSatE[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); + double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); + MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); + VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); + + double normVelSatB = VectorOperations::norm(velSatB, 3); + double normRefSatRate = normVelSatB / normTargetDirB; + + double satRateDir[3] = {0, 0, 0}; + VectorOperations::cross(velSatB, targetDirB, satRateDir); + VectorOperations::normalize(satRateDir, satRateDir, 3); + VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); + +//------------------------------------------------------------------------------------- +// 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 ) { + + sunDirJ[0] = outputValues->sunDirModel[0]; + sunDirJ[1] = outputValues->sunDirModel[1]; + sunDirJ[2] = outputValues->sunDirModel[2]; + MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); + } + + else { + sunDirB[0] = outputValues->sunDirEst[0]; + sunDirB[1] = outputValues->sunDirEst[1]; + sunDirB[2] = outputValues->sunDirEst[2]; + + } + + double exclAngle = acsParameters.strParameters.exclusionAngle, + blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, + blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; + + double sightAngleSun = VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); + + if ( !(strBlindAvoidFlag) ) { + + double critSightAngle = blindStart * exclAngle; + + if ( sightAngleSun < critSightAngle) { + strBlindAvoidFlag = true; + } + + } + + else { + if ( sightAngleSun < blindEnd * exclAngle) { + + double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; + double blindRefRate[3] = {0, 0, 0}; + + + if ( sunDirB[1] < 0) { + blindRefRate[0] = normBlindRefRate; + blindRefRate[1] = 0; + blindRefRate[2] = 0; + } + else { + blindRefRate[0] = -normBlindRefRate; + blindRefRate[1] = 0; + blindRefRate[2] = 0; + } + + VectorOperations::add(blindRefRate, refSatRate, refSatRate, 3); + + } + else { + strBlindAvoidFlag = false; + } + } + } +} + + +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}; + 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}; + satRate[0] = outputValues->satRateMekf[0]; + satRate[1] = outputValues->satRateMekf[1]; + satRate[2] = outputValues->satRateMekf[2]; + VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); + // valid checks ? + double quatErrorMtx[4][4] = {{ quatRef[3], quatRef[2], -quatRef[1], -quatRef[0] }, + { -quatRef[2], quatRef[3], quatRef[0], -quatRef[1] }, + { 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) { + quatErrorComplete[3] *= -1; + } + + quatError[0] = quatErrorComplete[0]; + 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 ?? + +} + + +void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv) { + + if (sensorValues->validRw0 && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) { + + rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; + + } + + else if (!(sensorValues->validRw0) && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) { + + rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2]; + } + + else if ((sensorValues->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) { + + rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2]; + } + + else if ((sensorValues->validRw0) && (sensorValues->validRw1) && !(sensorValues->validRw2) && sensorValues->validRw3) { + + rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2]; + } + + else if ((sensorValues->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && (sensorValues->validRw3)) { + + rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2]; + } + + else { +// @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. + rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; + + } +} diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h new file mode 100644 index 00000000..736d8b9b --- /dev/null +++ b/mission/controller/acs/Guidance.h @@ -0,0 +1,42 @@ +/* + * Guidance.h + * + * Created on: 6 Jun 2022 + * Author: Robin Marquardt + */ + +#ifndef GUIDANCE_H_ +#define GUIDANCE_H_ + + +#include +#include +#include +#include + + +class Guidance { +public: + Guidance(AcsParameters *acsParameters_); + virtual ~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 targetQuatPtg(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] ); + +// @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); + + +private: + AcsParameters acsParameters; + bool strBlindAvoidFlag = false; +}; + +#endif /* ACS_GUIDANCE_H_ */ From 84b0856b52ee4480ca58e22875175100640487fc Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 15:26:51 +0200 Subject: [PATCH 010/244] added OutputValues --- mission/controller/acs/OutputValues.cpp | 19 +++++++++ mission/controller/acs/OutputValues.h | 51 +++++++++++++++++++++++++ 2 files changed, 70 insertions(+) create mode 100644 mission/controller/acs/OutputValues.cpp create mode 100644 mission/controller/acs/OutputValues.h diff --git a/mission/controller/acs/OutputValues.cpp b/mission/controller/acs/OutputValues.cpp new file mode 100644 index 00000000..3deb4043 --- /dev/null +++ b/mission/controller/acs/OutputValues.cpp @@ -0,0 +1,19 @@ +/* + * OutputValues.cpp + * + * Created on: 30 Mar 2022 + * Author: rooob + */ +#include + +namespace ACS { + +OutputValues::OutputValues(){ + +} + +OutputValues::~OutputValues(){ + +} + +} diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h new file mode 100644 index 00000000..a73dc43c --- /dev/null +++ b/mission/controller/acs/OutputValues.h @@ -0,0 +1,51 @@ +/* + * OutputValues.h + * + * Created on: 30 Mar 2022 + * Author: rooob + */ + +#ifndef OUTPUTVALUES_H_ +#define OUTPUTVALUES_H_ + + +namespace ACS { + +class OutputValues { + +public: + + OutputValues(); + virtual ~OutputValues(); + + double magFieldEst[3]; // sensor fusion (G) + bool magFieldEstValid; + double magFieldModel[3]; //igrf (IJK) + bool magFieldModelValid; + double magneticFieldVectorDerivative[3]; + bool magneticFieldVectorDerivativeValid; + + bool mgmUpdated; + + double sunDirEst[3]; // sensor fusion (G) + bool sunDirEstValid; + double sunDirModel[3]; //sun model (IJK) + bool sunDirModelValid; + + double quatMekfBJ[4]; //mekf + bool quatMekfBJValid; + + double satRateEst[3]; + bool satRateEstValid; + double satRateMekf[3]; // after mekf with correction of bias + bool satRateMekfValid; + double sunVectorDerivative[3]; + bool sunVectorDerivativeValid; + + double gcLatitude; // geocentric latitude, radian + double gdLongitude; // Radian longitude +}; +} + + +#endif OUTPUTVALUES_H_ From 295a2e3fdc2dab0066c9d12477b677bc72f5fc22 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 15:37:05 +0200 Subject: [PATCH 011/244] added SensorValues --- mission/controller/acs/SensorValues.cpp | 35 +++++++++ mission/controller/acs/SensorValues.h | 99 +++++++++++++++++++++++++ 2 files changed, 134 insertions(+) create mode 100644 mission/controller/acs/SensorValues.cpp create mode 100644 mission/controller/acs/SensorValues.h diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp new file mode 100644 index 00000000..f3a52dea --- /dev/null +++ b/mission/controller/acs/SensorValues.cpp @@ -0,0 +1,35 @@ +/* + * SensorValues.cpp + * + * Created on: 30 Mar 2022 + * Author: rooob + */ +#include + +#include +#include +#include + +namespace ACS { + +SensorValues::SensorValues() { +} + +SensorValues::~SensorValues() { +} + +ReturnValue_t SensorValues::update() { +// lp_var_t quaternion(objects::STAR_TRACKER, PoolIds::CALI_QW, nullptr, pool_rwm_t::VAR_READ); +// ReturnValue_t result = quaternion.read(); + +// if ( result != RETURN_OK) { +// return RETURN_FAILED; +// } +// quatJB[3] = static_cast(quaternion.value); +// quatJBValid = quaternion.isValid(); + + return RETURN_OK; +} +} + + diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h new file mode 100644 index 00000000..1a328e16 --- /dev/null +++ b/mission/controller/acs/SensorValues.h @@ -0,0 +1,99 @@ +/* Created on: 08.03.2022 + * Author: Robin + */ + +#ifndef SENSORVALUES_H_ +#define SENSORVALUES_H_ + +#include + +namespace ACS { + +class SensorValues: public HasReturnvaluesIF { +public: + SensorValues(); + virtual ~SensorValues(); + + ReturnValue_t update(); + + float mgm0[3]; + float mgm1[3]; + float mgm2[3]; + float mgm3[3]; + float mgm4[3]; + + bool mgm0Valid; + bool mgm1Valid; + bool mgm2Valid; + bool mgm3Valid; + bool mgm4Valid; + + float sus0[3]; + float sus1[3]; + float sus2[3]; + float sus3[3]; + float sus4[3]; + float sus5[3]; + float sus6[3]; + float sus7[3]; + float sus8[3]; + float sus9[3]; + float sus10[3]; + float sus11[3]; + + bool sus0Valid; + bool sus1Valid; + bool sus2Valid; + bool sus3Valid; + bool sus4Valid; + bool sus5Valid; + bool sus6Valid; + bool sus7Valid; + bool sus8Valid; + bool sus9Valid; + bool sus10Valid; + bool sus11Valid; + + + double rmu0[3]; + double rmu1[3]; + double rmu2[3]; + + bool rmu0Valid; + bool rmu1Valid; + bool rmu2Valid; + + double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? + bool quatJBValid; + int strIntTime[2]; + + double gps0latitude; //Reference is WGS84, so this one will probably be geodetic + double gps0longitude; //Should be geocentric for IGRF + double gps0altitude; + double gps0Velocity[3]; // speed over ground = ?? + double gps0Time; //utc + +// valid ids for gps values ! + int gps0TimeYear; + int gps0TimeMonth; + int gps0TimeHour; // should be double + bool gps0Valid; + + + bool mgt0valid; + +// Reaction wheel measurements + double speedRw0; // RPM [1/min] + double speedRw1; // RPM [1/min] + double speedRw2; // RPM [1/min] + double speedRw3; // RPM [1/min] + bool validRw0; + bool validRw1; + bool validRw2; + bool validRw3; + +}; +} /* namespace ACS */ + +#endif ENSORVALUES_H_ + From 28dfcbf0ef4e41a74acc823d3b60062baaa87e7c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 19 Sep 2022 15:44:14 +0200 Subject: [PATCH 012/244] added SensorProcessing --- mission/controller/acs/SensorProcessing.cpp | 387 ++++++++++++++++++++ mission/controller/acs/SensorProcessing.h | 89 +++++ 2 files changed, 476 insertions(+) create mode 100644 mission/controller/acs/SensorProcessing.cpp create mode 100644 mission/controller/acs/SensorProcessing.h diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp new file mode 100644 index 00000000..eeb7f457 --- /dev/null +++ b/mission/controller/acs/SensorProcessing.cpp @@ -0,0 +1,387 @@ +/* + * SensorProcessing.cpp + * + * Created on: 7 Mar 2022 + * Author: Robin Marquardt + */ + +#include "SensorProcessing.h" +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Math; +// Thought: Maybe separate file for transforming of sensor values +// into geometry frame and body frame + +SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : + savedMagFieldEst { 0, 0, 0 }{ + validMagField = false; + validGcLatitude = false; +} + +SensorProcessing::~SensorProcessing() { +} + +bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, + const float *mgm1Value, bool mgm1valid, const float *mgm2Value, + bool mgm2valid, const float *mgm3Value, bool mgm3valid, + const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, + const AcsParameters::MgmHandlingParameters *mgmParameters, + const double gpsLatitude, const double gpsLongitude, + const double gpsAltitude, bool gpsValid, double *magFieldEst, bool *outputValid, + double *magFieldModel, bool*magFieldModelValid, + double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid) { + + if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { + *outputValid = false; + validMagField = false; + return false; + } + // Transforming Values to the Body Frame (actually it is the geometry frame atm) + float mgm0ValueBody[3] = {0,0,0}, mgm1ValueBody[3] = {0,0,0}, + mgm2ValueBody[3] = {0,0,0}, mgm3ValueBody[3] = {0,0,0}, + mgm4ValueBody[3] = {0,0,0}; + + bool validUnit[5] = {false, false, false, false, false}; + uint8_t validCount = 0; + if (mgm0valid) { + MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, mgm0ValueBody, 3, 3, 1); + validCount += 1; + validUnit[0] = true; + } + if (mgm1valid) { + MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, mgm1ValueBody, 3, 3, 1); + validCount += 1; + validUnit[1] = true; + } + if (mgm2valid) { + MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, mgm2ValueBody, 3, 3, 1); + validCount += 1; + validUnit[2] = true; + } + if (mgm3valid) { + MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, mgm3ValueBody, 3, 3, 1); + validCount += 1; + validUnit[3] = true; + } + if (mgm4valid) { + MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value, mgm4ValueBody, 3, 3, 1); + validCount += 1; + validUnit[4] = true; + } + + /* -------- MagFieldEst: Middle Value ------- */ + float mgmValues[3][5] = { { mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], + mgm3ValueBody[0], mgm4ValueBody[0] }, { mgm0ValueBody[1], mgm1ValueBody[1], + mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1] }, { mgm0ValueBody[2], + mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2] } }; + double mgmValidValues[3][validCount]; + uint8_t j = 0; + for (uint8_t i = 0; i < validCount; i++) { + if (validUnit[i]) { + mgmValidValues[0][j] = mgmValues[0][i]; + mgmValidValues[1][j] = mgmValues[1][i]; + mgmValidValues[2][j] = mgmValues[2][i]; + j += 1; + } + } + //Selection Sort + double mgmValidValuesSort[3][validCount]; + MathOperations::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount); + + uint8_t n = ceil(validCount/2); + magFieldEst[0] = mgmValidValuesSort[0][n]; + magFieldEst[1] = mgmValidValuesSort[1][n]; + magFieldEst[2] = mgmValidValuesSort[2][n]; + validMagField = true; + + //-----------------------Mag Rate Computation --------------------------------------------------- + double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); + for (uint8_t i = 0; i < 3; i++) { + magneticFieldVectorDerivative[i] = (magFieldEst[i] + - savedMagFieldEst[i]) / timeDiff; + savedMagFieldEst[i] = magFieldEst[i]; + } + + *magneticFieldVectorDerivativeValid = true; + if (timeOfSavedMagFieldEst.tv_sec == 0) { + magneticFieldVectorDerivative[0] = 0; + magneticFieldVectorDerivative[1] = 0; + magneticFieldVectorDerivative[2] = 0; + *magneticFieldVectorDerivativeValid = false; + } + + timeOfSavedMagFieldEst = timeOfMgmMeasurement; + + + *outputValid = true; + + // ---------------- IGRF- 13 Implementation here ------------------------------------------------ + if (!gpsValid){ + *magFieldModelValid = false; + } + else{ + // 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.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(gpsLongitude, gpsLatitude, gpsAltitude, + timeOfMgmMeasurement, magFieldModel); + *magFieldModelValid = false; + } + + return true; +} + +void SensorProcessing::processSus(const float sus0Value[], bool sus0valid, const float sus1Value[], bool sus1valid, + const float sus2Value[], bool sus2valid, const float sus3Value[], bool sus3valid, + const float sus4Value[], bool sus4valid, const float sus5Value[], bool sus5valid, + const float sus6Value[], bool sus6valid, const float sus7Value[], bool sus7valid, + const float sus8Value[], bool sus8valid, const float sus9Value[], bool sus9valid, + const float sus10Value[], bool sus10valid, const float sus11Value[], bool sus11valid, + timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, + double *sunVectorInertial, bool *sunVectorInertialValid, + double *sunVectorDerivative, bool *sunVectorDerivativeValid){ + + if(!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid + && !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid){ + *sunDirEstValid = false; + return; + } + else{ + // WARNING: NOT TRANSFORMED IN BODY FRAME YET + // Transformation into Geomtry Frame + float sus0ValueBody[3] = {0,0,0}, sus1ValueBody[3] = {0,0,0}, sus2ValueBody[3] = {0,0,0}, + sus3ValueBody[3] = {0,0,0}, sus4ValueBody[3] = {0,0,0}, sus5ValueBody[3] = {0,0,0}, + sus6ValueBody[3] = {0,0,0}, sus7ValueBody[3] = {0,0,0}, sus8ValueBody[3] = {0,0,0}, + sus9ValueBody[3] = {0,0,0}, sus10ValueBody[3] = {0,0,0}, sus11ValueBody[3] = {0,0,0}; + + if (sus0valid) { + MatrixOperations::multiply(susParameters->sus0orientationMatrix[0], sus0Value, sus0ValueBody, 3, 3, 1); + } + if (sus1valid) { + MatrixOperations::multiply(susParameters->sus1orientationMatrix[0], sus1Value, sus1ValueBody, 3, 3, 1); + } + if (sus2valid) { + MatrixOperations::multiply(susParameters->sus2orientationMatrix[0], sus2Value, sus2ValueBody, 3, 3, 1); + } + if (sus3valid) { + MatrixOperations::multiply(susParameters->sus3orientationMatrix[0], sus3Value, sus3ValueBody, 3, 3, 1); + } + if (sus4valid) { + MatrixOperations::multiply(susParameters->sus4orientationMatrix[0], sus4Value, sus4ValueBody, 3, 3, 1); + } + if (sus5valid) { + MatrixOperations::multiply(susParameters->sus5orientationMatrix[0], sus5Value, sus5ValueBody, 3, 3, 1); + } + if (sus6valid) { + MatrixOperations::multiply(susParameters->sus6orientationMatrix[0], sus6Value, sus6ValueBody, 3, 3, 1); + } + if (sus7valid) { + MatrixOperations::multiply(susParameters->sus7orientationMatrix[0], sus7Value, sus7ValueBody, 3, 3, 1); + } + if (sus8valid) { + MatrixOperations::multiply(susParameters->sus8orientationMatrix[0], sus8Value, sus8ValueBody, 3, 3, 1); + } + if (sus9valid) { + MatrixOperations::multiply(susParameters->sus9orientationMatrix[0], sus9Value, sus9ValueBody, 3, 3, 1); + } + if (sus10valid) { + MatrixOperations::multiply(susParameters->sus10orientationMatrix[0], sus10Value, sus10ValueBody, 3, 3, 1); + } + if (sus11valid) { + MatrixOperations::multiply(susParameters->sus11orientationMatrix[0], sus11Value, sus11ValueBody, 3, 3, 1); + } + + /* ------ Mean Value: susDirEst ------ */ + // Timo already done + bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; + float susValuesBody[3][12] = {{sus0ValueBody[0], sus1ValueBody[0], sus2ValueBody[0], sus3ValueBody[0], sus4ValueBody[0], + sus5ValueBody[0], sus6ValueBody[0], sus7ValueBody[0], sus8ValueBody[0], sus9ValueBody[0], sus10ValueBody[0], sus11ValueBody[0]}, + {sus0ValueBody[1], sus1ValueBody[1], sus2ValueBody[1], sus3ValueBody[1], sus4ValueBody[1], + sus5ValueBody[1], sus6ValueBody[1], sus7ValueBody[1], sus8ValueBody[1], sus9ValueBody[1], sus10ValueBody[1], sus11ValueBody[1]}, + {sus0ValueBody[2], sus1ValueBody[2], sus2ValueBody[2], sus3ValueBody[2], sus4ValueBody[2], + sus5ValueBody[2], sus6ValueBody[2], sus7ValueBody[2], sus8ValueBody[2], sus9ValueBody[2], sus10ValueBody[2], sus11ValueBody[2]}}; + + double susMeanValue[3] = {0,0,0}; + uint8_t validSusCounter = 0; + for (uint8_t i = 0; i < 12; i++){ + if (validIds[i]){ + susMeanValue[0]+=susValuesBody[0][i]; + susMeanValue[1]+=susValuesBody[1][i]; + susMeanValue[2]+=susValuesBody[2][i]; + validSusCounter+=1; + } + } + double divisor = 1/validSusCounter; + VectorOperations::mulScalar(susMeanValue, divisor, sunDirEst, 3); + *sunDirEstValid = true; + } + + /* -------- Sun Derivatiative --------------------- */ + + double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); + for (uint8_t i = 0; i < 3; i++) { + sunVectorDerivative[i] = (sunDirEst[i] + - savedSunVector[i]) / timeDiff; + savedSunVector[i] = sunDirEst[i]; + } + + *sunVectorDerivativeValid = true; + if (timeOfSavedSusDirEst.tv_sec == 0) { + sunVectorDerivative[0] = 0; + sunVectorDerivative[1] = 0; + sunVectorDerivative[2] = 0; + *sunVectorDerivativeValid = false; + } + + timeOfSavedSusDirEst = timeOfSusMeasurement; + + /* -------- Sun Model Direction (IJK frame) ------- */ + // if (useSunModel) eventuell + double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); + + //Julean Centuries + 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; + + + sunVectorInertial[0] = cos(eclipticLongitude); + sunVectorInertial[1] = sin(eclipticLongitude) + * cos(epsilon); + sunVectorInertial[2] = sin(eclipticLongitude) + * sin(epsilon); + + + *sunVectorInertialValid = true; +} + + +void SensorProcessing::processRmu(const double rmu0Value[], bool rmu0valid, + const double rmu1Value[], bool rmu1valid, + const double rmu2Value[], bool rmu2valid, + timeval timeOfrmuMeasurement, const AcsParameters::RmuHandlingParameters *rmuParameters, + double *satRatEst, bool *satRateEstValid){ + + if (!rmu0valid && !rmu1valid && !rmu2valid) { + *satRateEstValid = false; + return; + } + // Transforming Values to the Body Frame (actually it is the geometry frame atm) + double rmu0ValueBody[3] = {0,0,0}, rmu1ValueBody[3]= {0,0,0}, + rmu2ValueBody[3] = {0,0,0}; + + bool validUnit[3] = {false, false, false}; + uint8_t validCount = 0; + if (rmu0valid) { + MatrixOperations::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value, rmu0ValueBody, 3, 3, 1); + validCount += 1; + validUnit[0] = true; + } + if (rmu1valid) { + MatrixOperations::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value, rmu1ValueBody, 3, 3, 1); + validCount += 1; + validUnit[1] = true; + } + if (rmu2valid) { + MatrixOperations::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value, rmu2ValueBody, 3, 3, 1); + validCount += 1; + validUnit[2] = true; + } + + /* -------- SatRateEst: Middle Value ------- */ + double rmuValues[3][3] = { { rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]}, { rmu0ValueBody[1], rmu1ValueBody[1], + rmu2ValueBody[1]}, { rmu0ValueBody[2], + rmu1ValueBody[2], rmu2ValueBody[2]} }; + double rmuValidValues[3][validCount]; + uint8_t j = 0; + for (uint8_t i = 0; i < validCount; i++) { + if (validUnit[i]) { + rmuValidValues[0][j] = rmuValues[0][i]; + rmuValidValues[1][j] = rmuValues[1][i]; + rmuValidValues[2][j] = rmuValues[2][i]; + j += 1; + } + } + //Selection Sort + double rmuValidValuesSort[3][validCount]; + MathOperations::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount); + + uint8_t n = ceil(validCount/2); + satRatEst[0] = rmuValidValuesSort[0][n]; + satRatEst[1] = rmuValidValuesSort[1][n]; + satRatEst[2] = rmuValidValuesSort[2][n]; + *satRateEstValid = true; + +} + +void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, + const bool validGps, double *gcLatitude, double *gdLongitude){ + // name to convert not process + if (validGps) { + // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic + *gdLongitude = gps0longitude * PI / 180; + double latitudeRad = gps0latitude * PI / 180; + double eccentricityWgs84 = 0.0818195; + double factor = 1 - pow(eccentricityWgs84, 2); + *gcLatitude = atan(factor * tan(latitudeRad)); + validGcLatitude = true; + + } +} + +void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, + ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { + + sensorValues->update(); + processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, + sensorValues->gps0Valid, &outputValues->gcLatitude, &outputValues->gdLongitude); + + outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, + sensorValues->mgm1, sensorValues->mgm1Valid, + sensorValues->mgm2, sensorValues->mgm2Valid, + sensorValues->mgm3, sensorValues->mgm3Valid, + sensorValues->mgm4, sensorValues->mgm4Valid, now, + &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, + outputValues->gdLongitude, sensorValues->gps0altitude, + sensorValues->gps0Valid, + outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid, + outputValues->magneticFieldVectorDerivative, &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? + + processSus(sensorValues->sus0, sensorValues->sus0Valid, sensorValues->sus1, sensorValues->sus1Valid, + sensorValues->sus2, sensorValues->sus2Valid, sensorValues->sus3, sensorValues->sus3Valid, + sensorValues->sus4, sensorValues->sus4Valid, sensorValues->sus5, sensorValues->sus5Valid, + sensorValues->sus6, sensorValues->sus6Valid, sensorValues->sus7, sensorValues->sus7Valid, + sensorValues->sus8, sensorValues->sus8Valid, sensorValues->sus9, sensorValues->sus9Valid, + sensorValues->sus10, sensorValues->sus10Valid, sensorValues->sus11, sensorValues->sus11Valid, + now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, + outputValues->sunDirEst, &outputValues->sunDirEstValid, + outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // VALID outputs ? + + processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, sensorValues->rmu1Valid, + sensorValues->rmu2, sensorValues->rmu2Valid, now, &acsParameters->rmuHandlingParameters, + outputValues->satRateEst, &outputValues->satRateEstValid); + +} + diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h new file mode 100644 index 00000000..9db8e25b --- /dev/null +++ b/mission/controller/acs/SensorProcessing.h @@ -0,0 +1,89 @@ +/******************************* + * EIVE Flight Software Framework (FSFW) + * (c) 2022 IRS, Uni Stuttgart + *******************************/ +#ifndef SENSORPROCESSING_H_ +#define SENSORPROCESSING_H_ + +#include //uint8_t +#include /*purpose, timeval ?*/ +#include "acs/config/classIds.h" +#include +#include "AcsParameters.h" +#include +#include + +/*Planned: + * - Fusion of Sensor Measurements - + * sunDirEst (mean value) + * magField (mean value) + * rmuSatRate (rmus, mean value) + * - Models to get inertia values - + * sunModelDir (input: time) + * magModelField (input: position,time) + * - Low Pass Filter maybe - + * magField + * SunDirEst*/ + +class SensorProcessing: public HasReturnvaluesIF{ +public: + + void reset(); + + SensorProcessing(AcsParameters *acsParameters_); + virtual ~SensorProcessing(); + + void process(timeval now, ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, + const AcsParameters *acsParameters); // Will call protected functions +private: + +protected: + // short description needed for every function + bool processMgm(const float *mgm0Value, bool mgm0valid, + const float *mgm1Value, bool mgm1valid, + const float *mgm2Value, bool mgm2valid, + const float *mgm3Value, bool mgm3valid, + const float *mgm4Value, bool mgm4valid, + timeval timeOfMgmMeasurement, + const AcsParameters::MgmHandlingParameters *mgmParameters, + const double gpsLatitude, const double gpsLongitude, + const double gpsAltitude, bool gpsValid, + double *magFieldEst, bool *outputValid, + double *magFieldModel, bool*magFieldModelValid, + double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid); //Output + + void processSus(const float sus0Value[], bool sus0valid, const float sus1Value[], bool sus1valid, + const float sus2Value[], bool sus2valid, const float sus3Value[], bool sus3valid, + const float sus4Value[], bool sus4valid, const float sus5Value[], bool sus5valid, + const float sus6Value[], bool sus6valid, const float sus7Value[], bool sus7valid, + const float sus8Value[], bool sus8valid, const float sus9Value[], bool sus9valid, + const float sus10Value[], bool sus10valid, const float sus11Value[], bool sus11valid, + timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, + double *sunVectorModel, bool *sunVectorModelValid, + double *sunVectorDerivative, bool *sunVectorDerivativeValid); + + void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu + const double rmu1Value[], bool rmu1valid, + const double rmu2Value[], bool rmu2valid, + timeval timeOfrmuMeasurement, const AcsParameters::RmuHandlingParameters *rmuParameters, + double *satRatEst, bool *satRateEstValid); + + void processStr(); + + void processGps(const double gps0latitude, const double gps0longitude, + const bool validGps, double *gcLatitude, double *gdLongitude); + + + double savedMagFieldEst[3]; + timeval timeOfSavedMagFieldEst; + double savedSunVector[3]; + timeval timeOfSavedSusDirEst; + bool validMagField; + bool validGcLatitude; + + +}; + +#endif SENSORPROCESSING_H_ + From cc82b8aa4223ac038eced821427426db6338a4a9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Sep 2022 11:20:13 +0200 Subject: [PATCH 013/244] small formatting fixes --- mission/controller/acs/AcsParameters.cpp | 93 ++++++++++++++---------- 1 file changed, 53 insertions(+), 40 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 63d8951d..7f5b26c1 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -8,38 +8,46 @@ AcsParameters::AcsParameters() { onBoardParams.sampleTime = 0.1; - inertiaEIVE.inertiaMatrix = {{ 1.0, 0.0, 0.0}, - { 0.0, 1.0, 0.0}, - { 0.0, 0.5, 1.0}}; + inertiaEIVE.inertiaMatrix = { + { 1.0, 0.0, 0.0}, + { 0.0, 1.0, 0.0}, + { 0.0, 0.5, 1.0}}; - mgmHandlingParameters.mgm0orientationMatrix = {{ 0, 0,-1}, - { 0, 1, 0}, - { 1, 0, 0}}; - /*mgmHandlingParameters.mgm1orientationMatrix = {{ 0, 0, 1}, - { 0,-1, 0}, - { 1, 0, 0}}; - mgmHandlingParameters.mgm2orientationMatrix = {{ 0, 0,-1}, - { 0, 1, 0}, - { 1 ,0, 0}}; - mgmHandlingParameters.mgm3orientationMatrix = {{ 0, 0, 1}, - { 0,-1, 0}, - { 1, 0, 0}}; - mgmHandlingParameters.mgm4orientationMatrix = {{ 0, 0,-1}, - {-1, 0, 0}, - { 0, 1, 0}};*/ + mgmHandlingParameters.mgm0orientationMatrix = { + { 0, 0,-1}, + { 0, 1, 0}, + { 1, 0, 0}}; + mgmHandlingParameters.mgm1orientationMatrix = { + { 0, 0, 1}, + { 0,-1, 0}, + { 1, 0, 0}}; + mgmHandlingParameters.mgm2orientationMatrix = { + { 0, 0,-1}, + { 0, 1, 0}, + { 1 ,0, 0}}; + mgmHandlingParameters.mgm3orientationMatrix = { + { 0, 0, 1}, + { 0,-1, 0}, + { 1, 0, 0}}; + mgmHandlingParameters.mgm4orientationMatrix = { + { 0, 0,-1}, + {-1, 0, 0}, + { 0, 1, 0}}; rwHandlingParameters.inertiaWheel = 0.000028198; rwHandlingParameters.maxTrq = 0.0032; //3.2 [mNm] //Geometry frame - rwMatrices.alignmentMatrix = {{ 0.9205, 0.0000, -0.9205, 0.0000}, - { 0.0000, -0.9205, 0.0000, 0.9205}, - { 0.3907, 0.3907, 0.3907, 0.3907}}; + rwMatrices.alignmentMatrix = { + { 0.9205, 0.0000, -0.9205, 0.0000}, + { 0.0000, -0.9205, 0.0000, 0.9205}, + { 0.3907, 0.3907, 0.3907, 0.3907}}; - rwMatrices.pseudoInverse = {{ 0.4434, -0.2845, 0.3597}, - { 0.2136, -0.3317, 1.0123}, - { -0.8672, -0.1406, 0.1778}, - { 0.6426, 0.4794, 1.3603}}; + rwMatrices.pseudoInverse = { + { 0.4434, -0.2845, 0.3597}, + { 0.2136, -0.3317, 1.0123}, + { -0.8672, -0.1406, 0.1778}, + { 0.6426, 0.4794, 1.3603}}; rwMatrices.nullspace = { -0.7358, 0.5469, -0.3637, -0.1649}; @@ -96,21 +104,26 @@ AcsParameters::AcsParameters() { strParameters.boresightAxis = { 0.7593, 0,-0.6508}; strParameters.exclusionAngle = 20 * Math::PI /180; - magnetorquesParameter.mtq0orientationMatrix = {{ 1, 0, 0}, - { 0, 0, 1}, - { 0,-1, 0}}; - magnetorquesParameter.mtq1orientationMatrix = {{ 1, 0, 0}, - { 0, 1, 0}, - { 0, 0, 1}}; - magnetorquesParameter.mtq2orientationMatrix = {{ 0, 0, 1}, - { 0, 1, 0}, - {-1, 0, 0}}; - magnetorquesParameter.alignmentMatrixMtq = {{ 0, 0,-1}, - {-1, 0, 0}, - { 0, 1, 0}}; - magnetorquesParameter.inverseAlignment = {{ 0,-1, 0}, - { 0, 0, 1}, - {-1, 0, 0}}; + magnetorquesParameter.mtq0orientationMatrix = { + { 1, 0, 0}, + { 0, 0, 1}, + { 0,-1, 0}}; + magnetorquesParameter.mtq1orientationMatrix = { + { 1, 0, 0}, + { 0, 1, 0}, + { 0, 0, 1}}; + magnetorquesParameter.mtq2orientationMatrix = { + { 0, 0, 1}, + { 0, 1, 0}, + {-1, 0, 0}}; + magnetorquesParameter.alignmentMatrixMtq = { + { 0, 0,-1}, + {-1, 0, 0}, + { 0, 1, 0}}; + magnetorquesParameter.inverseAlignment = { + { 0,-1, 0}, + { 0, 0, 1}, + {-1, 0, 0}}; magnetorquesParameter.DipolMax = 0.2; } From 3d123a09f3270ead90dd53cf84292198332289a6 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Sep 2022 13:36:17 +0200 Subject: [PATCH 014/244] added control subfolder. added Detumble --- mission/controller/acs/control/Detumble.cpp | 65 +++++++++++++++++++++ mission/controller/acs/control/Detumble.h | 46 +++++++++++++++ 2 files changed, 111 insertions(+) create mode 100644 mission/controller/acs/control/Detumble.cpp create mode 100644 mission/controller/acs/control/Detumble.h diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp new file mode 100644 index 00000000..283d6dc1 --- /dev/null +++ b/mission/controller/acs/control/Detumble.cpp @@ -0,0 +1,65 @@ + +/* + * Detumble.cpp + * + * Created on: 17 Aug 2022 + * Author: Robin Marquardt + */ + + +#include "Detumble.h" +#include +#include +#include +#include +#include +#include +#include + + +Detumble::Detumble(AcsParameters *acsParameters_){ + loadAcsParameters(acsParameters_); +} + +Detumble::~Detumble(){ + +} + +void Detumble::loadAcsParameters(AcsParameters *acsParameters_){ + + detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); + 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 = detumbleCtrlParameters->gainD; + double factor = -gain / pow(VectorOperations::norm(magField,3),2); + VectorOperations::mulScalar(magRate, factor, magMom, 3); + return RETURN_OK; + +} + +ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom) { + + if (!magRateValid) { + return DETUMBLE_NO_SENSORDATA; + } + + double dipolMax = magnetorquesParameter->DipolMax; + for (int i = 0; i<3; i++) { + + magMom[i] = - dipolMax * sign(magRate[i]); + + } + + return RETURN_OK; + +} diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h new file mode 100644 index 00000000..54d62179 --- /dev/null +++ b/mission/controller/acs/control/Detumble.h @@ -0,0 +1,46 @@ +/* + * Detumble.h + * + * Created on: 17 Aug 2022 + * Author: Robin Marquardt + */ + +#ifndef ACS_CONTROL_DETUMBLE_H_ +#define ACS_CONTROL_DETUMBLE_H_ + +#include +#include +#include +#include +#include +#include "acs/config/classIds.h" +#include +#include + +class Detumble : public HasReturnvaluesIF { + +public: + Detumble(AcsParameters *acsParameters_); + virtual ~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 + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void loadAcsParameters(AcsParameters *acsParameters_); + + ReturnValue_t bDotLaw(const double *magRate, const bool *magRateValid, + const double *magField, const bool *magFieldValid, + double *magMom); + + ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); + +private: + AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters; + AcsParameters::MagnetorquesParameter* magnetorquesParameter; +}; + +#endif /* ACS_CONTROL_DETUMBLE_H_ */ + From 497149a628a72ac848c5a33e3a2db79465699ffb Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Sep 2022 13:43:26 +0200 Subject: [PATCH 015/244] added PtgCtrl --- mission/controller/acs/control/PtgCtrl.cpp | 165 +++++++++++++++++++++ mission/controller/acs/control/PtgCtrl.h | 60 ++++++++ 2 files changed, 225 insertions(+) create mode 100644 mission/controller/acs/control/PtgCtrl.cpp create mode 100644 mission/controller/acs/control/PtgCtrl.h diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp new file mode 100644 index 00000000..eaffd909 --- /dev/null +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -0,0 +1,165 @@ +/* + * PtgCtrl.cpp + * + * Created on: 17 Jul 2022 + * Author: Robin Marquardt + */ + + + +#include "PtgCtrl.h" +#include +#include +#include +#include +#include +#include +#include + +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ + loadAcsParameters(acsParameters_); +} + +PtgCtrl::~PtgCtrl(){ + +} + +void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){ + pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); + inertiaEIVE = &(acsParameters_->inertiaEIVE); + rwHandlingParameters = &(acsParameters_->rwHandlingParameters); + rwMatrices =&(acsParameters_->rwMatrices); +} + +void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){ + + //------------------------------------------------------------------------------------------------ + // Compute gain matrix K and P matrix + //------------------------------------------------------------------------------------------------ + double om = pointingModeControllerParameters->om; + double zeta = pointingModeControllerParameters->zeta; + double qErrorMin = pointingModeControllerParameters->qiMin; + double omMax = pointingModeControllerParameters->omMax; + + double cInt = 2 * om * zeta; + double kInt = 2 * pow(om,2); + + double qErrorLaw[3] = {0, 0, 0}; + + for (int i = 0; i < 3; i++) { + if (abs(qError[i]) < qErrorMin) { + qErrorLaw[i] = qErrorMin; + } + else { + qErrorLaw[i] = abs(qError[i]); + } + } + double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); + + double gain1 = cInt * omMax / qErrorLawNorm; + double gainVector[3] = {0, 0, 0}; + VectorOperations::mulScalar(qErrorLaw, gain1, gainVector, 3); + + double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + gainMatrixDiagonal[0][0] = gainVector[0]; + gainMatrixDiagonal[1][1] = gainVector[1]; + gainMatrixDiagonal[2][2] = gainVector[2]; + MatrixOperations::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3); + + // Inverse of gainMatrix + double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; + gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; + gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; + + double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3); + MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); + + //------------------------------------------------------------------------------------------------ + // Torque Calculations for the reaction wheels + //------------------------------------------------------------------------------------------------ + + double pError[3] = {0, 0, 0}; + MatrixOperations::multiply(*pMatrix, qError, pError, 3, 3, 1); + 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); + +// 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); + +} + +void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0, + double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes) { + if ( !(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { + + mgtDpDes[0] = 0; + mgtDpDes[1] = 0; + mgtDpDes[2] = 0; + return; + + } + +// calculating momentum of satellite and momentum of reaction wheels + double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; + VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); + MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, 1); + double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); +// calculating momentum error + double deltaMomentum[3] = {0, 0, 0}; + VectorOperations::subtract(momentumTotal, pointingModeControllerParameters->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; + VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); + +} + +void PtgCtrl::ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs) { + + double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double wheelMomentum[4] = {0, 0, 0, 0}; + double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; + //Conversion to [rad/s] for further calculations + VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); + VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); + double diffRwSpeed[4] = {0, 0, 0, 0}; + VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); + VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, wheelMomentum, 4); + double gainNs = pointingModeControllerParameters->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); + MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); + VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); + + +} diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h new file mode 100644 index 00000000..5bd7f0da --- /dev/null +++ b/mission/controller/acs/control/PtgCtrl.h @@ -0,0 +1,60 @@ +/* + * PtgCtrl.h + * + * Created on: 17 Jul 2022 + * Author: Robin Marquardt + * + * @brief: This class handles the pointing control mechanism. Calculation of an commanded torque + * for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * + * @note: A description of the used algorithms can be found in + * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 + */ + +#ifndef PTGCTRL_H_ +#define PTGCTRL_H_ + +#include +#include +#include +#include +#include +#include "acs/config/classIds.h" +#include +#include + +class PtgCtrl : public HasReturnvaluesIF { + +public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + PtgCtrl(AcsParameters *acsParameters_); + virtual ~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 + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void loadAcsParameters(AcsParameters *acsParameters_); + + /* @brief: Calculates the needed torque for the pointing control mechanism + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void ptgGroundstation(const double mode,const double *qError,const double *deltaRate,const double *rwPseudoInv, double *torqueRws); + + void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0, + double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes); + + void ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs); + +private: + AcsParameters::PointingModeControllerParameters* pointingModeControllerParameters; + AcsParameters::RwHandlingParameters* rwHandlingParameters; + AcsParameters::InertiaEIVE* inertiaEIVE; + AcsParameters::RWMatrices* rwMatrices; +}; + +#endif /* ACS_CONTROL_PTGCTRL_H_ */ From 40917ab988cb074e60aafc1ad9cc9e21fe88bed7 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Sep 2022 13:46:42 +0200 Subject: [PATCH 016/244] added SafeCtrl --- mission/controller/acs/control/SafeCtrl.cpp | 186 ++++++++++++++++++++ mission/controller/acs/control/SafeCtrl.h | 63 +++++++ 2 files changed, 249 insertions(+) create mode 100644 mission/controller/acs/control/SafeCtrl.cpp create mode 100644 mission/controller/acs/control/SafeCtrl.h diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp new file mode 100644 index 00000000..55772ae4 --- /dev/null +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -0,0 +1,186 @@ +/* + * SafeCtrl.cpp + * + * Created on: 19 Apr 2022 + * Author: Robin Marquardt + */ + +#include "SafeCtrl.h" +#include +#include +#include +#include +#include +#include + +SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){ + loadAcsParameters(acsParameters_); + MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, 3); +} + +SafeCtrl::~SafeCtrl(){ + +} + +void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){ + safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); + inertiaEIVE = &(acsParameters_->inertiaEIVE); +} + +ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid, + double *magFieldModel, bool *magFieldModelValid, + double *sunDirModel, bool *sunDirModelValid, + double *satRateMekf, bool *rateMekfValid, + double *sunDirRef, double *satRatRef, + double *outputMagMomB, bool *outputValid){ + + if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || + !(*rateMekfValid)) { + *outputValid = false; + return SAFECTRL_MEKF_INPUT_INVALID; + } + + double kRate = 0, kAlign = 0; + kRate = safeModeControllerParameters->k_rate_mekf; + kAlign = safeModeControllerParameters->k_align_mekf; + +// Calc sunDirB ,magFieldB with mekf output and model + double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + MathOperations::dcmFromQuat(quatBJ, *dcmBJ); + double sunDirB[3] = {0,0,0}, magFieldB[3] = {0,0,0}; + MatrixOperations::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); + + double crossSun[3] = {0, 0, 0}; + + VectorOperations::cross(sunDirRef, sunDirB, crossSun); + double normCrossSun = VectorOperations::norm(crossSun, 3); + +// calc angle alpha between sunDirRef and sunDIr + double alpha = 0, dotSun = 0; + dotSun = VectorOperations::dot(sunDirRef, sunDirB); + alpha = acos(dotSun); + +// Law Torque calculations + double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, + torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0}; + + double scalarFac = 0; + scalarFac = kAlign * alpha / normCrossSun; + VectorOperations::mulScalar(crossSun, scalarFac, torqueAlign, 3); + + double rateSafeMode[3] = {0,0,0}; + VectorOperations::subtract(satRateMekf, satRatRef, rateSafeMode, 3); + VectorOperations::mulScalar(rateSafeMode, -kRate, torqueRate, 3); + + VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); +// Adding factor of inertia for axes + MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); + +// MagMom B (orthogonal torque) + double torqueMgt[3] = {0,0,0}; + VectorOperations::cross(magFieldB, torqueCmd, torqueMgt); + double normMag = VectorOperations::norm(magFieldB, 3); + VectorOperations::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3); + *outputValid = true; + + return RETURN_OK; + +} + +// Will be the version in worst case scenario in event of no working MEKF (nor RMUs) +void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, + double *sunRateB, bool *sunRateBValid, + double *magFieldB, bool *magFieldBValid, + double *magRateB, bool *magRateBValid, + double *sunDirRef, double *satRateRef, + double *outputMagMomB, bool *outputValid){ + +// Check for invalid Inputs + if ( !susDirBValid || !magFieldBValid || !magRateBValid) { + *outputValid = false; + return; + } + +// normalize sunDir and magDir + double magDirB[3] = {0, 0, 0}; + VectorOperations::normalize(magFieldB, magDirB, 3); + VectorOperations::normalize(susDirB, susDirB, 3); + +// Cosinus angle between sunDir and magDir + double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); + +// Rate parallel to sun direction and magnetic field direction + double rateParaSun = 0, rateParaMag = 0; + double dotSunRateMag = 0, dotmagRateSun = 0, + rateFactor = 0; + dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); + dotmagRateSun = VectorOperations::dot(magRateB, susDirB); + rateFactor = 1 - pow(cosAngleSunMag,2); + rateParaSun = ( dotmagRateSun + cosAngleSunMag * dotSunRateMag ) / rateFactor; + rateParaMag = ( dotSunRateMag + cosAngleSunMag * dotmagRateSun ) / rateFactor; + +// Full rate or estimate + double estSatRate[3] = {0, 0, 0}; + double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0}; + VectorOperations::mulScalar(susDirB, rateParaSun, estSatRateSun, 3); + VectorOperations::add(sunRateB, estSatRateSun, estSatRateSun, 3); + VectorOperations::mulScalar(magDirB, rateParaMag, estSatRateMag, 3); + VectorOperations::add(magRateB, estSatRateMag, estSatRateMag, 3); + VectorOperations::add(estSatRateSun, estSatRateMag, estSatRate, 3); + VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); + +/* 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))) { + return; + } + +// Rate for Torque Calculation + double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ + VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); + +// Torque Align calculation + double kRateNoMekf = 0, kAlignNoMekf = 0; + kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; + kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + + double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); + double crossSusSunRef[3] = {0, 0, 0}; + VectorOperations::cross(sunDirRef, susDirB, crossSusSunRef); + double sinAngleAlignErr = VectorOperations::norm(crossSusSunRef, 3); + + double torqueAlign[3] = {0, 0, 0}; + double angleAlignErr = acos(cosAngleAlignErr); + double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr; + VectorOperations::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3); + +//Torque Rate Calculations + double torqueRate[3] = {0, 0, 0}; + VectorOperations::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3); + +//Final torque + double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, 1); + +//Magnetic moment + double magMomB[3] = {0, 0, 0}; + double crossMagFieldTorque[3] = {0, 0, 0}; + VectorOperations::cross(magFieldB, torqueB, crossMagFieldTorque); + double magMomFactor = pow( VectorOperations::norm(magFieldB, 3), 2 ); + VectorOperations::mulScalar(crossMagFieldTorque, 1/magMomFactor, magMomB, 3); + + outputMagMomB[0] = magMomB[0]; + outputMagMomB[1] = magMomB[1]; + outputMagMomB[2] = magMomB[2]; + + *outputValid = true; + +} + + diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h new file mode 100644 index 00000000..b0ce6a90 --- /dev/null +++ b/mission/controller/acs/control/SafeCtrl.h @@ -0,0 +1,63 @@ +/* + * safeCtrl.h + * + * Created on: 19 Apr 2022 + * Author: rooob + */ + +#ifndef SAFECTRL_H_ +#define SAFECTRL_H_ + +#include +#include +#include +#include +#include +#include "acs/config/classIds.h" +#include +#include + +class SafeCtrl : public HasReturnvaluesIF { + +public: + + SafeCtrl(AcsParameters *acsParameters_); + virtual ~SafeCtrl(); + + static const uint8_t INTERFACE_ID = CLASS_ID::SAFE; + static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + + void loadAcsParameters(AcsParameters *acsParameters_); + + ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, + double *magFieldModel, bool *magFieldModelValid, + double *sunDirModel, bool *sunDirModelValid, + double *satRateMekf, bool *rateMekfValid, + double *sunDirRef, double *satRatRef, // From Guidance (!) + double *outputMagMomB, bool *outputValid); + + void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, + double *sunRateB, bool *sunRateBValid, + double *magFieldB, bool *magFieldBValid, + double *magRateB, bool *magRateBValid, + double *sunDirRef, double *satRateRef, + double *outputMagMomB, bool *outputValid); + + void idleSunPointing(); // with reaction wheels + +protected: + +private: + AcsParameters::SafeModeControllerParameters* safeModeControllerParameters; + AcsParameters::InertiaEIVE* inertiaEIVE; + double gainMatrixInertia[3][3]; + + double magFieldBState[3]; + timeval magFieldBStateTime; + +}; + + + +#endif /* ACS_CONTROL_SAFECTRL_H_ */ + From 858b291f6105aef27dde6d8daa22a14d84d482a5 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Sep 2022 13:52:17 +0200 Subject: [PATCH 017/244] added config. added classIds --- mission/controller/acs/config/classIds.h | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 mission/controller/acs/config/classIds.h diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h new file mode 100644 index 00000000..0c03b408 --- /dev/null +++ b/mission/controller/acs/config/classIds.h @@ -0,0 +1,24 @@ +/* + * classIds.h + * + * Created on: 1 Mar 2022 + * Author: rooob + */ + +#ifndef ACS_CONFIG_CLASSIDS_H_ +#define ACS_CONFIG_CLASSIDS_H_ +#include "bsp_hosted/fsfwconfig/returnvalues/classIds.h" + +namespace CLASS_ID { +enum eiveclassIds: uint8_t { + EIVE_CLASS_ID_START = CLASS_ID_END, + KALMAN, + SAFE, + PTG, + DETUMBLE, + EIVE_CLASS_ID_END // [EXPORT] : [END] +}; +} + + +#endif /* ACS_CONFIG_CLASSIDS_H_ */ From 7885205a0740d4866ee9a48b0a48d5a2d9fcb882 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Sep 2022 14:15:55 +0200 Subject: [PATCH 018/244] added Navitation --- mission/controller/acs/Navigation.h | 35 +++++++++++++++++++ mission/controller/acs/Navitation.cpp | 49 +++++++++++++++++++++++++++ 2 files changed, 84 insertions(+) create mode 100644 mission/controller/acs/Navigation.h create mode 100644 mission/controller/acs/Navitation.cpp diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h new file mode 100644 index 00000000..7d71df99 --- /dev/null +++ b/mission/controller/acs/Navigation.h @@ -0,0 +1,35 @@ +/* + * Navigation.h + * + * Created on: 19 Apr 2022 + * Author: Robin Marquardt + */ + +#ifndef NAVIGATION_H_ +#define NAVIGATION_H_ + + +#include "AcsParameters.h" +#include "SensorProcessing.h" +#include +#include +#include + +class Navigation{ +public: + Navigation(AcsParameters *acsParameters_); //Input mode ? + virtual ~Navigation(); + + void useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid); + void processSensorData(); + +protected: + +private: + MultiplicativeKalmanFilter multiplicativeKalmanFilter; + AcsParameters acsParameters; + bool kalmanInit = false; +}; + +#endif /* ACS_NAVIGATION_H_ */ + diff --git a/mission/controller/acs/Navitation.cpp b/mission/controller/acs/Navitation.cpp new file mode 100644 index 00000000..50dbb049 --- /dev/null +++ b/mission/controller/acs/Navitation.cpp @@ -0,0 +1,49 @@ +/* + * Navigation.cpp + * + * Created on: 23 May 2022 + * Author: Robin Marquardt + */ + +#include "Navigation.h" +#include +#include +#include +#include +#include +#include + +Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){ + acsParameters = *acsParameters_; +} + +Navigation::~Navigation(){ + +} + +void Navigation::useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid){ + + if (kalmanInit) { + *mekfValid = multiplicativeKalmanFilter.mekfEst(sensorValues->quatJB, &sensorValues->quatJBValid, + outputValues->satRateEst, &outputValues->satRateEstValid, + outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->sunDirEst, &outputValues->sunDirEstValid, + outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid, + outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? + + } + else { + multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->sunDirEst, &outputValues->sunDirEstValid, + outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid); + kalmanInit = true; + *mekfValid = 0; + +// Maybe we need feedback from kalmanfilter to identify if there was a problem with the init of kalman filter +// where does this class know from that kalman filter was not initialized ? + } +} + + From 5ebdc9e7676aea838b2dce730c148d1cce3b2cb5 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Sep 2022 15:06:05 +0200 Subject: [PATCH 019/244] amended includes. removed links to HasParametersIF --- mission/controller/acs/AcsParameters.h | 2 +- mission/controller/acs/ActuatorCmd.cpp | 14 +++++++------- mission/controller/acs/ActuatorCmd.h | 10 +++++----- mission/controller/acs/Guidance.h | 2 +- mission/controller/acs/Igrf13Model.h | 2 +- mission/controller/acs/Navigation.h | 10 +++++----- mission/controller/acs/Navitation.cpp | 4 ++-- mission/controller/acs/SensorProcessing.cpp | 10 +++++----- mission/controller/acs/SensorProcessing.h | 14 +++++++------- mission/controller/acs/control/Detumble.cpp | 12 ++++++------ mission/controller/acs/control/Detumble.h | 10 +++++----- mission/controller/acs/util/MathOperations.h | 1 + 12 files changed, 46 insertions(+), 45 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index b4e32d65..6e8fd1a2 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -10,7 +10,7 @@ typedef unsigned char uint8_t; -class AcsParameters:public HasParametersIF{ +class AcsParameters/*:public HasParametersIF*/{ public: AcsParameters(); diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 25eeca9c..3b4ed71e 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -6,14 +6,14 @@ */ -#include "ActuatorCmd.h" -#include +#include +#include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 9b721505..85ef1532 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -9,11 +9,11 @@ #define ACTUATORCMD_H_ -#include "AcsParameters.h" -#include "SensorProcessing.h" -#include -#include -#include +#include +#include +#include +#include +#include class ActuatorCmd{ public: diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 736d8b9b..76cfce77 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include class Guidance { diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index 6e46a90e..b6f14e0f 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -25,7 +25,7 @@ // Output should be transformed to [T] instead of [nT] // Updating Coefficients has to be implemented yet. Question, updating everyday ? -class Igrf13Model:public HasParametersIF{ +class Igrf13Model/*:public HasParametersIF*/{ public: Igrf13Model(); diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 7d71df99..75bbade5 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -9,11 +9,11 @@ #define NAVIGATION_H_ -#include "AcsParameters.h" -#include "SensorProcessing.h" -#include -#include -#include +#include +#include +#include +#include +#include class Navigation{ public: diff --git a/mission/controller/acs/Navitation.cpp b/mission/controller/acs/Navitation.cpp index 50dbb049..2cb70389 100644 --- a/mission/controller/acs/Navitation.cpp +++ b/mission/controller/acs/Navitation.cpp @@ -7,11 +7,11 @@ #include "Navigation.h" #include -#include +#include #include #include #include -#include +#include Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){ acsParameters = *acsParameters_; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index eeb7f457..7b89daf1 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -6,11 +6,11 @@ */ #include "SensorProcessing.h" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 9db8e25b..43c93722 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -6,12 +6,12 @@ #define SENSORPROCESSING_H_ #include //uint8_t -#include /*purpose, timeval ?*/ -#include "acs/config/classIds.h" -#include -#include "AcsParameters.h" -#include -#include +#include /*purpose, timeval ?*/ +#include +#include +#include +#include +#include /*Planned: * - Fusion of Sensor Measurements - @@ -25,7 +25,7 @@ * magField * SunDirEst*/ -class SensorProcessing: public HasReturnvaluesIF{ +class SensorProcessing{ public: void reset(); diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 283d6dc1..8c3bdf86 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,14 +7,14 @@ */ -#include "Detumble.h" -#include -#include +#include +#include +#include #include -#include -#include +#include +#include #include -#include +#include Detumble::Detumble(AcsParameters *acsParameters_){ diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 54d62179..47efd49d 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -13,11 +13,11 @@ #include #include #include -#include "acs/config/classIds.h" -#include -#include +#include +#include +#include -class Detumble : public HasReturnvaluesIF { +class Detumble{ public: Detumble(AcsParameters *acsParameters_); @@ -42,5 +42,5 @@ private: AcsParameters::MagnetorquesParameter* magnetorquesParameter; }; -#endif /* ACS_CONTROL_DETUMBLE_H_ */ +#endif ACS_CONTROL_DETUMBLE_H_ diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 82d19b19..6a534880 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -9,6 +9,7 @@ #define MATH_MATHOPERATIONS_H_ #include +#include #include #include #include From b2484136b10826af2a72ddfbea623853b50cfd7c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 20 Sep 2022 15:45:49 +0200 Subject: [PATCH 020/244] further fixes for HasReturnValuesIF --- mission/controller/acs/Guidance.cpp | 2 +- mission/controller/acs/SensorValues.cpp | 2 +- mission/controller/acs/SensorValues.h | 4 ++-- mission/controller/acs/control/Detumble.cpp | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index d861769d..158ee9f0 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -74,7 +74,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues // 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(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, dcmTEST, dcmTEST, 3, 3, 3); MatrixOperations::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3); MatrixOperations::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3); diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index f3a52dea..25f2d61f 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -28,7 +28,7 @@ ReturnValue_t SensorValues::update() { // quatJB[3] = static_cast(quaternion.value); // quatJBValid = quaternion.isValid(); - return RETURN_OK; + return returnvalue::OK; } } diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 1a328e16..566eec81 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -5,11 +5,11 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ -#include +#include namespace ACS { -class SensorValues: public HasReturnvaluesIF { +class SensorValues{ public: SensorValues(); virtual ~SensorValues(); diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 8c3bdf86..8a15aa8a 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -43,7 +43,7 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid, double gain = detumbleCtrlParameters->gainD; double factor = -gain / pow(VectorOperations::norm(magField,3),2); VectorOperations::mulScalar(magRate, factor, magMom, 3); - return RETURN_OK; + return returnvalue::OK; } @@ -60,6 +60,6 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateVa } - return RETURN_OK; + return returnvalue::OK; } From 798668673941daaaa1a484c6d530c7d1ebe9934a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 22 Sep 2022 13:39:42 +0200 Subject: [PATCH 021/244] added SUS calibration coefficients to AcsParameters --- mission/controller/acs/AcsParameters.cpp | 677 ++++++++++++++++++++++- mission/controller/acs/AcsParameters.h | 25 + 2 files changed, 701 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 7f5b26c1..92273e04 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -1,7 +1,7 @@ #include #include "AcsParameters.h" -//#include +#include #include AcsParameters::AcsParameters() { @@ -34,6 +34,681 @@ AcsParameters::AcsParameters() { {-1, 0, 0}, { 0, 1, 0}}; + + susHandlingParameters.sus0coeffAlpha = { + {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}}; + susHandlingParameters.sus0coeffBeta = { + {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}}; + susHandlingParameters.sus1coeffAlpha = { + {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, + -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, + -0.000178087038629721, 4.09208968678946e-05}, + {2.24803085641869, 1.42938692406645, 0.30104994020693, 0.00756499999397385, 0.0117765927439368, + -0.000743685980641362, 4.69920803836194e-05, 0.000129815636957956, -9.10792250542345e-06, + -2.03870119873411e-05}, + {26.9943033817917, 0.147791175366868, -3.48256070200564, -0.0303332422478656, + 0.0183377266255394, 0.124593616125966, -0.000466003049304431, -0.000272000698791331, + -0.00063621309529853, -0.00158363678978767}, + {-0.221893380318465, 1.29919955307083, 0.21872487901019, 0.0049448219667127, + 0.00291224091529189, 0.00654651987282984, -9.86842469311185e-05, 8.20057454706638e-05, + 6.42331081725944e-05, 7.11656918299053e-05}, + {1.40178843964621, 1.1733111455249, 0.287485528779234, -0.000793970428759834, + 0.000170529273905818, -0.00268807864923086, 9.09553964483881e-05, -0.000271892733575409, + 8.52016306311741e-05, -0.000291797625433646}, + {0.65549617899457, 1.25716478394514, 0.301396415134214, -0.00357289640403958, + -0.000473416364133431, -0.010760332636205, -9.77220176481185e-05, 4.40798040046875e-05, + 2.84958344955681e-05, 0.000128583400693359}, + {6.20958048145025, 1.9528406481596, 1.32915657614139, -0.0326944423378284, -0.0158258335207969, + 0.0328249756354635, 0.00027113042931131, -0.000133980867173428, -0.000357964552318811, + 0.000224235061786191}, + {2.46222812180944, 1.1731834908026, 0.17440330925151, -0.00132279581980401, + -0.00447202005426964, -0.000804321602550913, -1.59526570766446e-05, 2.62946483533391e-05, + 3.28466749016414e-05, -6.63837547601294e-06}, + {42.615758859473, 2.46617281707273, -5.742515881283, -0.131942799763164, 0.20250702826603, + 0.0981562802911027, 0.00189939440077981, -0.0018591621618441, -0.00161121179693977, + -0.00058814458116749}}; + susHandlingParameters.sus1coeffBeta = { + {-12.300032617206, -1.06640894101328, 0.33950802247214, -0.00890867870617722, -0.04872758086642, + -0.0114263851027856, 0.000141061196404012, -0.000675469545483099, -0.000138249928781575, + -0.000138871036200597}, + {10.1631114109768, 0.261654603839785, 1.2376413405181, 0.00888558138614535, 0.00151674939001532, + -0.00534577602313027, 9.19430013005559e-05, 5.39804599087081e-05, -4.15760162347772e-05, + -7.60797902457032e-05}, + {-30.142329062199, 1.26939195100229, 6.14467186367471, 0.0464163689935328, 0.00379001947505376, + -0.165444163648109, 0.000516545385538741, 1.56053219154647e-05, -5.58651971370719e-05, + 0.00173185063955313}, + {12.1454103989862, -0.243589095509132, 2.02543716988677, -0.000857989774598331, + -0.00705278543432513, 0.0250580538307654, 3.50683653081847e-05, -2.63093897408875e-05, + -5.67352645830913e-05, 0.000232270832022029}, + {4.4338108906594, -0.305276965994378, 1.17293558142526, 0.000152618994429577, + 0.00134432642920902, -0.00104036813342885, 0.000334476082056995, 6.74826804343671e-05, + 0.000275311897725414, 7.58157740577916e-05}, + {3.47680700379043, -0.154163381023597, 1.389579838768, 0.000799705880026268, + 0.00401980026462874, -0.00915311817354667, -2.54817301605075e-06, -2.27422984169921e-05, + -2.61224817848938e-05, 6.00381132540332e-05}, + {29.469181543703, -0.722888948550437, 3.3623377135197, 0.00148445490093232, -0.0474780142430845, + 0.0486755575785462, 0.000126295091963757, 0.000526632230895258, -0.000259305985126003, + 0.000412751148048724}, + {2.67029041722834, -0.0837968038501666, 1.37628504937018, 0.00165061312885753, + -0.00953813055064273, 0.0032433005486936, -1.6522452172598e-05, 0.000144574078261271, + -8.47348746872376e-05, -1.92509604512729e-06}, + {-20.959201441285, -2.23605897639125, 5.73044624806043, 0.0354141964763815, 0.0887545371234514, + -0.193862330062381, 0.000216532998121618, -0.00207707610520973, 0.000552928905346826, + 0.00190182163597828}}; + susHandlingParameters.sus2coeffAlpha = { + {6.51602979328333, 0.690575501042577, 1.18185457002269, -0.0153161662266588, + 0.00145972227341484, 0.0351496474730776, -0.000172645571366945, -6.04213053580018e-05, + 9.74494676304114e-05, 0.000334122888261002}, + {0.954398509323963, 1.10996214782069, 0.313314231563221, -0.00367553051112208, + 0.0110290193380194, 0.000240079475656232, -6.93444423181303e-05, 0.000107433381295167, + 1.30750132315838e-05, -2.43580795300515e-05}, + {-55.1159841655056, -1.47449655191106, 3.40106264596874, -0.0621428271456258, + 0.0659788065633613, -0.0791732068323335, -0.000524264070592741, 0.000582093651418709, + -0.000586102213707195, 0.000658133691098817}, + {1.98614148820353, 1.32058724763677, 0.156843003413303, 0.002748082456053, 0.00202677073171519, + 0.00382360695862248, -0.000122364309010211, 5.33354637965168e-05, 3.93641210098335e-05, + 4.06398431916703e-05}, + {3.41223117010734, 1.1597568029329, 0.31881674291653, -0.000382400010917784, + -0.000754945672515052, -0.00079200882313927, 0.000145713118224563, -0.00026910957285589, + 0.000137876961532787, -0.000326798596746712}, + {6.23333031852853, 1.24902998148103, -0.0162317540018123, -0.00338184464699201, + 0.000420329743164687, 0.00202038442335185, -7.10435889754986e-05, -6.04039458988991e-06, + 7.25318569569788e-06, -2.5930447720704e-05}, + {191.759784636909, -10.5228276216193, 8.48306234734519, 0.243240262512846, -0.344226468125615, + 0.126267158197535, -0.00186612281541009, 0.00304415728817747, -0.00304958575196089, + 0.000457236034569107}, + {5.61375025356727, 1.1692295110657, 0.224665256727786, -0.00230481633344849, + -0.00746693012026367, -0.00172583925345173, -7.00823444553058e-06, 7.31362778266959e-05, + 5.81988007269583e-05, 1.3723604109425e-05}, + {98.0250669452855, -2.18500123986039, -6.68238707939385, 0.000754807832106659, + 0.256133336978808, 0.110826583415768, 0.000457663127670018, -0.00197655629847616, + -0.00254305206375073, -0.000466731538082995}}; + susHandlingParameters.sus2coeffBeta = { + {41.1102358678699, 2.3034699186519, 2.74551448799899, 0.061701310929235, 0.0317074142089495, + 0.0308171492962288, 0.00049453042200054, 0.000519222896270701, 2.85420168881716e-05, + 0.000259197384126413}, + {4.46821725251333, 0.0125273331991983, 1.32640678842532, 0.000543566569079156, + 0.00396616601484022, -0.00488408099728387, -3.05734704054868e-06, 7.3424831303621e-05, + -5.49439160235527e-05, -8.30708110469922e-05}, + {64.773396165255, 2.97057686090134, -1.90770757709096, 0.062747116236773, -0.077990648565002, + 0.0613989204238974, 0.00055512113297293, -0.000347045533958329, 0.00104059576098392, + -0.000348638726253297}, + {3.10352939390402, -0.2376108554276, 1.60523925160222, 0.00116454605680723, -0.0067958260462381, + 0.0136561370875238, 2.59929059167486e-05, 3.33825895937897e-05, -5.55828531601728e-05, + 0.000109833374761172}, + {0.156052891975873, -0.320721597024578, 1.15208488414874, 0.00164743688819939, + 0.000534718891498932, 0.000469870758457642, 0.000308432468885207, 0.00011789470679678, + 0.000292373398965513, 0.000183599033441813}, + {2.84967971406268, -0.21374251183113, 1.09938586447269, 2.34894704600407e-05, + 0.00588345375399262, 0.00296966835738407, 8.42707308834155e-06, 2.81870099202641e-06, + -3.56732787246631e-05, -7.04534663356379e-05}, + {-7.59892007483895, 0.358662160515702, 0.805137646978357, 0.00222144376998348, + 0.0464438387809707, 0.00847551828841782, 3.24805702347551e-05, 5.45500807838332e-05, + 0.000941378089367713, 0.000353137737023192}, + {-4.65367165487109, 0.201306010390421, 1.19135575710523, -0.00692801521395975, + 0.00394118754078443, 0.00426360093528599, 6.297683536736e-05, -7.15794236895102e-05, + -7.47076172176468e-05, -1.94516917836346e-05}, + {-59.5882618930651, 3.84530212586425, 3.50497032358686, -0.116100453177197, -0.0380997421813177, + -0.0581898335691109, 0.00111464935006159, 0.000559313074537689, 0.000168067749764069, + 0.000563224178849256}}; + susHandlingParameters.sus3coeffAlpha = { + {-174.687021034355, -7.53454036765748, -9.33798316371397, -0.18212338430986, -0.242523652239734, + -0.202086838965846, -0.00138648793335223, -0.00225430176012882, -0.00198887215340364, + -0.00160678535160774}, + {6.92009692410602, 1.8192037428209, 0.254908171908415, 0.0179273243472017, 0.00894059238779664, + -0.000436952529644, 0.000138070523903458, 9.22759645920339e-05, -9.4312261303588e-06, + -1.76647897892869e-05}, + {-17.9720006944368, 0.230510201259892, 1.10751755772907, -0.00973621304161327, + 0.0554450499198677, -0.00590970792122449, -2.92393772526592e-05, 0.000444329929586969, + -0.000436055839773919, -9.5869891049503e-05}, + {-4.9880829382985, 1.33627775121504, -0.330382157073243, 0.00306744056311184, + 0.00376353074674973, -0.0107453978169225, -0.00010680477021693, 5.17225535432745e-05, + 7.4423443938376e-05, -0.000107927900087035}, + {0.952867982900728, 1.14513280899596, 0.307744203675505, 0.000404669974219378, + -0.000737988606997615, 0.00120218232577844, 0.000191147653645603, -0.000275058867995882, + 0.000137187356620739, -0.000320202731145004}, + {8.076706574364, 1.31338618710295, -0.334634356394277, -0.00209719438033295, + -0.00381753503582303, 0.0100347823323616, -7.00550548221671e-05, -1.97626956996069e-05, + 7.80079707003333e-05, -8.95904360920744e-05}, + {-82.4748312650249, 8.63074484663009, -0.949295700187556, -0.178618807265278, 0.130143669167547, + 0.0284326533865768, 0.00149831261351137, -0.0011583692969717, 0.0010560778729661, + 0.000635404380970666}, + {3.34457857521978, 1.09242517408071, 0.241722402244944, 0.00381629887587041, + -0.00863580122530851, 0.00137050492069702, -8.91046701171713e-05, 8.44169683308007e-05, + -3.54608413548779e-05, 8.54042677832451e-06}, + {78.1540457908649, -1.30266922193303, -5.33605443700115, 0.0184226131926499, 0.146629920899062, + 0.110698519952472, 6.64041537651749e-05, -0.00120174584530713, -0.00133177694921411, + -0.000796422644338886}}; + susHandlingParameters.sus3coeffBeta = { + {-31.5704266802979, -5.10700699133189, 2.84549700473812, -0.122701561048957, -0.11257100034746, + 0.102120576206517, -0.000796645106694696, -0.00192211266325167, -4.99981232866237e-05, + 0.00104036677004523}, + {-0.734294938181273, -0.0694317595592039, 1.34746975389878, -0.00103465544451119, + 0.00389798465946559, -0.00308561832194191, -2.91843250099708e-06, 7.59634622232999e-05, + -6.54571602919161e-05, -0.000104146832644606}, + {24.2649069708536, 3.08145095664586, 1.88975821636026, 0.0767528234206466, -0.0526971951753399, + -0.0477053831942802, 0.000613806533422364, -0.000631628059238499, 0.00026217621127941, + 0.000555307997961608}, + {0.62884078560034, -0.152668817824194, 1.70304497205574, 0.000894387499536142, + -0.00306495168098874, 0.0180087418010658, 1.74990847586174e-05, 3.1263263531046e-05, + -7.1643235604579e-06, 0.000147876621100347}, + {-3.05400297018165, -0.316256447664344, 1.14841722699638, 0.000671621084688467, + -0.000906765726598906, 0.000687041032077189, 0.000323419818039841, 0.000128019308781935, + 0.000286018723737538, 0.000192248693306256}, + {-4.39855066935163, -0.322858945262125, 1.44405016355615, -4.93181749911261e-05, + 0.0127396834052722, -0.00523149676786941, 2.56561922352657e-05, 7.61202764874326e-06, + -0.00014623717850039, 8.12219846932013e-06}, + {110.820397525173, -10.9497307382094, 2.48939759290446, 0.296585618718034, -0.142611297893517, + -0.0141810186612052, -0.00275127095595919, 0.00160686698368569, -0.000872029428758877, + -0.000410522437887563}, + {-7.15740446281205, 0.104233532313688, 1.13155893729292, -0.00350418544400852, + 0.00532058598508803, 0.00459314980222008, 3.09155436939349e-05, -7.60935741692174e-05, + -5.87922606348196e-05, 2.56146268588382e-05}, + {44.8818060495112, -7.94729992210875, 3.59286389225051, 0.217944601088562, 0.108087933176612, + -0.116711715153385, -0.00194260120960441, -0.0015752762498594, -0.000331129410732722, + 0.00125896996438418}}; + susHandlingParameters.sus4coeffAlpha = { + {-12.4581187126738, 0.398038572289047, -0.438887880988151, -0.00965382887938283, + -0.0309322349328842, -0.00359106522420111, -7.79546112299913e-06, -0.000432733997178497, + -9.79031907635314e-05, -1.49299384451257e-05}, + {8.41054378583447, 1.87462327360707, 0.266809999719952, 0.0216455385250676, 0.00879426079919981, + -0.00142295319820553, 0.000194819780653264, 8.57549705064449e-05, -3.56478452552367e-05, + -1.65680920554434e-05}, + {16.4141780945815, 2.57697842088604, 0.373972171754278, 0.0498264199400303, 0.0183175817756131, + -0.008545409848878, 0.000422696533006382, -0.000268245978898508, -0.000663188021815416, + -7.51144017137827e-05}, + {0.796692054977593, 1.26773229735266, 0.247715261673662, 0.00358183885438128, + 0.00216435175662881, 0.00713732829335305, -0.000110129715615857, 3.56051594182427e-05, + 5.03074365340535e-05, 8.40279146176271e-05}, + {2.37491588500165, 1.05997969088519, 0.309540461340971, -0.000405047711742513, + 0.000462224730316111, -0.00201887171945793, 0.000260159805167265, -0.000282867209803598, + 0.000201613303652666, -0.000277796442847579}, + {6.36749007598708, 1.31659760017973, -0.122724934153231, -0.00328808937096891, + -0.00577347207798776, 0.00403172074457999, -7.45676459772001e-05, 1.79838644222274e-05, + 0.000104552066440564, -2.78115121929346e-05}, + {-47.9667098848496, 3.97703197139796, -1.96403894754299, -0.0577989657406978, + 0.0634225576208007, -0.0346023445055141, 0.00045886475369098, -0.000326132951996844, + 0.000716490441845967, -0.000136132038635483}, + {6.21505474256094, 0.871830486201601, 0.286906473833627, 0.007875292606045, + -0.00974634725746389, 0.00128416935792136, -0.000111796743751489, 0.000102016719989187, + -3.3503088289589e-05, -1.03874407813931e-05}, + {102.09801265482, -4.12715152309748, -5.04594403360339, 0.075499959116996, 0.216574192561683, + 0.0750031215784663, -0.000147358932612646, -0.0023710703422108, -0.00143310719642393, + -0.000431914403446768}}; + susHandlingParameters.sus4coeffBeta = { + {-21.5077132684032, -1.60004839699939, -0.0298995033958561, -0.0315563250430659, + -0.0424403625879891, -0.0245426225510417, -0.000209861203016225, -0.000422150973104431, + -0.00030514398458781, -0.000211986731019738}, + {9.07644247897601, 0.207457289788099, 1.26735366597312, 0.00768477352180427, + 0.00429230749575816, -0.00514802326062087, 7.56149591998578e-05, 8.42794730840662e-05, + -3.62215715492783e-05, -5.24384190165239e-05}, + {-33.5225408043693, -3.11167857248829, 1.91760591695775, -0.0963752386435729, + 0.00026620241534153, -0.0256680391021823, -0.00102188712837393, 2.63753563968978e-05, + 0.000113172463974702, 0.000271939918507175}, + {19.1379025029401, -0.225979661987912, 2.72337120022998, -0.00136982412154458, + -0.00447301210555274, 0.046496718064139, 2.09123846958985e-05, -4.30383094864847e-05, + -1.22808643520768e-05, 0.000440555709696048}, + {2.957867714783, -0.316069593806939, 1.06379930645214, 0.00103244713047271, 0.00148059212230411, + 0.000557885068990542, 0.000288633931072557, 0.000172775380291659, 0.000269738457990237, + 0.000254577019084984}, + {2.04155199929521, -0.318303488378033, 1.37820715117028, 0.00114788656817743, + 0.0130051117909245, -0.00743109928493789, 1.22403390396844e-05, -3.19245785131217e-05, + -0.000156735218010879, 3.81458400945988e-05}, + {27.314954181241, -1.43916155634084, 2.48967706992348, 0.0278695408478388, -0.0341141456915131, + 0.0281959785297513, -0.000252996164135396, 0.000163365679366542, -0.000380129463154642, + 0.000159350154429114}, + {-0.274693278266294, 0.0199711721436635, 1.26676843352524, -0.0006713759238817, + -0.00389715205101059, 0.00294298337610857, -9.58643121413979e-06, 6.30700938550725e-05, + -6.07188867796123e-05, 7.72199861279611e-06}, + {-74.1601853968901, 2.55641628908672, 6.38533530714782, -0.0582345132980647, + -0.0653804553172819, -0.138850555683872, 0.000489364157827405, 0.000469559629292745, + 0.000698140692952438, 0.00123017528239406}}; + susHandlingParameters.sus5coeffAlpha = { + {-12.1398741236355, 1.99425442858125, -1.9303044815802, 0.0418421279520049, -0.0309683799946315, + -0.0562201123081437, 0.000522607299552916, -0.000375386573815007, -0.000183899715035788, + -0.000600349486293698}, + {4.51862054729553, 1.72396080253297, 0.274562680698765, 0.0162681383591035, 0.0108410181586673, + -0.000272215427359511, 0.000124164068046579, 0.000125586897851351, -1.24082224214974e-05, + -1.63339067540159e-05}, + {63.0100748193658, 7.78014670478172, 0.327263471268564, 0.181264302704374, -0.0652454854214506, + -0.03906716801285, 0.00166924078925478, -0.000749939315526625, 0.000320696101132374, + 0.000499934751180042}, + {-2.14377722994325, 1.33617641673436, 0.0973465660282871, 0.00389526886867845, + 0.00526064997381395, 0.00244964888333519, -8.59416490903541e-05, 4.58871931007681e-05, + 8.6123353128647e-05, 2.85447259858337e-05}, + {0.164792977301912, 1.17541977248641, 0.348838798760518, -0.000180865118417534, + 0.000331789515553421, -0.000734333865631793, 9.76677859410759e-05, -0.000324347075049525, + 8.66683396011167e-05, -0.000385839566009832}, + {-0.228934187493575, 1.30552820143752, 0.306779576899158, -0.00508763741184706, + -0.00318524263093038, -0.00878095392529144, -6.59040013073836e-05, 8.69122529321691e-05, + 5.73853071731283e-05, 8.56628414466758e-05}, + {22.6047744510684, -0.591739857860868, 0.566728856847393, 0.0498124268150265, + -0.0214126910277926, 0.00538091942017912, -0.000391517685229849, 0.000554321668236216, + 0.000191004410219065, 0.000102775124022018}, + {4.54704081104052, 0.844841244606025, 0.181355971462193, 0.0109743851006749, + -0.00363467884122547, 0.00108873046814694, -0.000153236888951059, 3.14623342713789e-06, + -2.78503202185463e-05, 3.99983788680736e-06}, + {-30.878359404848, 5.20536009886854, -0.674455093700773, -0.10801865891189, -0.0514805639475938, + 0.0503660452068572, 0.00072776817295273, 0.00120288537038655, -0.000301602375634166, + -0.000477098479809266}}; + susHandlingParameters.sus5coeffBeta = { + {16.8155737032787, 0.65475660868259, 1.95532810363711, 0.000295624718662669, 0.0426379914736747, + 0.00192544771588337, -4.94534888281508e-05, 8.32299142575155e-05, 0.000645497238623369, + -0.000234155227840799}, + {9.48268090632318, 0.528942263930744, 1.34030963800712, 0.0173605129814363, 0.00581086655972212, + -0.00365006277801141, 0.000180048140973223, 0.000102002650672644, -4.10833110241736e-05, + -8.7810396165556e-05}, + {-47.8325489165383, -4.78262055949503, 1.66912859871505, -0.143518014673292, 0.0288441527062856, + -0.00322823115861497, -0.00148509910480755, 0.000284265179004289, -0.000175299737313045, + -7.04175618676909e-05}, + {3.70510151312723, -0.272200626024415, 1.5527519845099, 0.000589727630962265, + -0.00889682554869096, 0.0109857452472628, 3.05876215574877e-05, 2.09194236165814e-05, + -8.33769024439277e-05, 6.90991113575066e-05}, + {0.820199776906695, -0.355683467192776, 1.17142130858009, -0.000160174871610729, + 4.09723480153701e-05, 0.000209103751629257, 0.000390331989170637, 6.45642836249667e-05, + 0.000318092703362044, 0.000107158633760141}, + {5.52084497768914, -0.227775345312466, 0.845897282556327, 0.00157426476122436, + 0.00657189797805861, 0.0103797665963117, 2.51479848048895e-05, -4.78371400399983e-05, + -5.20221896473413e-05, -0.000143840492906166}, + {-33.4875689683454, 0.937557276329106, -1.02741065470967, -0.0140023273976314, + 0.0401908729477037, -0.0512457211360142, 7.05537967426573e-05, -0.00027521752411122, + 0.000407657552700476, -0.000458411000693613}, + {0.931346887326171, -0.320804452025793, 1.28866325376154, 0.00912456151698805, + -0.00404367403569981, 0.00477543659981282, -9.43987917474817e-05, 4.66464249533497e-05, + -7.89362487264572e-05, -1.0951496495443e-05}, + {-38.3689359928435, 3.8540516906071, 1.26391725545116, -0.108584643500539, -0.0542697403292778, + 0.0285360568428252, 0.000845084580479371, 0.00114184315411245, -0.000169538153750085, + -0.000336529204350355}}; + susHandlingParameters.sus6coeffAlpha = { + {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}}; + susHandlingParameters.sus6coeffBeta = { + {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}}; + susHandlingParameters.sus7coeffAlpha = { + {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, + -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, + -0.00119021101127241, -0.000460110780350978}, + {1.60822506792345, 1.12993931449931, 0.300781032865641, -0.00405149856360946, + 0.0116663280665617, -0.000746071920075153, -8.36092173253351e-05, 0.000126762041147563, + -1.57820750462019e-05, -2.13840141586661e-05}, + {-151.403952985468, -5.77049222793992, 9.71132757422642, -0.113259116970462, 0.284142453949027, + -0.198625061659164, -0.000836450164210354, 0.00174062771509636, -0.00323746390757859, + 0.00124721932086258}, + {3.47391964888809, 1.28788318973591, 0.358380140281919, 0.0033863520864927, 0.00154601909793475, + 0.0103457296050314, -9.56426572270873e-05, 5.48838958555808e-05, 2.97537427220847e-05, + 0.000104735911514185}, + {3.32650947866065, 1.16701012685798, 0.293514063672376, -0.00065850791542434, + -8.61746510464303e-05, -0.00212038990772211, 0.00010377123197, -0.000262818127593837, + 0.000103360882478383, -0.000296739688930329}, + {-0.440176043435378, 1.18923278867097, 0.519516382652818, -0.00138846714677511, + 0.00266491699926247, -0.014254675949624, -4.20279929822439e-05, -5.49260281515447e-05, + -1.00328708454487e-05, 0.000138142092498215}, + {9.54962966738358, 1.83809145920811, 1.82162819067959, -0.0116786627338505, + -0.00496037444422313, 0.0590883547819332, 7.48465315787857e-05, 0.000221693951602584, + 7.96466345174136e-06, 0.000638822537725177}, + {7.04862901290925, 0.876813777672465, 0.16368093989381, 0.00928717461441627, + -0.00276538956293246, 0.00117995419940653, -0.000141511492474493, -6.09796031786385e-06, + -2.62114930414747e-05, -2.88713611443788e-06}, + {135.349147631811, -7.21933296299596, -6.02379024934871, 0.19557354282067, 0.207680233512614, + 0.12880101618361, -0.00169832076532024, -0.00192216719797732, -0.00188763612041332, + -0.00103101801961442}}; + susHandlingParameters.sus7coeffBeta = { + {-12.7115487367622, -1.08890790360556, 0.0579616268854079, -0.0212303293514951, + -0.0395948453851818, -0.0275564242614342, -0.000228652851842222, -0.000148106159109458, + -0.000555136649469199, -0.000198260004582737}, + {-0.988147625946871, -0.759018567468546, 1.20998292002818, -0.0241231836977845, + -0.000572110443300516, -0.00294835038249426, -0.00026533039022186, 6.82250069765274e-06, + 7.21038415209318e-06, -6.54881435118179e-05}, + {98.0979345921564, 4.27381413621355, -4.39956005193548, 0.0709109587666745, -0.172774236139236, + 0.107243391488741, 0.000421832640471043, -0.00140450884710288, 0.00158019019392239, + -0.00078512547169536}, + {4.10892685652543, -0.229301778557857, 1.33380992987117, -0.000250095848720304, + -0.00555205065514645, 0.00355052914398176, 1.62727119770752e-05, -1.26026527654764e-05, + -3.25505031810898e-05, 5.79970895921158e-06}, + {3.09432502337258, -0.300556003790433, 1.17085811008124, 0.00128679594824324, + 0.00148229981422985, 9.15267474159147e-05, 0.000300497843413856, 6.31378865575566e-05, + 0.000258447032558814, 9.79142983264352e-05}, + {8.92336134924575, -0.197306981784312, 0.659908505354084, 0.00175572239373996, + 0.006801023678097, 0.0189775987436792, 9.2187857727721e-06, -4.8706332690626e-05, + -6.887009887486e-05, -0.000266455617735054}, + {-52.0734887320227, 2.64822385560272, -1.72387600304694, -0.0383944891609251, 0.110873671161269, + -0.0475247245070445, 0.000194652401328063, -0.000697307928990137, 0.00124021816001, + -0.000194213899980878}, + {2.08203985879155, -0.127503525368396, 1.17628056094647, 0.00283288065938444, + 0.00394668214608305, 0.00314868636161131, -2.99504350569853e-05, -7.11070816314279e-05, + -6.30148122529749e-05, 2.28114298989664e-05}, + {191.321181158032, -12.2449557187473, -7.21933741885107, 0.267954293388644, 0.331529493933124, + 0.149867703984027, -0.00222279201444128, -0.00284724570619954, -0.00298774060233964, + -0.000988903783752156}}; + susHandlingParameters.sus8coeffAlpha = { + {5.46354311880959, 1.15370126035432, 0.568432485840475, -0.00105094692478431, + -0.000472899673842554, 0.015581320536192, 2.26460844314248e-05, -0.000254397947062058, + 0.000198938007250408, 0.000102026690279006}, + {8.8976133108173, 1.89502416095352, 0.268670471819199, 0.0217013413241972, 0.00973925295182384, + -0.00116357269193765, 0.000185865842232419, 0.000103311614912702, -2.46539447920969e-05, + -2.06292928734686e-05}, + {-45.4550803910752, 1.27220123406993, 5.21483855848504, 0.0315791081623634, 0.0725172355124129, + -0.13947591535243, 0.000412577580637848, 0.000434545096994917, -0.000840043932292312, + 0.00126857487044307}, + {1.81302768546433, 1.20563501267535, 0.344815267182167, 0.00546879453241056, + -0.00115382996865884, 0.010597876132341, -7.75885604486581e-05, 8.99568815949154e-05, + -2.98129544974679e-06, 0.000108913239345604}, + {2.19111439539173, 1.06951675598148, 0.283707798607213, 0.00016478588207518, + 0.000196086067268121, -0.00214980231173703, 0.000237820475654357, -0.000256402967908595, + 0.000165966620658577, -0.000268394081675921}, + {15.0858674915897, 1.27922724811168, -1.0803137812576, -0.00184009775302466, + -0.00458792284209219, 0.0359393555418547, -6.05121024079603e-05, -1.2288384024143e-05, + 8.55484605384438e-05, -0.000379241348638065}, + {-14.9594190080906, 1.79473182195746, -1.00830704063572, 0.000890685410857856, + 0.0408932029176081, -0.0165460857151619, -0.000170544299916973, -0.000370901607010145, + 0.000324089709129097, -9.33010240878062e-05}, + {0.867614491733251, 1.38248194737027, 0.233408537422123, -0.00772942878114575, + -0.00783126068079782, -0.000413713955432221, 4.5775750146291e-05, 6.97323029940275e-05, + 1.70664456940787e-05, 6.75517901233086e-06}, + {2.34474364146174, -0.777275400251477, 2.09531381577911, 0.0170780716714389, 0.102855060371092, + -0.1203441505925, 0.000187004964420911, -0.00141720441050986, -0.000336251285258365, + 0.00145175125888695}}; + susHandlingParameters.sus8coeffBeta = { + {28.3033101237397, 1.77504446792811, 1.70758838986317, 0.0307800697044683, 0.0598759344275936, + -0.014461432284373, 0.000128415617799076, 0.000664419128546701, 0.000312923304130995, + -0.000269026446641855}, + {7.73040563051023, 0.0267291479555493, 1.16189582308493, 0.000611047892976521, + -0.00213680506915073, -0.00517435586596902, -3.60304406049766e-06, -1.74452976404459e-05, + -3.95396925228538e-05, -7.01948519410633e-05}, + {-48.0766126130725, -3.77981206700298, 3.03482861087335, -0.0678496412519532, 0.115260678424016, + -0.0109681510065038, -0.000438011443691466, 0.00097230136258486, -0.000930875177732769, + -0.000203144239955507}, + {12.1881935626341, -0.234345089308583, 2.01134619426134, 0.000181529284001169, + -0.00642848065105061, 0.0243985799415726, 2.0224042581776e-05, 5.22503286757285e-06, + -4.75196303016323e-05, 0.000221160482364556}, + {3.49559433498742, -0.294995112674766, 1.07892379698257, 0.000861664794052587, + 0.00138978933062055, 0.000436385106465176, 0.000288095124755908, 0.000147259769247883, + 0.000256686898599516, 0.000198982412957039}, + {9.36663996178607, -0.171266136751803, 0.799869891484541, -0.000896305696610864, + 0.00477919972789653, 0.0077876110326094, 9.16475263625076e-06, 3.02461250100473e-05, + -3.63917701783264e-05, -0.000101376940843402}, + {9.93372683055145, 1.02056557854246, 3.01635426591734, -0.0477113881140277, -0.0280426434406976, + 0.0438813017696874, 0.000470431190169998, -7.55753674679743e-05, -0.000516444906323815, + 0.000253844418223843}, + {4.12868774589741, -0.305711790187688, 1.15739216407191, 0.00942395115281056, + 0.00264758462357433, 0.00227985850688479, -0.000107537164019682, -4.91396736189963e-05, + -5.3803493814502e-05, 6.80587059728718e-06}, + {64.9193383444005, -1.57724255547465, -3.82166532626293, 0.0104712238987591, 0.0898786950946473, + 0.128910456296131, -8.27123227422217e-05, -0.000143979624107479, -0.00146684876653306, + -0.00102226799570239}}; + susHandlingParameters.sus9coeffAlpha = { + {65.8975109449121, 2.19115342242175, 6.11069527811832, -0.0219884864133703, 0.119985456538482, + 0.142746712551924, -0.000465882328687976, 0.000606525132125852, 0.00141667074621881, + 0.00109715845894006}, + {5.70337356029945, 1.86705636976809, 0.235584190291708, 0.0194937327615426, 0.00973291465247784, + -0.00155675297510773, 0.000147099297988423, 0.000115708967219349, -4.1462310493722e-05, + -9.80097031103588e-06}, + {138.221145997284, 6.07665575619595, -9.08085914250542, 0.0839801072927519, -0.143071750033303, + 0.237868300719915, 0.000626693630444932, -0.000579788170871402, 0.00181740650944343, + -0.00207086879728281}, + {-7.78295582666151, 1.37506685179192, -0.507596181420042, 0.00350118305456038, + 0.00380814310115541, -0.0174012437563343, -0.000124801268056815, 2.96314830184492e-05, + 6.3416992450033e-05, -0.000190177262510221}, + {0.13102597129751, 1.24228303845143, 0.328808873447393, 2.6858679536165e-05, + 0.000231428138164498, -0.000584089095259736, 5.5322167970451e-05, -0.000322205709821716, + 7.71348293209208e-05, -0.000393885990364776}, + {4.64571633968935, 1.2668223691397, -0.158952088650432, -0.0038344859267428, 0.0030051503726095, + 0.00455578826025588, -9.42520993914957e-05, 5.81633314412289e-05, -4.43545804544095e-05, + -4.83524454851519e-05}, + {99.2385930314563, -3.65569343617926, 5.54203926675588, 0.0975630395981933, -0.15701634159692, + 0.107834711298836, -0.000885326636237814, 0.000960753844480462, -0.00179894024848343, + 0.000583066757644971}, + {2.82671549736619, 1.11214198870501, 0.214735318432744, 0.00284415167563662, + -0.00743289575690122, 0.000382705440762292, -7.43232442872501e-05, 6.96994098083348e-05, + -4.15108111710131e-06, 1.33520085213482e-05}, + {36.9013743125415, -0.522392401546163, -1.52452843963663, 0.0261375433218879, 0.060573568610239, + 0.0182582125221054, -0.000244373383911157, -0.000271385147292484, -0.000723799969427732, + 6.76324880239196e-05}}; + susHandlingParameters.sus9coeffBeta = { + {128.70886435409, 7.27355509732751, 7.18142203531244, 0.1536100459329, 0.199455846541636, + 0.101824964939793, 0.00116666116789421, 0.00181595584079788, 0.00159271319494017, + 0.000556768406475719}, + {-7.07933839681024, -0.979062424441878, 1.21792546815617, -0.0295740143783226, + -0.00442780611714201, -0.00329612819203176, -0.000291373125216143, -7.47259350176359e-05, + -4.87265282482212e-05, -7.87490350444332e-05}, + {41.1357193180502, 2.75138456414254, -0.0160889117718198, 0.0274001112562423, + -0.145644717742057, -0.0316076203283094, -0.000136443337244472, -0.00153945199081365, + 0.000938960439977633, 0.000599987111822885}, + {2.7980384746608, -0.234741037383589, 1.5342193016705, -0.000993791566721689, + -0.00787533639513478, 0.00927468655141365, 2.63308697896639e-05, -3.42816267184975e-05, + -8.48879419798771e-05, 3.84043821333798e-05}, + {0.427687530667804, -0.346076633694936, 1.22968527483851, -4.95098138311122e-05, + 0.000298245372198029, 0.000332756250024796, 0.00040375986210644, 5.20675972504572e-05, + 0.000327042170278218, 5.93011568264671e-05}, + {4.50337810133314, -0.279364254817202, 0.945812187846199, 0.000116182663432306, + 0.0115646046622083, 0.00908289960302886, 1.90394667311541e-05, -4.4360223646434e-06, + -0.000131398914898614, -0.000145568992865512}, + {-36.3377213654193, 2.21047221783626, 0.0609982245149821, -0.0670546774988572, + 0.016827777144747, -0.0277834084058314, 0.000778301409125556, 0.000135846745194401, + 0.00043261858797068, -0.00021172728254561}, + {-0.737678205841529, -0.217352122193475, 1.23494846329297, 0.00748173441779792, + 0.0019595873704705, 0.00567253723266176, -8.34768773292938e-05, -3.50608394184873e-05, + -0.000107500091550635, -5.1379722947632e-07}, + {-36.6150844777671, 3.24952006904945, 1.7222457840185, -0.0846362445435584, -0.0625549615377418, + 0.019178365782485, 0.000664877496455304, 0.000942971403881222, 0.000190754698755098, + -0.000372226659190439}}; + susHandlingParameters.sus10coeffAlpha = { + {14.4562393748324, 0.669162330324919, 2.13895255446541, -0.0161997097021299, + 0.00185995785065838, 0.0621351118528379, -0.000278999272493087, 0.000238469666491965, + -0.000279407497782961, 0.000726904943739837}, + {-4.45678285887022, 0.92869611919737, 0.186752102727282, -0.00706160758952316, + 0.00532680276723634, -0.00119102617674229, -0.000105283880098953, 3.90673052334419e-05, + -3.13338277344246e-05, 5.32977236959767e-06}, + {30.4255268053197, 3.00991076401191, -1.4855621363519, 0.033934286288413, -0.0553588742704929, + 0.0299275582316466, 0.000167915322354466, -0.00050925078118232, 0.000463662961330962, + -0.000232919143454163}, + {2.45076465343337, 1.30206564388838, 0.635121046212765, 0.00517109639797675, + 0.00360579544364496, 0.0198490668911362, -9.31556816982662e-05, 6.7313653707875e-05, + 6.4669137025142e-05, 0.000209727581169138}, + {-0.784841314851562, 1.10058314980836, 0.314063830836532, 0.000583003703415889, + 0.000312635453606579, -0.000183738114552387, 0.000214096205760617, -0.000286744686021244, + 0.000159157597180407, -0.00032235099420715}, + {7.19568036510586, 1.33307479701657, -0.465585141952456, -0.0031910726544199, + -0.00546273504371797, 0.0145494754402526, -7.9863949693769e-05, 4.83681329120104e-05, + 8.85844309936609e-05, -0.000143217870916994}, + {-12.8344546267449, 1.36023633150143, -0.728527724854506, 0.019982118403416, 0.0385056413989437, + -0.00468598272326268, -0.000303957957649245, -6.37783846968216e-05, 0.000514049116643205, + 0.000112015427600697}, + {-2.58279031298065, 1.42167821629586, 0.208769467511292, -0.00640190372145885, + -0.0056405289717473, 0.000509611313918708, 2.23310562107823e-05, 3.23685469522147e-05, + -7.55982776243849e-06, 2.78417756661088e-06}, + {-29.7178996143914, 2.636972251183, 1.97316329325243, -0.03983524158327, -0.0193152048730234, + -0.0600902798379509, 0.00031786916010672, 0.000162178988605602, 0.000224550786416246, + 0.000614337977361927}}; + susHandlingParameters.sus10coeffBeta = { + {12.4771349792459, 1.44317849705414, 0.975637226331561, 0.0430284146301043, 0.0220810531548995, + -0.0220926906772, 0.000310052324529521, 0.000658151808869523, -0.000288026365111098, + -0.000214619731807045}, + {-0.113203260140131, -0.272424061092191, 1.27704377191184, -0.00791746619331075, + 0.00278646694862191, -0.00398881099259934, -8.09569694307212e-05, 5.99617384829016e-05, + -5.4550919751855e-05, -8.6314530565085e-05}, + {-48.585664295448, -2.04899787231903, 4.48757129623549, -0.0226180460431321, 0.090326735447661, + -0.0722998813632622, -6.77623771415477e-05, 0.000562585419036509, -0.000956171370931993, + 0.000491554402311223}, + {-1.20986884955482, -0.215604107185474, 1.22123198786617, 0.000256508527822089, + -0.00625056735692847, 0.00262961582224303, 2.27433984698861e-05, 1.60471509861372e-05, + -4.85061736834262e-05, -1.8387092782907e-06}, + {-0.250205907903409, -0.315819331560782, 1.09018364376391, -0.000521787614293089, + -0.000500747760913489, 2.48184307342838e-05, 0.000313799238640988, 0.000136669146368744, + 0.000278914324565192, 0.000218512838469476}, + {-1.38512578184076, -0.240456589364121, 1.34170304231345, 0.00017499230372669, + 0.0070862275911073, -0.00460640844814105, 1.27594111036696e-05, -4.73855624902052e-06, + -5.41141037812903e-05, 8.50767021818388e-06}, + {58.9917559342216, -2.28705697628345, 5.35995190407842, 0.0214721399750612, -0.112195722921667, + 0.0890150265857542, -0.000100675657768708, 0.000493488022135339, -0.00137672908303878, + 0.000518683157694955}, + {3.18905073365834, -0.633376549706314, 1.17190259811174, 0.0188817945597344, + 0.00107470708915782, 0.00400880471375267, -0.000197312295539118, -2.46543035998379e-05, + -6.07871064300252e-05, 1.91822310311955e-05}, + {-21.6881499304099, -0.563186103920008, 3.70747028664292, 0.021112883967427, + -0.00650020689049325, -0.0778533644688476, -0.000131921888670268, -0.000402754836445439, + 0.000551249824375055, 0.00062236627391337}}; + susHandlingParameters.sus11coeffAlpha = { + {-5.23569698615548, -1.45500092391928, 2.7643243644756, -0.0762912296128707, + -0.0201645929971608, 0.0997226845779083, -0.000741669441569556, -0.000485368004931528, + 0.000166230212359982, 0.00103455037278067}, + {-7.7405077383712, 0.892040861541276, 0.39014957203484, -0.00952030929935314, + 0.0185577462685363, 0.000500600568760257, -0.000151227821554572, 0.000245334737283439, + 1.89380065823205e-05, 1.83481122973969e-07}, + {-27.462143709831, -1.68192474287299, 0.689411302961069, -0.0146021086710062, 0.227153492753605, + 0.0815806579791421, 2.92919049429149e-05, 0.00153760357651792, -0.00247865821192621, + -0.00166333309739387}, + {-6.74664748624368, 1.43279156053015, 0.0212787292914553, 0.00764792230365732, + 0.00796410301290615, 0.0014384998868733, -8.95239151813685e-05, 9.55245417090909e-05, + 0.000127251739461239, 3.26943341606363e-05}, + {-2.20391533717674, 1.32902400478083, 0.38633027011889, 0.00104660852197061, + 0.00105228824412283, -0.00242067551428214, -6.98346290136652e-05, -0.000369075232184835, + -1.59510520000704e-05, -0.000448565104826966}, + {-5.29476778147188, 1.4549556336236, 0.561334186252557, -0.00260896342687109, + -0.00855934179001141, -0.0182515354646452, -8.79385828606048e-05, 5.98357681659175e-05, + 0.000146570207542509, 0.000201596912834554}, + {-45.7906613832612, 3.77630104475902, -1.67494598155515, -0.0545433897761635, 0.047897938410221, + -0.0355687158405231, 0.000374738707508583, -0.000448453494537518, 0.000377784972619365, + -0.000276573228333836}, + {-9.11681182090372, 2.06933872940742, 0.26131496122122, -0.0259534033367855, + -0.00777266937872862, -0.00262135395843891, 0.000223790782655445, 6.40488537928934e-05, + 7.75581514100296e-05, -9.25934285039627e-06}, + {183.243883340634, -8.02281039502717, -10.0756951652703, 0.168750521462303, 0.314006821405967, + 0.200264755034566, -0.0011895153717447, -0.00253812476819896, -0.00291324393641628, + -0.00140062522117514}}; + susHandlingParameters.sus11coeffBeta = { + {34.4499366074013, -0.438583698052091, 4.72111001451028, -0.041810050989433, 0.0562461093661426, + 0.0856849892524893, -0.000477813051406167, -3.16404257494464e-05, 0.00102633196865105, + 0.000552974013759876}, + {7.6366298088699, 0.150314752958302, 1.31364679484924, 0.00557696667395871, 0.00163731860604376, + -0.00454759608980269, 5.83979683674572e-05, 4.45944881220665e-05, -4.27874746147066e-05, + -8.77418673597557e-05}, + {130.156215011893, 1.85759000444524, -10.986892391833, -0.00686275191260681, -0.188837138116058, + 0.346177462085361, -0.000183276946352264, -0.000702183496893294, 0.00293145272693857, + -0.00318194442670715}, + {-1.67854820161036, -0.358899332859806, 0.956690839640595, -4.93862910503803e-05, + -0.0136134783014874, -0.00848731301504507, 3.75950499927045e-05, 1.35374694383289e-06, + -0.000156596507890443, -0.000123254220377897}, + {3.67569209537767, -0.387260959713287, 1.31343215605952, -0.00206444615206506, + 0.00145334813110285, -0.00151259497696238, 0.000449492568365603, 6.95883968949488e-07, + 0.000368585523744765, -6.3420715525635e-05}, + {14.3296323024886, -0.182979476956897, 0.306817119309235, -0.00022212115978293, + 0.00463485302909649, 0.0277574953550035, 1.1422454625565e-05, 1.06053257479502e-05, + -2.05720000720608e-05, -0.000338584671430337}, + {-18.7534921817754, 1.14272710923224, 0.460498062012866, -0.00995826989278202, + 0.0658502318647112, 0.00616942819937029, -7.70857153768402e-05, -0.000641755741925561, + 0.00047849204592989, 0.000158509018296766}, + {1.26543621388607, -0.176674379740481, 1.38814920935488, 0.00545485262295305, + -0.00499775616702264, 0.0038057039142173, -6.59604252054511e-05, 6.40211116049053e-05, + -6.74778593434431e-05, -2.81973589469059e-05}, + {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, + 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, + -0.00168429567131815}}; + + rwHandlingParameters.inertiaWheel = 0.000028198; rwHandlingParameters.maxTrq = 0.0032; //3.2 [mNm] diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 6e8fd1a2..494ddb68 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -51,6 +51,31 @@ public: float sus10orientationMatrix[3][3]; float sus11orientationMatrix[3][3]; + float sus0coeffAlpha[9][10]; // FM07 + float sus0coeffBeta[9][10]; + float sus1coeffAlpha[9][10]; // FM06 + float sus1coeffBeta[9][10]; + float sus2coeffAlpha[9][10]; // FM13 + float sus2coeffBeta[9][10]; + float sus3coeffAlpha[9][10]; // FM14 + float sus3coeffBeta[9][10]; + float sus4coeffAlpha[9][10]; // FM05 + float sus4coeffBeta[9][10]; + float sus5coeffAlpha[9][10]; // FM02 + float sus5coeffBeta[9][10]; + float sus6coeffAlpha[9][10]; // FM10 + float sus6coeffBeta[9][10]; + float sus7coeffAlpha[9][10]; // FM01 + float sus7coeffBeta[9][10]; + float sus8coeffAlpha[9][10]; // FM03 + float sus8coeffBeta[9][10]; + float sus9coeffAlpha[9][10]; // FM11 + float sus9coeffBeta[9][10]; + float sus10coeffAlpha[9][10]; // FM09 + float sus10coeffBeta[9][10]; + float sus11coeffAlpha[9][10]; // FM08 + float sus11coeffBeta[9][10]; + float filterAlpha; float sunThresh; } susHandlingParameters; From 4b96997f6cc9e09999cd3cbfac4502d6948f6fa1 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 22 Sep 2022 14:47:53 +0200 Subject: [PATCH 022/244] added SUS transformation matrices to AcsParameters --- mission/controller/acs/AcsParameters.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 92273e04..8559d868 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -35,6 +35,7 @@ AcsParameters::AcsParameters() { { 0, 1, 0}}; + susHandlingParameters.sus0orientationMatrix = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; susHandlingParameters.sus0coeffAlpha = { {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707, -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396, -0.000124096561459262, @@ -91,6 +92,7 @@ AcsParameters::AcsParameters() { {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461, -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688, 0.000141998709314758, 0.000101051304611037}}; + susHandlingParameters.sus1orientationMatrix = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; susHandlingParameters.sus1coeffAlpha = { {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, @@ -147,6 +149,7 @@ AcsParameters::AcsParameters() { {-20.959201441285, -2.23605897639125, 5.73044624806043, 0.0354141964763815, 0.0887545371234514, -0.193862330062381, 0.000216532998121618, -0.00207707610520973, 0.000552928905346826, 0.00190182163597828}}; + susHandlingParameters.sus2orientationMatrix = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; susHandlingParameters.sus2coeffAlpha = { {6.51602979328333, 0.690575501042577, 1.18185457002269, -0.0153161662266588, 0.00145972227341484, 0.0351496474730776, -0.000172645571366945, -6.04213053580018e-05, @@ -203,6 +206,7 @@ AcsParameters::AcsParameters() { {-59.5882618930651, 3.84530212586425, 3.50497032358686, -0.116100453177197, -0.0380997421813177, -0.0581898335691109, 0.00111464935006159, 0.000559313074537689, 0.000168067749764069, 0.000563224178849256}}; + susHandlingParameters.sus3orientationMatrix = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; susHandlingParameters.sus3coeffAlpha = { {-174.687021034355, -7.53454036765748, -9.33798316371397, -0.18212338430986, -0.242523652239734, -0.202086838965846, -0.00138648793335223, -0.00225430176012882, -0.00198887215340364, @@ -259,6 +263,7 @@ AcsParameters::AcsParameters() { {44.8818060495112, -7.94729992210875, 3.59286389225051, 0.217944601088562, 0.108087933176612, -0.116711715153385, -0.00194260120960441, -0.0015752762498594, -0.000331129410732722, 0.00125896996438418}}; + susHandlingParameters.sus4orientationMatrix = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; susHandlingParameters.sus4coeffAlpha = { {-12.4581187126738, 0.398038572289047, -0.438887880988151, -0.00965382887938283, -0.0309322349328842, -0.00359106522420111, -7.79546112299913e-06, -0.000432733997178497, @@ -315,6 +320,7 @@ AcsParameters::AcsParameters() { {-74.1601853968901, 2.55641628908672, 6.38533530714782, -0.0582345132980647, -0.0653804553172819, -0.138850555683872, 0.000489364157827405, 0.000469559629292745, 0.000698140692952438, 0.00123017528239406}}; + susHandlingParameters.sus5orientationMatrix = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; susHandlingParameters.sus5coeffAlpha = { {-12.1398741236355, 1.99425442858125, -1.9303044815802, 0.0418421279520049, -0.0309683799946315, -0.0562201123081437, 0.000522607299552916, -0.000375386573815007, -0.000183899715035788, @@ -371,6 +377,7 @@ AcsParameters::AcsParameters() { {-38.3689359928435, 3.8540516906071, 1.26391725545116, -0.108584643500539, -0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245, -0.000169538153750085, -0.000336529204350355}}; + susHandlingParameters.sus6orientationMatrix = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; susHandlingParameters.sus6coeffAlpha = { {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293, 0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146, @@ -427,6 +434,7 @@ AcsParameters::AcsParameters() { {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785, 0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741, -0.000186887396585446, 0.00119710704771344}}; + susHandlingParameters.sus7orientationMatrix = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; susHandlingParameters.sus7coeffAlpha = { {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, @@ -483,6 +491,7 @@ AcsParameters::AcsParameters() { {191.321181158032, -12.2449557187473, -7.21933741885107, 0.267954293388644, 0.331529493933124, 0.149867703984027, -0.00222279201444128, -0.00284724570619954, -0.00298774060233964, -0.000988903783752156}}; + susHandlingParameters.sus8orientationMatrix = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; susHandlingParameters.sus8coeffAlpha = { {5.46354311880959, 1.15370126035432, 0.568432485840475, -0.00105094692478431, -0.000472899673842554, 0.015581320536192, 2.26460844314248e-05, -0.000254397947062058, @@ -539,6 +548,7 @@ AcsParameters::AcsParameters() { {64.9193383444005, -1.57724255547465, -3.82166532626293, 0.0104712238987591, 0.0898786950946473, 0.128910456296131, -8.27123227422217e-05, -0.000143979624107479, -0.00146684876653306, -0.00102226799570239}}; + susHandlingParameters.sus9orientationMatrix = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; susHandlingParameters.sus9coeffAlpha = { {65.8975109449121, 2.19115342242175, 6.11069527811832, -0.0219884864133703, 0.119985456538482, 0.142746712551924, -0.000465882328687976, 0.000606525132125852, 0.00141667074621881, @@ -595,6 +605,7 @@ AcsParameters::AcsParameters() { {-36.6150844777671, 3.24952006904945, 1.7222457840185, -0.0846362445435584, -0.0625549615377418, 0.019178365782485, 0.000664877496455304, 0.000942971403881222, 0.000190754698755098, -0.000372226659190439}}; + susHandlingParameters.sus10orientationMatrix = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; susHandlingParameters.sus10coeffAlpha = { {14.4562393748324, 0.669162330324919, 2.13895255446541, -0.0161997097021299, 0.00185995785065838, 0.0621351118528379, -0.000278999272493087, 0.000238469666491965, @@ -651,6 +662,7 @@ AcsParameters::AcsParameters() { {-21.6881499304099, -0.563186103920008, 3.70747028664292, 0.021112883967427, -0.00650020689049325, -0.0778533644688476, -0.000131921888670268, -0.000402754836445439, 0.000551249824375055, 0.00062236627391337}}; + susHandlingParameters.sus11orientationMatrix = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; susHandlingParameters.sus11coeffAlpha = { {-5.23569698615548, -1.45500092391928, 2.7643243644756, -0.0762912296128707, -0.0201645929971608, 0.0997226845779083, -0.000741669441569556, -0.000485368004931528, From e70ee1bf9d61d5a7cffa64bdf24e3e0bbf3278a7 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 23 Sep 2022 09:56:32 +0200 Subject: [PATCH 023/244] initial commit of SusConverter --- mission/controller/acs/SusConverter.cpp | 382 ++++++++++++++++++++++++ mission/controller/acs/SusConverter.h | 59 ++++ 2 files changed, 441 insertions(+) create mode 100644 mission/controller/acs/SusConverter.cpp create mode 100644 mission/controller/acs/SusConverter.h diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp new file mode 100644 index 00000000..0ef2fd28 --- /dev/null +++ b/mission/controller/acs/SusConverter.cpp @@ -0,0 +1,382 @@ +/* + * SusConverter.cpp + * + * Created on: 17.01.2022 + * Author: Timon Schwarz + */ + +#include //for atan2 +#include +#include + +void SunSensor::setSunSensorData(uint8_t Sensornumber) { + // Creates dummy sensordata, replace with SUS devicehandler / channel readout + ChannelValue[0] = 3913; + ChannelValue[1] = 3912; + ChannelValue[2] = 3799; + ChannelValue[3] = 3797; + ChannelValue[4] = 4056; +} + +void SunSensor::checkSunSensorData(uint8_t Sensornumber) { + uint16_t ChannelValueSum; + + // Check individual channel values + for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters + + if (ChannelValue[k] <= ChannelValueCheckLow || + ChannelValue[k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS + // channel measurement range? + ValidityNumber = false; // false --> Data not valid + printf( + "The value of channel %i from sun sensor %i is not inside the borders of valid data with " + "a value of %i \n", + k, Sensornumber, ChannelValue[k]); + } else if (ChannelValue[k] > + ChannelValue[4]) { // Channel values higher than zero current threshold GNDREF? + ValidityNumber = false; + printf( + "The value of channel %i from sun sensor %i is higher than the zero current threshold " + "GNDREF\n", + k, Sensornumber); + }; + }; + + // check sum of all channel values to check if sun sensor is illuminated by the sun (sum is + // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon + // reflection or earth albedo) + ChannelValueSum = + 4 * ChannelValue[4] - (ChannelValue[0] + ChannelValue[1] + ChannelValue[2] + ChannelValue[3]); + if ((ChannelValueSum < ChannelValueSumHigh) && (ChannelValueSum > ChannelValueSumLow)) { + ValidityNumber = false; + printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); + }; +} + +void SunSensor::AngleCalculation() { + float xout, yout, s = 0.03; // s=[mm] + uint8_t d = 5, h = 1; // d=[mm] h=[mm] + int ch0, ch1, ch2, ch3; + // Substract measurement values from GNDREF zero current threshold + ch0 = ChannelValue[4] - ChannelValue[0]; + ch1 = ChannelValue[4] - ChannelValue[1]; + ch2 = ChannelValue[4] - ChannelValue[2]; + ch3 = ChannelValue[4] - ChannelValue[3]; + + // Calculation of x and y + xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] + + // Calculation of the angles + AlphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] + AlphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] +} + +void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { + switch (Sensornumber) { // search for the correct calibration coefficients for each SUS + + case 0: + for (uint8_t row = 0; row < 9; + row++) { // save the correct coefficients in the right SUS class + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; + } + } + break; + + case 1: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; + } + } + break; + + case 2: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; + } + } + break; + + case 3: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; + } + } + break; + + case 4: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; + } + } + break; + + case 5: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; + } + } + break; + + case 6: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; + } + } + break; + + case 7: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; + } + } + break; + + case 8: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; + } + } + break; + + case 9: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; + } + } + break; + + case 10: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; + } + } + break; + + case 11: + for (uint8_t row = 0; row < 9; row++) { + for (uint8_t column = 0; column < 10; column++) { + CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; + CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; + } + } + break; + } +} + +void SunSensor::Calibration() { + float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; + uint8_t index; + + alpha_m = AlphaBetaRaw[0]; //[°] + beta_m = AlphaBetaRaw[1]; //[°] + + // while loop iterates above all calibration cells to use the different calibration functions in + // each cell + k = 0; + while (k < 3) { + k = k + 1; + l = 0; + while (l < 3) { + l = l + 1; + // if-condition to check in which cell the data point has to be + if ((alpha_m > ((CompleteCellWidth * ((k - 1) / 3)) - HalfCellWidth) && + alpha_m < ((CompleteCellWidth * (k / 3)) - HalfCellWidth)) && + (beta_m > ((CompleteCellWidth * ((l - 1) / 3)) - HalfCellWidth) && + beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) { + index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell + // -> first cell has number 0 + alpha_calibrated = + CoeffAlpha[index][0] + CoeffAlpha[index][1] * alpha_m + CoeffAlpha[index][2] * beta_m + + CoeffAlpha[index][3] * alpha_m * alpha_m + CoeffAlpha[index][4] * alpha_m * beta_m + + CoeffAlpha[index][5] * beta_m * beta_m + + CoeffAlpha[index][6] * alpha_m * alpha_m * alpha_m + + CoeffAlpha[index][7] * alpha_m * alpha_m * beta_m + + CoeffAlpha[index][8] * alpha_m * beta_m * beta_m + + CoeffAlpha[index][9] * beta_m * beta_m * beta_m; + } + } + } + + // while loop iterates above all calibration cells to use the different calibration functions in + // each cell + k = 0; + while (k < 3) { + k = k + 1; + l = 0; + while (l < 3) { + l = l + 1; + // if condition to check in which cell the data point has to be + if ((alpha_m > ((CompleteCellWidth * ((k - 1) / 3)) - HalfCellWidth) && + alpha_m < ((CompleteCellWidth * (k / 3)) - HalfCellWidth)) && + (beta_m > ((CompleteCellWidth * ((l - 1) / 3)) - HalfCellWidth) && + beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) { + index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell + // -> first cell has number 0 + beta_calibrated = CoeffBeta[index][0] + CoeffBeta[index][1] * alpha_m + + CoeffBeta[index][2] * beta_m + CoeffBeta[index][3] * alpha_m * alpha_m + + CoeffBeta[index][4] * alpha_m * beta_m + + CoeffBeta[index][5] * beta_m * beta_m + + CoeffBeta[index][6] * alpha_m * alpha_m * alpha_m + + CoeffBeta[index][7] * alpha_m * alpha_m * beta_m + + CoeffBeta[index][8] * alpha_m * beta_m * beta_m + + CoeffBeta[index][9] * beta_m * beta_m * beta_m; + } + } + } + + AlphaBetaCalibrated[0] = alpha_calibrated; //[°] + AlphaBetaCalibrated[1] = beta_calibrated; //[°] +} + +void SunSensor::CalculateSunVector() { + float alpha, beta; + alpha = AlphaBetaCalibrated[0]; //[°] + beta = AlphaBetaCalibrated[1]; //[°] + + // Calculate the normalized Sun Vector + SunVectorBodyFrame[0] = + (tan(alpha * (M_PI / 180)) / + (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); + SunVectorBodyFrame[1] = + (tan(beta * (M_PI / 180)) / + (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); + SunVectorBodyFrame[2] = + (-1 / + (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); +} + +float* SunSensor::getSunVectorBodyFrame() { + // return function for the sun vector in the body frame + float* SunVectorBodyFrameReturn = 0; + SunVectorBodyFrameReturn = new float[3]; + + SunVectorBodyFrameReturn[0] = SunVectorBodyFrame[0]; + SunVectorBodyFrameReturn[1] = SunVectorBodyFrame[1]; + SunVectorBodyFrameReturn[2] = SunVectorBodyFrame[2]; + + return SunVectorBodyFrameReturn; +} + +float* SunSensor::TransferSunVector(SunSensor SUS[12]) { + float* SunVectorEIVE = 0; + SunVectorEIVE = new float[3]; + + uint8_t counter = 0; + int8_t BasisMatrixUse[3][3]; + float SunVectorMatrixEIVE[3][12] = {0}, sum; + float SunVectorMatrixBodyFrame[3][12]; + + for (uint8_t Sensornumber = 0; Sensornumber < 12; + Sensornumber++) { // save the sun vector of each SUS in their body frame into an array for + // further processing + float* SunVectorBodyFrame = this[Sensornumber].getSunVectorBodyFrame(); + SunVectorMatrixBodyFrame[0][Sensornumber] = SunVectorBodyFrame[0]; + SunVectorMatrixBodyFrame[1][Sensornumber] = SunVectorBodyFrame[1]; + SunVectorMatrixBodyFrame[2][Sensornumber] = SunVectorBodyFrame[2]; + } + + for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { + if (SUS[Sensornumber].getValidityNumber() == false) { + counter = counter + 1; + } // if the SUS data is not valid -> + + for (uint8_t c1 = 0; c1 < 3; c1++) { + for (uint8_t c2 = 0; c2 < 3; c2++) { + switch (Sensornumber) { // find right basis matrix for each SUS + + case 0: + BasisMatrixUse[c1][c2] = AcsParameters[c1][c2]; + break; + case 1: + BasisMatrixUse[c1][c2] = BasisMatrix1[c1][c2]; + break; + case 2: + BasisMatrixUse[c1][c2] = BasisMatrix2[c1][c2]; + break; + case 3: + BasisMatrixUse[c1][c2] = BasisMatrix3[c1][c2]; + break; + case 4: + BasisMatrixUse[c1][c2] = BasisMatrix4[c1][c2]; + break; + case 5: + BasisMatrixUse[c1][c2] = BasisMatrix5[c1][c2]; + break; + case 6: + BasisMatrixUse[c1][c2] = BasisMatrix6[c1][c2]; + break; + case 7: + BasisMatrixUse[c1][c2] = BasisMatrix7[c1][c2]; + break; + case 8: + BasisMatrixUse[c1][c2] = BasisMatrix8[c1][c2]; + break; + case 9: + BasisMatrixUse[c1][c2] = BasisMatrix9[c1][c2]; + break; + case 10: + BasisMatrixUse[c1][c2] = BasisMatrix10[c1][c2]; + break; + case 11: + BasisMatrixUse[c1][c2] = BasisMatrix11[c1][c2]; + break; + } + } + } + + // matrix multiplication for transition in EIVE coordinatesystem + for (uint8_t p = 0; p < 3; p++) { + for (uint8_t q = 0; q < 3; q++) { + // normal matrix multiplication + SunVectorMatrixEIVE[p][Sensornumber] += + (BasisMatrixUse[p][q] * SunVectorMatrixBodyFrame[q][Sensornumber]); + } + } + } + + // ToDo: remove invalid SUSs from being used for calculating the combined sun vector + + if (counter < 12) { // Calculate one sun vector out of all sun vectors from the different SUS + for (uint8_t i = 0; i < 3; i++) { + sum = 0; + for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { + sum += SunVectorMatrixEIVE[i][Sensornumber]; + printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]); + } + SunVectorEIVE[i] = + sum / (12 - counter); // FLAG Ergebnis ist falsch, kann an einem Fehler im Programm + // liegen, vermutlich aber an den falschen ChannelValues da die + // transformierten Sonnenvektoren jedes SUS plausibel sind + } + } else { + // No sus is valid + throw std::invalid_argument("No sun sensor is valid"); // throw error + } + + return SunVectorEIVE; +} + + diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h new file mode 100644 index 00000000..6f1b7420 --- /dev/null +++ b/mission/controller/acs/SusConverter.h @@ -0,0 +1,59 @@ +/* + * SusConverter.h + * + * Created on: Sep 22, 2022 + * Author: marius + */ + +#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ +#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ + + +#include +#include + +class SunSensor { + public: + SunSensor() {} + + void setSunSensorData(uint8_t Sensornumber); + void checkSunSensorData(uint8_t Sensornumber); + void AngleCalculation(); + void setCalibrationCoefficients(uint8_t Sensornumber); + void Calibration(); + void CalculateSunVector(); + + bool getValidityNumber() { return ValidityNumber; } + float* getSunVectorBodyFrame(); + float* TransferSunVector(SunSensor SUS[12]); + + private: + uint16_t ChannelValue[5]; //[Bit] + float AlphaBetaRaw[2]; //[°] + float AlphaBetaCalibrated[2]; //[°] + float SunVectorBodyFrame[3]; //[-] + + bool ValidityNumber = true; + + uint16_t ChannelValueCheckHigh = + 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check + uint8_t ChannelValueCheckLow = + 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check + uint16_t ChannelValueSumHigh = + 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by + // the reflection of sunlight from the moon/earth + uint8_t ChannelValueSumLow = + 0; //[Bit]low borderline for check if the sun sensor is illuminated + // by the sun or by the reflection of sunlight from the moon/earth + uint8_t CompleteCellWidth = 140, + HalfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in + // which cell a data point should be + + float CoeffAlpha[9][10]; + float CoeffBeta[9][10]; + + AcsParameters acsParameters; +}; + + +#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */ From 46ec376e4d40ed4ad9c7c67ce150185ba5112859 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 23 Sep 2022 14:27:05 +0200 Subject: [PATCH 024/244] amended SusConverter for use as OBSW --- mission/controller/acs/SusConverter.cpp | 208 +++++++++++++----------- mission/controller/acs/SusConverter.h | 26 +-- 2 files changed, 125 insertions(+), 109 deletions(-) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 0ef2fd28..d29f3706 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -8,14 +8,22 @@ #include //for atan2 #include #include +#include -void SunSensor::setSunSensorData(uint8_t Sensornumber) { +void SunSensor::setSunSensorData() { // Creates dummy sensordata, replace with SUS devicehandler / channel readout - ChannelValue[0] = 3913; - ChannelValue[1] = 3912; - ChannelValue[2] = 3799; - ChannelValue[3] = 3797; - ChannelValue[4] = 4056; + susChannelValues[0] = {3913, 3912, 3799, 4056}; + susChannelValues[1] = {3913, 3912, 3799, 4056}; + susChannelValues[2] = {3913, 3912, 3799, 4056}; + susChannelValues[3] = {3913, 3912, 3799, 4056}; + susChannelValues[4] = {3913, 3912, 3799, 4056}; + susChannelValues[5] = {3913, 3912, 3799, 4056}; + susChannelValues[6] = {3913, 3912, 3799, 4056}; + susChannelValues[7] = {3913, 3912, 3799, 4056}; + susChannelValues[8] = {3913, 3912, 3799, 4056}; + susChannelValues[9] = {3913, 3912, 3799, 4056}; + susChannelValues[10] = {3913, 3912, 3799, 4056}; + susChannelValues[11] = {3913, 3912, 3799, 4056}; } void SunSensor::checkSunSensorData(uint8_t Sensornumber) { @@ -24,21 +32,21 @@ void SunSensor::checkSunSensorData(uint8_t Sensornumber) { // Check individual channel values for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters - if (ChannelValue[k] <= ChannelValueCheckLow || - ChannelValue[k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS + if (susChannelValues[Sensornumber][k] <= ChannelValueCheckLow || + susChannelValues[Sensornumber][k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS // channel measurement range? - ValidityNumber = false; // false --> Data not valid - printf( + ValidityNumber[Sensornumber] = false; // false --> Data not valid + /*printf( "The value of channel %i from sun sensor %i is not inside the borders of valid data with " "a value of %i \n", - k, Sensornumber, ChannelValue[k]); - } else if (ChannelValue[k] > - ChannelValue[4]) { // Channel values higher than zero current threshold GNDREF? - ValidityNumber = false; - printf( + k, Sensornumber, ChannelValue[k]);*/ + } else if (susChannelValues[Sensornumber][k] > + susChannelValues[Sensornumber][4]) { // Channel values higher than zero current threshold GNDREF? + ValidityNumber[Sensornumber] = false; + /*printf( "The value of channel %i from sun sensor %i is higher than the zero current threshold " "GNDREF\n", - k, Sensornumber); + k, Sensornumber);*/ }; }; @@ -46,30 +54,32 @@ void SunSensor::checkSunSensorData(uint8_t Sensornumber) { // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon // reflection or earth albedo) ChannelValueSum = - 4 * ChannelValue[4] - (ChannelValue[0] + ChannelValue[1] + ChannelValue[2] + ChannelValue[3]); + 4 * susChannelValues[Sensornumber][4] - (susChannelValues[Sensornumber][0] + + susChannelValues[Sensornumber][1] + susChannelValues[Sensornumber][2] + + susChannelValues[Sensornumber][3]); if ((ChannelValueSum < ChannelValueSumHigh) && (ChannelValueSum > ChannelValueSumLow)) { - ValidityNumber = false; - printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); + ValidityNumber[Sensornumber] = false; + //printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); }; } -void SunSensor::AngleCalculation() { +void SunSensor::AngleCalculation(uint8_t Sensornumber) { float xout, yout, s = 0.03; // s=[mm] uint8_t d = 5, h = 1; // d=[mm] h=[mm] int ch0, ch1, ch2, ch3; // Substract measurement values from GNDREF zero current threshold - ch0 = ChannelValue[4] - ChannelValue[0]; - ch1 = ChannelValue[4] - ChannelValue[1]; - ch2 = ChannelValue[4] - ChannelValue[2]; - ch3 = ChannelValue[4] - ChannelValue[3]; + ch0 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][0]; + ch1 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][1]; + ch2 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][2]; + ch3 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][3]; // Calculation of x and y xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] // Calculation of the angles - AlphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] - AlphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] + AlphaBetaRaw[Sensornumber][0] = atan2(xout, h) * (180 / M_PI); //[°] + AlphaBetaRaw[Sensornumber][1] = atan2(yout, h) * (180 / M_PI); //[°] } void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { @@ -79,8 +89,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { for (uint8_t row = 0; row < 9; row++) { // save the correct coefficients in the right SUS class for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; } } break; @@ -88,8 +98,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 1: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; } } break; @@ -97,8 +107,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 2: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; } } break; @@ -106,8 +116,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 3: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; } } break; @@ -115,8 +125,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 4: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; } } break; @@ -124,8 +134,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 5: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; } } break; @@ -133,8 +143,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 6: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; } } break; @@ -142,8 +152,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 7: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; } } break; @@ -151,8 +161,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 8: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; } } break; @@ -160,8 +170,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 9: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; } } break; @@ -169,8 +179,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 10: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; } } break; @@ -178,20 +188,20 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 11: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; - CoeffBeta[row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; + CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; + CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; } } break; } } -void SunSensor::Calibration() { +void SunSensor::Calibration(uint8_t Sensornumber) { float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; uint8_t index; - alpha_m = AlphaBetaRaw[0]; //[°] - beta_m = AlphaBetaRaw[1]; //[°] + alpha_m = AlphaBetaRaw[Sensornumber][0]; //[°] + beta_m = AlphaBetaRaw[Sensornumber][1]; //[°] // while loop iterates above all calibration cells to use the different calibration functions in // each cell @@ -209,13 +219,13 @@ void SunSensor::Calibration() { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell // -> first cell has number 0 alpha_calibrated = - CoeffAlpha[index][0] + CoeffAlpha[index][1] * alpha_m + CoeffAlpha[index][2] * beta_m + - CoeffAlpha[index][3] * alpha_m * alpha_m + CoeffAlpha[index][4] * alpha_m * beta_m + - CoeffAlpha[index][5] * beta_m * beta_m + - CoeffAlpha[index][6] * alpha_m * alpha_m * alpha_m + - CoeffAlpha[index][7] * alpha_m * alpha_m * beta_m + - CoeffAlpha[index][8] * alpha_m * beta_m * beta_m + - CoeffAlpha[index][9] * beta_m * beta_m * beta_m; + CoeffAlpha[Sensornumber][index][0] + CoeffAlpha[Sensornumber][index][1] * alpha_m + CoeffAlpha[Sensornumber][index][2] * beta_m + + CoeffAlpha[Sensornumber][index][3] * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][4] * alpha_m * beta_m + + CoeffAlpha[Sensornumber][index][5] * beta_m * beta_m + + CoeffAlpha[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + + CoeffAlpha[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + + CoeffAlpha[Sensornumber][index][8] * alpha_m * beta_m * beta_m + + CoeffAlpha[Sensornumber][index][9] * beta_m * beta_m * beta_m; } } } @@ -235,35 +245,35 @@ void SunSensor::Calibration() { beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell // -> first cell has number 0 - beta_calibrated = CoeffBeta[index][0] + CoeffBeta[index][1] * alpha_m + - CoeffBeta[index][2] * beta_m + CoeffBeta[index][3] * alpha_m * alpha_m + - CoeffBeta[index][4] * alpha_m * beta_m + - CoeffBeta[index][5] * beta_m * beta_m + - CoeffBeta[index][6] * alpha_m * alpha_m * alpha_m + - CoeffBeta[index][7] * alpha_m * alpha_m * beta_m + - CoeffBeta[index][8] * alpha_m * beta_m * beta_m + - CoeffBeta[index][9] * beta_m * beta_m * beta_m; + beta_calibrated = CoeffBeta[Sensornumber][index][0] + CoeffBeta[Sensornumber][index][1] * alpha_m + + CoeffBeta[Sensornumber][index][2] * beta_m + CoeffBeta[Sensornumber][index][3] * alpha_m * alpha_m + + CoeffBeta[Sensornumber][index][4] * alpha_m * beta_m + + CoeffBeta[Sensornumber][index][5] * beta_m * beta_m + + CoeffBeta[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + + CoeffBeta[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + + CoeffBeta[Sensornumber][index][8] * alpha_m * beta_m * beta_m + + CoeffBeta[Sensornumber][index][9] * beta_m * beta_m * beta_m; } } } - AlphaBetaCalibrated[0] = alpha_calibrated; //[°] - AlphaBetaCalibrated[1] = beta_calibrated; //[°] + AlphaBetaCalibrated[Sensornumber][0] = alpha_calibrated; //[°] + AlphaBetaCalibrated[Sensornumber][1] = beta_calibrated; //[°] } -void SunSensor::CalculateSunVector() { +void SunSensor::CalculateSunVector(uint8_t Sensornumber) { float alpha, beta; - alpha = AlphaBetaCalibrated[0]; //[°] - beta = AlphaBetaCalibrated[1]; //[°] + alpha = AlphaBetaCalibrated[Sensornumber][0]; //[°] + beta = AlphaBetaCalibrated[Sensornumber][1]; //[°] // Calculate the normalized Sun Vector - SunVectorBodyFrame[0] = + SunVectorBodyFrame[Sensornumber][0] = (tan(alpha * (M_PI / 180)) / (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - SunVectorBodyFrame[1] = + SunVectorBodyFrame[Sensornumber][1] = (tan(beta * (M_PI / 180)) / (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - SunVectorBodyFrame[2] = + SunVectorBodyFrame[Sensornumber][2] = (-1 / (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); } @@ -280,7 +290,11 @@ float* SunSensor::getSunVectorBodyFrame() { return SunVectorBodyFrameReturn; } -float* SunSensor::TransferSunVector(SunSensor SUS[12]) { +bool SunSensor::getValidityNumber(uint8_t Sensornumber) { + return ValidityNumber[Sensornumber]; +} + +float* SunSensor::TransferSunVector() { float* SunVectorEIVE = 0; SunVectorEIVE = new float[3]; @@ -292,14 +306,14 @@ float* SunSensor::TransferSunVector(SunSensor SUS[12]) { for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { // save the sun vector of each SUS in their body frame into an array for // further processing - float* SunVectorBodyFrame = this[Sensornumber].getSunVectorBodyFrame(); + float* SunVectorBodyFrame = SunVectorBodyFrame[Sensornumber]; SunVectorMatrixBodyFrame[0][Sensornumber] = SunVectorBodyFrame[0]; SunVectorMatrixBodyFrame[1][Sensornumber] = SunVectorBodyFrame[1]; SunVectorMatrixBodyFrame[2][Sensornumber] = SunVectorBodyFrame[2]; } for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { - if (SUS[Sensornumber].getValidityNumber() == false) { + if (getValidityNumber(Sensornumber) == false) { counter = counter + 1; } // if the SUS data is not valid -> @@ -308,40 +322,40 @@ float* SunSensor::TransferSunVector(SunSensor SUS[12]) { switch (Sensornumber) { // find right basis matrix for each SUS case 0: - BasisMatrixUse[c1][c2] = AcsParameters[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; break; case 1: - BasisMatrixUse[c1][c2] = BasisMatrix1[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; break; case 2: - BasisMatrixUse[c1][c2] = BasisMatrix2[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; break; case 3: - BasisMatrixUse[c1][c2] = BasisMatrix3[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; break; case 4: - BasisMatrixUse[c1][c2] = BasisMatrix4[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; break; case 5: - BasisMatrixUse[c1][c2] = BasisMatrix5[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; break; case 6: - BasisMatrixUse[c1][c2] = BasisMatrix6[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; break; case 7: - BasisMatrixUse[c1][c2] = BasisMatrix7[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; break; case 8: - BasisMatrixUse[c1][c2] = BasisMatrix8[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; break; case 9: - BasisMatrixUse[c1][c2] = BasisMatrix9[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; break; case 10: - BasisMatrixUse[c1][c2] = BasisMatrix10[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; break; case 11: - BasisMatrixUse[c1][c2] = BasisMatrix11[c1][c2]; + BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; break; } } @@ -357,20 +371,22 @@ float* SunSensor::TransferSunVector(SunSensor SUS[12]) { } } - // ToDo: remove invalid SUSs from being used for calculating the combined sun vector - if (counter < 12) { // Calculate one sun vector out of all sun vectors from the different SUS for (uint8_t i = 0; i < 3; i++) { sum = 0; for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { - sum += SunVectorMatrixEIVE[i][Sensornumber]; - printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]); + if (getValidityNumber(Sensornumber)){ + sum += SunVectorMatrixEIVE[i][Sensornumber]; + //printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]); + } } + // ToDo: decide on length on sun vector SunVectorEIVE[i] = - sum / (12 - counter); // FLAG Ergebnis ist falsch, kann an einem Fehler im Programm + sum/* / (12 - counter)*/; // FLAG Ergebnis ist falsch, kann an einem Fehler im Programm // liegen, vermutlich aber an den falschen ChannelValues da die // transformierten Sonnenvektoren jedes SUS plausibel sind } + VectorOperations::normalize(SunVectorEIVE, SunVectorEIVE, 3); } else { // No sus is valid throw std::invalid_argument("No sun sensor is valid"); // throw error diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 6f1b7420..4012bb5a 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -16,24 +16,24 @@ class SunSensor { public: SunSensor() {} - void setSunSensorData(uint8_t Sensornumber); + void setSunSensorData(); void checkSunSensorData(uint8_t Sensornumber); - void AngleCalculation(); + void AngleCalculation(uint8_t Sensornumber); void setCalibrationCoefficients(uint8_t Sensornumber); - void Calibration(); - void CalculateSunVector(); + void Calibration(uint8_t Sensornumber); + void CalculateSunVector(uint8_t Sensornumber); - bool getValidityNumber() { return ValidityNumber; } + bool getValidityNumber(uint8_t Sensornumber); float* getSunVectorBodyFrame(); - float* TransferSunVector(SunSensor SUS[12]); + float* TransferSunVector(); private: - uint16_t ChannelValue[5]; //[Bit] - float AlphaBetaRaw[2]; //[°] - float AlphaBetaCalibrated[2]; //[°] - float SunVectorBodyFrame[3]; //[-] + uint16_t susChannelValues[12][4]; //[Bit] + float AlphaBetaRaw[12][2]; //[°] + float AlphaBetaCalibrated[12][2]; //[°] + float SunVectorBodyFrame[12][3]; //[-] - bool ValidityNumber = true; + bool ValidityNumber[12] = true; uint16_t ChannelValueCheckHigh = 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check @@ -49,8 +49,8 @@ class SunSensor { HalfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in // which cell a data point should be - float CoeffAlpha[9][10]; - float CoeffBeta[9][10]; + float CoeffAlpha[12][9][10]; + float CoeffBeta[12][9][10]; AcsParameters acsParameters; }; From cd11e081932e1529148af327e9d1fbb47f5701ad Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 23 Sep 2022 14:37:48 +0200 Subject: [PATCH 025/244] removed redundant loop --- mission/controller/acs/SusConverter.cpp | 43 +++++++------------------ 1 file changed, 11 insertions(+), 32 deletions(-) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index d29f3706..151648ed 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -218,47 +218,26 @@ void SunSensor::Calibration(uint8_t Sensornumber) { beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell // -> first cell has number 0 - alpha_calibrated = + AlphaBetaCalibrated[Sensornumber][0] = CoeffAlpha[Sensornumber][index][0] + CoeffAlpha[Sensornumber][index][1] * alpha_m + CoeffAlpha[Sensornumber][index][2] * beta_m + CoeffAlpha[Sensornumber][index][3] * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][4] * alpha_m * beta_m + CoeffAlpha[Sensornumber][index][5] * beta_m * beta_m + CoeffAlpha[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + CoeffAlpha[Sensornumber][index][8] * alpha_m * beta_m * beta_m + - CoeffAlpha[Sensornumber][index][9] * beta_m * beta_m * beta_m; + CoeffAlpha[Sensornumber][index][9] * beta_m * beta_m * beta_m; //[°] + AlphaBetaCalibrated[Sensornumber][1] = + CoeffBeta[Sensornumber][index][0] + CoeffBeta[Sensornumber][index][1] * alpha_m + + CoeffBeta[Sensornumber][index][2] * beta_m + CoeffBeta[Sensornumber][index][3] * alpha_m * alpha_m + + CoeffBeta[Sensornumber][index][4] * alpha_m * beta_m + + CoeffBeta[Sensornumber][index][5] * beta_m * beta_m + + CoeffBeta[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + + CoeffBeta[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + + CoeffBeta[Sensornumber][index][8] * alpha_m * beta_m * beta_m + + CoeffBeta[Sensornumber][index][9] * beta_m * beta_m * beta_m; //[°] } } } - - // while loop iterates above all calibration cells to use the different calibration functions in - // each cell - k = 0; - while (k < 3) { - k = k + 1; - l = 0; - while (l < 3) { - l = l + 1; - // if condition to check in which cell the data point has to be - if ((alpha_m > ((CompleteCellWidth * ((k - 1) / 3)) - HalfCellWidth) && - alpha_m < ((CompleteCellWidth * (k / 3)) - HalfCellWidth)) && - (beta_m > ((CompleteCellWidth * ((l - 1) / 3)) - HalfCellWidth) && - beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) { - index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell - // -> first cell has number 0 - beta_calibrated = CoeffBeta[Sensornumber][index][0] + CoeffBeta[Sensornumber][index][1] * alpha_m + - CoeffBeta[Sensornumber][index][2] * beta_m + CoeffBeta[Sensornumber][index][3] * alpha_m * alpha_m + - CoeffBeta[Sensornumber][index][4] * alpha_m * beta_m + - CoeffBeta[Sensornumber][index][5] * beta_m * beta_m + - CoeffBeta[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + - CoeffBeta[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + - CoeffBeta[Sensornumber][index][8] * alpha_m * beta_m * beta_m + - CoeffBeta[Sensornumber][index][9] * beta_m * beta_m * beta_m; - } - } - } - - AlphaBetaCalibrated[Sensornumber][0] = alpha_calibrated; //[°] - AlphaBetaCalibrated[Sensornumber][1] = beta_calibrated; //[°] } void SunSensor::CalculateSunVector(uint8_t Sensornumber) { From e140ae4063a784a04eb13e3c722a4e6bf108ca4d Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 26 Sep 2022 10:13:00 +0200 Subject: [PATCH 026/244] fixed typo in filename --- mission/controller/acs/{Navitation.cpp => Navigation.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename mission/controller/acs/{Navitation.cpp => Navigation.cpp} (100%) diff --git a/mission/controller/acs/Navitation.cpp b/mission/controller/acs/Navigation.cpp similarity index 100% rename from mission/controller/acs/Navitation.cpp rename to mission/controller/acs/Navigation.cpp From 2ede1ed27c694fc30d63bc5937ec84c04d79b299 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 26 Sep 2022 10:15:03 +0200 Subject: [PATCH 027/244] added CMake files for new directories. amended existing controller Cmake --- mission/controller/CMakeLists.txt | 2 ++ mission/controller/acs/CMakeLists.txt | 14 ++++++++++++++ mission/controller/acs/control/CMakeLists.txt | 5 +++++ 3 files changed, 21 insertions(+) create mode 100644 mission/controller/acs/CMakeLists.txt create mode 100644 mission/controller/acs/control/CMakeLists.txt diff --git a/mission/controller/CMakeLists.txt b/mission/controller/CMakeLists.txt index 84ff7f9e..463e6a5d 100644 --- a/mission/controller/CMakeLists.txt +++ b/mission/controller/CMakeLists.txt @@ -2,3 +2,5 @@ if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "") target_sources(${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp AcsController.cpp) endif() + +add_subdirectory(acs) \ No newline at end of file diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt new file mode 100644 index 00000000..c949a067 --- /dev/null +++ b/mission/controller/acs/CMakeLists.txt @@ -0,0 +1,14 @@ +target_sources( + ${LIB_EIVE_MISSION} + PRIVATE AcsParameters.cpp + ActuatorCmd.cpp + Guidance.cpp + Igrf13Model.cpp + MultiplicativeKalmanFilter.cpp + Navigation.cpp + OutputValues.cpp + SensorProcessing.cpp + SensorValues.cpp + SusConverter.cpp) + +add_subdirectory(control) diff --git a/mission/controller/acs/control/CMakeLists.txt b/mission/controller/acs/control/CMakeLists.txt new file mode 100644 index 00000000..dc540435 --- /dev/null +++ b/mission/controller/acs/control/CMakeLists.txt @@ -0,0 +1,5 @@ +target_sources( + ${LIB_EIVE_MISSION} + PRIVATE Detumble.cpp + PtgCtrl.cpp + SafeCtrl.cpp) \ No newline at end of file From 96bbde7bbc280d7d918d245167125013599945e9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 26 Sep 2022 10:39:57 +0200 Subject: [PATCH 028/244] removed HasReturnValuesIF --- mission/controller/acs/MultiplicativeKalmanFilter.cpp | 5 ++--- mission/controller/acs/MultiplicativeKalmanFilter.h | 2 +- mission/controller/acs/control/PtgCtrl.h | 5 ++--- mission/controller/acs/control/SafeCtrl.h | 2 +- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5a546409..b2c8a963 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include "MultiplicativeKalmanFilter.h" #include #include @@ -300,7 +299,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( else{ sensorsAvail = 8; //no measurements validMekf = false; - return RETURN_FAILED; + return returnvalue::FAILED; } // If we are here, MEKF will perform @@ -1201,5 +1200,5 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // Check for new data in measurement -> SensorProcessing ? - return RETURN_OK; + return returnvalue::OK; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index d4370ad3..f972ab5a 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -21,7 +21,7 @@ #include "AcsParameters.h" -class MultiplicativeKalmanFilter : public HasReturnvaluesIF { +class MultiplicativeKalmanFilter{ public: /* @brief: Constructor * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 5bd7f0da..fe5d0e96 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -19,11 +19,10 @@ #include #include #include -#include "acs/config/classIds.h" -#include +#include #include -class PtgCtrl : public HasReturnvaluesIF { +class PtgCtrl{ public: /* @brief: Constructor diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index b0ce6a90..36807bcc 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -17,7 +17,7 @@ #include #include -class SafeCtrl : public HasReturnvaluesIF { +class SafeCtrl{ public: From e7b6ca42bf98a80cd0336628aa5df89cc54f8abb Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 26 Sep 2022 13:23:20 +0200 Subject: [PATCH 029/244] cleaned up includes --- mission/controller/acs/AcsParameters.cpp | 2 +- mission/controller/acs/Guidance.h | 2 +- mission/controller/acs/Igrf13Model.h | 4 ++-- mission/controller/acs/SensorProcessing.h | 2 +- mission/controller/acs/SensorValues.cpp | 2 +- mission/controller/acs/SensorValues.h | 2 +- mission/controller/acs/control/Detumble.h | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 8559d868..42408868 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -2,7 +2,7 @@ #include "AcsParameters.h" #include -#include +#include AcsParameters::AcsParameters() { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 76cfce77..736d8b9b 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include class Guidance { diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index b6f14e0f..3ea2cb40 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -17,10 +17,10 @@ #define IGRF13MODEL_H_ #include -//#include +#include #include #include -#include +#include // Output should be transformed to [T] instead of [nT] diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 43c93722..23abcc40 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -6,7 +6,7 @@ #define SENSORPROCESSING_H_ #include //uint8_t -#include /*purpose, timeval ?*/ +#include /*purpose, timeval ?*/ #include #include #include diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 25f2d61f..414c93ef 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -4,7 +4,7 @@ * Created on: 30 Mar 2022 * Author: rooob */ -#include +#include #include #include diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 566eec81..001b141b 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -95,5 +95,5 @@ public: }; } /* namespace ACS */ -#endif ENSORVALUES_H_ +#endif /*ENSORVALUES_H_*/ diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 47efd49d..618be6c6 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include class Detumble{ From dca433532998f4f54e4127ce5b6f31fe835b9ac6 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 27 Sep 2022 10:49:46 +0200 Subject: [PATCH 030/244] fixed array assignment after intialization --- mission/controller/acs/AcsParameters.cpp | 814 +------------------- mission/controller/acs/AcsParameters.h | 929 ++++++++++++++++++++--- 2 files changed, 830 insertions(+), 913 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 42408868..c22e8ede 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -4,819 +4,9 @@ #include #include -AcsParameters::AcsParameters() { +AcsParameters::AcsParameters() {} - onBoardParams.sampleTime = 0.1; - - inertiaEIVE.inertiaMatrix = { - { 1.0, 0.0, 0.0}, - { 0.0, 1.0, 0.0}, - { 0.0, 0.5, 1.0}}; - - mgmHandlingParameters.mgm0orientationMatrix = { - { 0, 0,-1}, - { 0, 1, 0}, - { 1, 0, 0}}; - mgmHandlingParameters.mgm1orientationMatrix = { - { 0, 0, 1}, - { 0,-1, 0}, - { 1, 0, 0}}; - mgmHandlingParameters.mgm2orientationMatrix = { - { 0, 0,-1}, - { 0, 1, 0}, - { 1 ,0, 0}}; - mgmHandlingParameters.mgm3orientationMatrix = { - { 0, 0, 1}, - { 0,-1, 0}, - { 1, 0, 0}}; - mgmHandlingParameters.mgm4orientationMatrix = { - { 0, 0,-1}, - {-1, 0, 0}, - { 0, 1, 0}}; - - - susHandlingParameters.sus0orientationMatrix = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; - susHandlingParameters.sus0coeffAlpha = { - {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}}; - susHandlingParameters.sus0coeffBeta = { - {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}}; - susHandlingParameters.sus1orientationMatrix = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; - susHandlingParameters.sus1coeffAlpha = { - {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, - -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, - -0.000178087038629721, 4.09208968678946e-05}, - {2.24803085641869, 1.42938692406645, 0.30104994020693, 0.00756499999397385, 0.0117765927439368, - -0.000743685980641362, 4.69920803836194e-05, 0.000129815636957956, -9.10792250542345e-06, - -2.03870119873411e-05}, - {26.9943033817917, 0.147791175366868, -3.48256070200564, -0.0303332422478656, - 0.0183377266255394, 0.124593616125966, -0.000466003049304431, -0.000272000698791331, - -0.00063621309529853, -0.00158363678978767}, - {-0.221893380318465, 1.29919955307083, 0.21872487901019, 0.0049448219667127, - 0.00291224091529189, 0.00654651987282984, -9.86842469311185e-05, 8.20057454706638e-05, - 6.42331081725944e-05, 7.11656918299053e-05}, - {1.40178843964621, 1.1733111455249, 0.287485528779234, -0.000793970428759834, - 0.000170529273905818, -0.00268807864923086, 9.09553964483881e-05, -0.000271892733575409, - 8.52016306311741e-05, -0.000291797625433646}, - {0.65549617899457, 1.25716478394514, 0.301396415134214, -0.00357289640403958, - -0.000473416364133431, -0.010760332636205, -9.77220176481185e-05, 4.40798040046875e-05, - 2.84958344955681e-05, 0.000128583400693359}, - {6.20958048145025, 1.9528406481596, 1.32915657614139, -0.0326944423378284, -0.0158258335207969, - 0.0328249756354635, 0.00027113042931131, -0.000133980867173428, -0.000357964552318811, - 0.000224235061786191}, - {2.46222812180944, 1.1731834908026, 0.17440330925151, -0.00132279581980401, - -0.00447202005426964, -0.000804321602550913, -1.59526570766446e-05, 2.62946483533391e-05, - 3.28466749016414e-05, -6.63837547601294e-06}, - {42.615758859473, 2.46617281707273, -5.742515881283, -0.131942799763164, 0.20250702826603, - 0.0981562802911027, 0.00189939440077981, -0.0018591621618441, -0.00161121179693977, - -0.00058814458116749}}; - susHandlingParameters.sus1coeffBeta = { - {-12.300032617206, -1.06640894101328, 0.33950802247214, -0.00890867870617722, -0.04872758086642, - -0.0114263851027856, 0.000141061196404012, -0.000675469545483099, -0.000138249928781575, - -0.000138871036200597}, - {10.1631114109768, 0.261654603839785, 1.2376413405181, 0.00888558138614535, 0.00151674939001532, - -0.00534577602313027, 9.19430013005559e-05, 5.39804599087081e-05, -4.15760162347772e-05, - -7.60797902457032e-05}, - {-30.142329062199, 1.26939195100229, 6.14467186367471, 0.0464163689935328, 0.00379001947505376, - -0.165444163648109, 0.000516545385538741, 1.56053219154647e-05, -5.58651971370719e-05, - 0.00173185063955313}, - {12.1454103989862, -0.243589095509132, 2.02543716988677, -0.000857989774598331, - -0.00705278543432513, 0.0250580538307654, 3.50683653081847e-05, -2.63093897408875e-05, - -5.67352645830913e-05, 0.000232270832022029}, - {4.4338108906594, -0.305276965994378, 1.17293558142526, 0.000152618994429577, - 0.00134432642920902, -0.00104036813342885, 0.000334476082056995, 6.74826804343671e-05, - 0.000275311897725414, 7.58157740577916e-05}, - {3.47680700379043, -0.154163381023597, 1.389579838768, 0.000799705880026268, - 0.00401980026462874, -0.00915311817354667, -2.54817301605075e-06, -2.27422984169921e-05, - -2.61224817848938e-05, 6.00381132540332e-05}, - {29.469181543703, -0.722888948550437, 3.3623377135197, 0.00148445490093232, -0.0474780142430845, - 0.0486755575785462, 0.000126295091963757, 0.000526632230895258, -0.000259305985126003, - 0.000412751148048724}, - {2.67029041722834, -0.0837968038501666, 1.37628504937018, 0.00165061312885753, - -0.00953813055064273, 0.0032433005486936, -1.6522452172598e-05, 0.000144574078261271, - -8.47348746872376e-05, -1.92509604512729e-06}, - {-20.959201441285, -2.23605897639125, 5.73044624806043, 0.0354141964763815, 0.0887545371234514, - -0.193862330062381, 0.000216532998121618, -0.00207707610520973, 0.000552928905346826, - 0.00190182163597828}}; - susHandlingParameters.sus2orientationMatrix = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; - susHandlingParameters.sus2coeffAlpha = { - {6.51602979328333, 0.690575501042577, 1.18185457002269, -0.0153161662266588, - 0.00145972227341484, 0.0351496474730776, -0.000172645571366945, -6.04213053580018e-05, - 9.74494676304114e-05, 0.000334122888261002}, - {0.954398509323963, 1.10996214782069, 0.313314231563221, -0.00367553051112208, - 0.0110290193380194, 0.000240079475656232, -6.93444423181303e-05, 0.000107433381295167, - 1.30750132315838e-05, -2.43580795300515e-05}, - {-55.1159841655056, -1.47449655191106, 3.40106264596874, -0.0621428271456258, - 0.0659788065633613, -0.0791732068323335, -0.000524264070592741, 0.000582093651418709, - -0.000586102213707195, 0.000658133691098817}, - {1.98614148820353, 1.32058724763677, 0.156843003413303, 0.002748082456053, 0.00202677073171519, - 0.00382360695862248, -0.000122364309010211, 5.33354637965168e-05, 3.93641210098335e-05, - 4.06398431916703e-05}, - {3.41223117010734, 1.1597568029329, 0.31881674291653, -0.000382400010917784, - -0.000754945672515052, -0.00079200882313927, 0.000145713118224563, -0.00026910957285589, - 0.000137876961532787, -0.000326798596746712}, - {6.23333031852853, 1.24902998148103, -0.0162317540018123, -0.00338184464699201, - 0.000420329743164687, 0.00202038442335185, -7.10435889754986e-05, -6.04039458988991e-06, - 7.25318569569788e-06, -2.5930447720704e-05}, - {191.759784636909, -10.5228276216193, 8.48306234734519, 0.243240262512846, -0.344226468125615, - 0.126267158197535, -0.00186612281541009, 0.00304415728817747, -0.00304958575196089, - 0.000457236034569107}, - {5.61375025356727, 1.1692295110657, 0.224665256727786, -0.00230481633344849, - -0.00746693012026367, -0.00172583925345173, -7.00823444553058e-06, 7.31362778266959e-05, - 5.81988007269583e-05, 1.3723604109425e-05}, - {98.0250669452855, -2.18500123986039, -6.68238707939385, 0.000754807832106659, - 0.256133336978808, 0.110826583415768, 0.000457663127670018, -0.00197655629847616, - -0.00254305206375073, -0.000466731538082995}}; - susHandlingParameters.sus2coeffBeta = { - {41.1102358678699, 2.3034699186519, 2.74551448799899, 0.061701310929235, 0.0317074142089495, - 0.0308171492962288, 0.00049453042200054, 0.000519222896270701, 2.85420168881716e-05, - 0.000259197384126413}, - {4.46821725251333, 0.0125273331991983, 1.32640678842532, 0.000543566569079156, - 0.00396616601484022, -0.00488408099728387, -3.05734704054868e-06, 7.3424831303621e-05, - -5.49439160235527e-05, -8.30708110469922e-05}, - {64.773396165255, 2.97057686090134, -1.90770757709096, 0.062747116236773, -0.077990648565002, - 0.0613989204238974, 0.00055512113297293, -0.000347045533958329, 0.00104059576098392, - -0.000348638726253297}, - {3.10352939390402, -0.2376108554276, 1.60523925160222, 0.00116454605680723, -0.0067958260462381, - 0.0136561370875238, 2.59929059167486e-05, 3.33825895937897e-05, -5.55828531601728e-05, - 0.000109833374761172}, - {0.156052891975873, -0.320721597024578, 1.15208488414874, 0.00164743688819939, - 0.000534718891498932, 0.000469870758457642, 0.000308432468885207, 0.00011789470679678, - 0.000292373398965513, 0.000183599033441813}, - {2.84967971406268, -0.21374251183113, 1.09938586447269, 2.34894704600407e-05, - 0.00588345375399262, 0.00296966835738407, 8.42707308834155e-06, 2.81870099202641e-06, - -3.56732787246631e-05, -7.04534663356379e-05}, - {-7.59892007483895, 0.358662160515702, 0.805137646978357, 0.00222144376998348, - 0.0464438387809707, 0.00847551828841782, 3.24805702347551e-05, 5.45500807838332e-05, - 0.000941378089367713, 0.000353137737023192}, - {-4.65367165487109, 0.201306010390421, 1.19135575710523, -0.00692801521395975, - 0.00394118754078443, 0.00426360093528599, 6.297683536736e-05, -7.15794236895102e-05, - -7.47076172176468e-05, -1.94516917836346e-05}, - {-59.5882618930651, 3.84530212586425, 3.50497032358686, -0.116100453177197, -0.0380997421813177, - -0.0581898335691109, 0.00111464935006159, 0.000559313074537689, 0.000168067749764069, - 0.000563224178849256}}; - susHandlingParameters.sus3orientationMatrix = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; - susHandlingParameters.sus3coeffAlpha = { - {-174.687021034355, -7.53454036765748, -9.33798316371397, -0.18212338430986, -0.242523652239734, - -0.202086838965846, -0.00138648793335223, -0.00225430176012882, -0.00198887215340364, - -0.00160678535160774}, - {6.92009692410602, 1.8192037428209, 0.254908171908415, 0.0179273243472017, 0.00894059238779664, - -0.000436952529644, 0.000138070523903458, 9.22759645920339e-05, -9.4312261303588e-06, - -1.76647897892869e-05}, - {-17.9720006944368, 0.230510201259892, 1.10751755772907, -0.00973621304161327, - 0.0554450499198677, -0.00590970792122449, -2.92393772526592e-05, 0.000444329929586969, - -0.000436055839773919, -9.5869891049503e-05}, - {-4.9880829382985, 1.33627775121504, -0.330382157073243, 0.00306744056311184, - 0.00376353074674973, -0.0107453978169225, -0.00010680477021693, 5.17225535432745e-05, - 7.4423443938376e-05, -0.000107927900087035}, - {0.952867982900728, 1.14513280899596, 0.307744203675505, 0.000404669974219378, - -0.000737988606997615, 0.00120218232577844, 0.000191147653645603, -0.000275058867995882, - 0.000137187356620739, -0.000320202731145004}, - {8.076706574364, 1.31338618710295, -0.334634356394277, -0.00209719438033295, - -0.00381753503582303, 0.0100347823323616, -7.00550548221671e-05, -1.97626956996069e-05, - 7.80079707003333e-05, -8.95904360920744e-05}, - {-82.4748312650249, 8.63074484663009, -0.949295700187556, -0.178618807265278, 0.130143669167547, - 0.0284326533865768, 0.00149831261351137, -0.0011583692969717, 0.0010560778729661, - 0.000635404380970666}, - {3.34457857521978, 1.09242517408071, 0.241722402244944, 0.00381629887587041, - -0.00863580122530851, 0.00137050492069702, -8.91046701171713e-05, 8.44169683308007e-05, - -3.54608413548779e-05, 8.54042677832451e-06}, - {78.1540457908649, -1.30266922193303, -5.33605443700115, 0.0184226131926499, 0.146629920899062, - 0.110698519952472, 6.64041537651749e-05, -0.00120174584530713, -0.00133177694921411, - -0.000796422644338886}}; - susHandlingParameters.sus3coeffBeta = { - {-31.5704266802979, -5.10700699133189, 2.84549700473812, -0.122701561048957, -0.11257100034746, - 0.102120576206517, -0.000796645106694696, -0.00192211266325167, -4.99981232866237e-05, - 0.00104036677004523}, - {-0.734294938181273, -0.0694317595592039, 1.34746975389878, -0.00103465544451119, - 0.00389798465946559, -0.00308561832194191, -2.91843250099708e-06, 7.59634622232999e-05, - -6.54571602919161e-05, -0.000104146832644606}, - {24.2649069708536, 3.08145095664586, 1.88975821636026, 0.0767528234206466, -0.0526971951753399, - -0.0477053831942802, 0.000613806533422364, -0.000631628059238499, 0.00026217621127941, - 0.000555307997961608}, - {0.62884078560034, -0.152668817824194, 1.70304497205574, 0.000894387499536142, - -0.00306495168098874, 0.0180087418010658, 1.74990847586174e-05, 3.1263263531046e-05, - -7.1643235604579e-06, 0.000147876621100347}, - {-3.05400297018165, -0.316256447664344, 1.14841722699638, 0.000671621084688467, - -0.000906765726598906, 0.000687041032077189, 0.000323419818039841, 0.000128019308781935, - 0.000286018723737538, 0.000192248693306256}, - {-4.39855066935163, -0.322858945262125, 1.44405016355615, -4.93181749911261e-05, - 0.0127396834052722, -0.00523149676786941, 2.56561922352657e-05, 7.61202764874326e-06, - -0.00014623717850039, 8.12219846932013e-06}, - {110.820397525173, -10.9497307382094, 2.48939759290446, 0.296585618718034, -0.142611297893517, - -0.0141810186612052, -0.00275127095595919, 0.00160686698368569, -0.000872029428758877, - -0.000410522437887563}, - {-7.15740446281205, 0.104233532313688, 1.13155893729292, -0.00350418544400852, - 0.00532058598508803, 0.00459314980222008, 3.09155436939349e-05, -7.60935741692174e-05, - -5.87922606348196e-05, 2.56146268588382e-05}, - {44.8818060495112, -7.94729992210875, 3.59286389225051, 0.217944601088562, 0.108087933176612, - -0.116711715153385, -0.00194260120960441, -0.0015752762498594, -0.000331129410732722, - 0.00125896996438418}}; - susHandlingParameters.sus4orientationMatrix = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; - susHandlingParameters.sus4coeffAlpha = { - {-12.4581187126738, 0.398038572289047, -0.438887880988151, -0.00965382887938283, - -0.0309322349328842, -0.00359106522420111, -7.79546112299913e-06, -0.000432733997178497, - -9.79031907635314e-05, -1.49299384451257e-05}, - {8.41054378583447, 1.87462327360707, 0.266809999719952, 0.0216455385250676, 0.00879426079919981, - -0.00142295319820553, 0.000194819780653264, 8.57549705064449e-05, -3.56478452552367e-05, - -1.65680920554434e-05}, - {16.4141780945815, 2.57697842088604, 0.373972171754278, 0.0498264199400303, 0.0183175817756131, - -0.008545409848878, 0.000422696533006382, -0.000268245978898508, -0.000663188021815416, - -7.51144017137827e-05}, - {0.796692054977593, 1.26773229735266, 0.247715261673662, 0.00358183885438128, - 0.00216435175662881, 0.00713732829335305, -0.000110129715615857, 3.56051594182427e-05, - 5.03074365340535e-05, 8.40279146176271e-05}, - {2.37491588500165, 1.05997969088519, 0.309540461340971, -0.000405047711742513, - 0.000462224730316111, -0.00201887171945793, 0.000260159805167265, -0.000282867209803598, - 0.000201613303652666, -0.000277796442847579}, - {6.36749007598708, 1.31659760017973, -0.122724934153231, -0.00328808937096891, - -0.00577347207798776, 0.00403172074457999, -7.45676459772001e-05, 1.79838644222274e-05, - 0.000104552066440564, -2.78115121929346e-05}, - {-47.9667098848496, 3.97703197139796, -1.96403894754299, -0.0577989657406978, - 0.0634225576208007, -0.0346023445055141, 0.00045886475369098, -0.000326132951996844, - 0.000716490441845967, -0.000136132038635483}, - {6.21505474256094, 0.871830486201601, 0.286906473833627, 0.007875292606045, - -0.00974634725746389, 0.00128416935792136, -0.000111796743751489, 0.000102016719989187, - -3.3503088289589e-05, -1.03874407813931e-05}, - {102.09801265482, -4.12715152309748, -5.04594403360339, 0.075499959116996, 0.216574192561683, - 0.0750031215784663, -0.000147358932612646, -0.0023710703422108, -0.00143310719642393, - -0.000431914403446768}}; - susHandlingParameters.sus4coeffBeta = { - {-21.5077132684032, -1.60004839699939, -0.0298995033958561, -0.0315563250430659, - -0.0424403625879891, -0.0245426225510417, -0.000209861203016225, -0.000422150973104431, - -0.00030514398458781, -0.000211986731019738}, - {9.07644247897601, 0.207457289788099, 1.26735366597312, 0.00768477352180427, - 0.00429230749575816, -0.00514802326062087, 7.56149591998578e-05, 8.42794730840662e-05, - -3.62215715492783e-05, -5.24384190165239e-05}, - {-33.5225408043693, -3.11167857248829, 1.91760591695775, -0.0963752386435729, - 0.00026620241534153, -0.0256680391021823, -0.00102188712837393, 2.63753563968978e-05, - 0.000113172463974702, 0.000271939918507175}, - {19.1379025029401, -0.225979661987912, 2.72337120022998, -0.00136982412154458, - -0.00447301210555274, 0.046496718064139, 2.09123846958985e-05, -4.30383094864847e-05, - -1.22808643520768e-05, 0.000440555709696048}, - {2.957867714783, -0.316069593806939, 1.06379930645214, 0.00103244713047271, 0.00148059212230411, - 0.000557885068990542, 0.000288633931072557, 0.000172775380291659, 0.000269738457990237, - 0.000254577019084984}, - {2.04155199929521, -0.318303488378033, 1.37820715117028, 0.00114788656817743, - 0.0130051117909245, -0.00743109928493789, 1.22403390396844e-05, -3.19245785131217e-05, - -0.000156735218010879, 3.81458400945988e-05}, - {27.314954181241, -1.43916155634084, 2.48967706992348, 0.0278695408478388, -0.0341141456915131, - 0.0281959785297513, -0.000252996164135396, 0.000163365679366542, -0.000380129463154642, - 0.000159350154429114}, - {-0.274693278266294, 0.0199711721436635, 1.26676843352524, -0.0006713759238817, - -0.00389715205101059, 0.00294298337610857, -9.58643121413979e-06, 6.30700938550725e-05, - -6.07188867796123e-05, 7.72199861279611e-06}, - {-74.1601853968901, 2.55641628908672, 6.38533530714782, -0.0582345132980647, - -0.0653804553172819, -0.138850555683872, 0.000489364157827405, 0.000469559629292745, - 0.000698140692952438, 0.00123017528239406}}; - susHandlingParameters.sus5orientationMatrix = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; - susHandlingParameters.sus5coeffAlpha = { - {-12.1398741236355, 1.99425442858125, -1.9303044815802, 0.0418421279520049, -0.0309683799946315, - -0.0562201123081437, 0.000522607299552916, -0.000375386573815007, -0.000183899715035788, - -0.000600349486293698}, - {4.51862054729553, 1.72396080253297, 0.274562680698765, 0.0162681383591035, 0.0108410181586673, - -0.000272215427359511, 0.000124164068046579, 0.000125586897851351, -1.24082224214974e-05, - -1.63339067540159e-05}, - {63.0100748193658, 7.78014670478172, 0.327263471268564, 0.181264302704374, -0.0652454854214506, - -0.03906716801285, 0.00166924078925478, -0.000749939315526625, 0.000320696101132374, - 0.000499934751180042}, - {-2.14377722994325, 1.33617641673436, 0.0973465660282871, 0.00389526886867845, - 0.00526064997381395, 0.00244964888333519, -8.59416490903541e-05, 4.58871931007681e-05, - 8.6123353128647e-05, 2.85447259858337e-05}, - {0.164792977301912, 1.17541977248641, 0.348838798760518, -0.000180865118417534, - 0.000331789515553421, -0.000734333865631793, 9.76677859410759e-05, -0.000324347075049525, - 8.66683396011167e-05, -0.000385839566009832}, - {-0.228934187493575, 1.30552820143752, 0.306779576899158, -0.00508763741184706, - -0.00318524263093038, -0.00878095392529144, -6.59040013073836e-05, 8.69122529321691e-05, - 5.73853071731283e-05, 8.56628414466758e-05}, - {22.6047744510684, -0.591739857860868, 0.566728856847393, 0.0498124268150265, - -0.0214126910277926, 0.00538091942017912, -0.000391517685229849, 0.000554321668236216, - 0.000191004410219065, 0.000102775124022018}, - {4.54704081104052, 0.844841244606025, 0.181355971462193, 0.0109743851006749, - -0.00363467884122547, 0.00108873046814694, -0.000153236888951059, 3.14623342713789e-06, - -2.78503202185463e-05, 3.99983788680736e-06}, - {-30.878359404848, 5.20536009886854, -0.674455093700773, -0.10801865891189, -0.0514805639475938, - 0.0503660452068572, 0.00072776817295273, 0.00120288537038655, -0.000301602375634166, - -0.000477098479809266}}; - susHandlingParameters.sus5coeffBeta = { - {16.8155737032787, 0.65475660868259, 1.95532810363711, 0.000295624718662669, 0.0426379914736747, - 0.00192544771588337, -4.94534888281508e-05, 8.32299142575155e-05, 0.000645497238623369, - -0.000234155227840799}, - {9.48268090632318, 0.528942263930744, 1.34030963800712, 0.0173605129814363, 0.00581086655972212, - -0.00365006277801141, 0.000180048140973223, 0.000102002650672644, -4.10833110241736e-05, - -8.7810396165556e-05}, - {-47.8325489165383, -4.78262055949503, 1.66912859871505, -0.143518014673292, 0.0288441527062856, - -0.00322823115861497, -0.00148509910480755, 0.000284265179004289, -0.000175299737313045, - -7.04175618676909e-05}, - {3.70510151312723, -0.272200626024415, 1.5527519845099, 0.000589727630962265, - -0.00889682554869096, 0.0109857452472628, 3.05876215574877e-05, 2.09194236165814e-05, - -8.33769024439277e-05, 6.90991113575066e-05}, - {0.820199776906695, -0.355683467192776, 1.17142130858009, -0.000160174871610729, - 4.09723480153701e-05, 0.000209103751629257, 0.000390331989170637, 6.45642836249667e-05, - 0.000318092703362044, 0.000107158633760141}, - {5.52084497768914, -0.227775345312466, 0.845897282556327, 0.00157426476122436, - 0.00657189797805861, 0.0103797665963117, 2.51479848048895e-05, -4.78371400399983e-05, - -5.20221896473413e-05, -0.000143840492906166}, - {-33.4875689683454, 0.937557276329106, -1.02741065470967, -0.0140023273976314, - 0.0401908729477037, -0.0512457211360142, 7.05537967426573e-05, -0.00027521752411122, - 0.000407657552700476, -0.000458411000693613}, - {0.931346887326171, -0.320804452025793, 1.28866325376154, 0.00912456151698805, - -0.00404367403569981, 0.00477543659981282, -9.43987917474817e-05, 4.66464249533497e-05, - -7.89362487264572e-05, -1.0951496495443e-05}, - {-38.3689359928435, 3.8540516906071, 1.26391725545116, -0.108584643500539, -0.0542697403292778, - 0.0285360568428252, 0.000845084580479371, 0.00114184315411245, -0.000169538153750085, - -0.000336529204350355}}; - susHandlingParameters.sus6orientationMatrix = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; - susHandlingParameters.sus6coeffAlpha = { - {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}}; - susHandlingParameters.sus6coeffBeta = { - {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}}; - susHandlingParameters.sus7orientationMatrix = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; - susHandlingParameters.sus7coeffAlpha = { - {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, - -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, - -0.00119021101127241, -0.000460110780350978}, - {1.60822506792345, 1.12993931449931, 0.300781032865641, -0.00405149856360946, - 0.0116663280665617, -0.000746071920075153, -8.36092173253351e-05, 0.000126762041147563, - -1.57820750462019e-05, -2.13840141586661e-05}, - {-151.403952985468, -5.77049222793992, 9.71132757422642, -0.113259116970462, 0.284142453949027, - -0.198625061659164, -0.000836450164210354, 0.00174062771509636, -0.00323746390757859, - 0.00124721932086258}, - {3.47391964888809, 1.28788318973591, 0.358380140281919, 0.0033863520864927, 0.00154601909793475, - 0.0103457296050314, -9.56426572270873e-05, 5.48838958555808e-05, 2.97537427220847e-05, - 0.000104735911514185}, - {3.32650947866065, 1.16701012685798, 0.293514063672376, -0.00065850791542434, - -8.61746510464303e-05, -0.00212038990772211, 0.00010377123197, -0.000262818127593837, - 0.000103360882478383, -0.000296739688930329}, - {-0.440176043435378, 1.18923278867097, 0.519516382652818, -0.00138846714677511, - 0.00266491699926247, -0.014254675949624, -4.20279929822439e-05, -5.49260281515447e-05, - -1.00328708454487e-05, 0.000138142092498215}, - {9.54962966738358, 1.83809145920811, 1.82162819067959, -0.0116786627338505, - -0.00496037444422313, 0.0590883547819332, 7.48465315787857e-05, 0.000221693951602584, - 7.96466345174136e-06, 0.000638822537725177}, - {7.04862901290925, 0.876813777672465, 0.16368093989381, 0.00928717461441627, - -0.00276538956293246, 0.00117995419940653, -0.000141511492474493, -6.09796031786385e-06, - -2.62114930414747e-05, -2.88713611443788e-06}, - {135.349147631811, -7.21933296299596, -6.02379024934871, 0.19557354282067, 0.207680233512614, - 0.12880101618361, -0.00169832076532024, -0.00192216719797732, -0.00188763612041332, - -0.00103101801961442}}; - susHandlingParameters.sus7coeffBeta = { - {-12.7115487367622, -1.08890790360556, 0.0579616268854079, -0.0212303293514951, - -0.0395948453851818, -0.0275564242614342, -0.000228652851842222, -0.000148106159109458, - -0.000555136649469199, -0.000198260004582737}, - {-0.988147625946871, -0.759018567468546, 1.20998292002818, -0.0241231836977845, - -0.000572110443300516, -0.00294835038249426, -0.00026533039022186, 6.82250069765274e-06, - 7.21038415209318e-06, -6.54881435118179e-05}, - {98.0979345921564, 4.27381413621355, -4.39956005193548, 0.0709109587666745, -0.172774236139236, - 0.107243391488741, 0.000421832640471043, -0.00140450884710288, 0.00158019019392239, - -0.00078512547169536}, - {4.10892685652543, -0.229301778557857, 1.33380992987117, -0.000250095848720304, - -0.00555205065514645, 0.00355052914398176, 1.62727119770752e-05, -1.26026527654764e-05, - -3.25505031810898e-05, 5.79970895921158e-06}, - {3.09432502337258, -0.300556003790433, 1.17085811008124, 0.00128679594824324, - 0.00148229981422985, 9.15267474159147e-05, 0.000300497843413856, 6.31378865575566e-05, - 0.000258447032558814, 9.79142983264352e-05}, - {8.92336134924575, -0.197306981784312, 0.659908505354084, 0.00175572239373996, - 0.006801023678097, 0.0189775987436792, 9.2187857727721e-06, -4.8706332690626e-05, - -6.887009887486e-05, -0.000266455617735054}, - {-52.0734887320227, 2.64822385560272, -1.72387600304694, -0.0383944891609251, 0.110873671161269, - -0.0475247245070445, 0.000194652401328063, -0.000697307928990137, 0.00124021816001, - -0.000194213899980878}, - {2.08203985879155, -0.127503525368396, 1.17628056094647, 0.00283288065938444, - 0.00394668214608305, 0.00314868636161131, -2.99504350569853e-05, -7.11070816314279e-05, - -6.30148122529749e-05, 2.28114298989664e-05}, - {191.321181158032, -12.2449557187473, -7.21933741885107, 0.267954293388644, 0.331529493933124, - 0.149867703984027, -0.00222279201444128, -0.00284724570619954, -0.00298774060233964, - -0.000988903783752156}}; - susHandlingParameters.sus8orientationMatrix = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}}; - susHandlingParameters.sus8coeffAlpha = { - {5.46354311880959, 1.15370126035432, 0.568432485840475, -0.00105094692478431, - -0.000472899673842554, 0.015581320536192, 2.26460844314248e-05, -0.000254397947062058, - 0.000198938007250408, 0.000102026690279006}, - {8.8976133108173, 1.89502416095352, 0.268670471819199, 0.0217013413241972, 0.00973925295182384, - -0.00116357269193765, 0.000185865842232419, 0.000103311614912702, -2.46539447920969e-05, - -2.06292928734686e-05}, - {-45.4550803910752, 1.27220123406993, 5.21483855848504, 0.0315791081623634, 0.0725172355124129, - -0.13947591535243, 0.000412577580637848, 0.000434545096994917, -0.000840043932292312, - 0.00126857487044307}, - {1.81302768546433, 1.20563501267535, 0.344815267182167, 0.00546879453241056, - -0.00115382996865884, 0.010597876132341, -7.75885604486581e-05, 8.99568815949154e-05, - -2.98129544974679e-06, 0.000108913239345604}, - {2.19111439539173, 1.06951675598148, 0.283707798607213, 0.00016478588207518, - 0.000196086067268121, -0.00214980231173703, 0.000237820475654357, -0.000256402967908595, - 0.000165966620658577, -0.000268394081675921}, - {15.0858674915897, 1.27922724811168, -1.0803137812576, -0.00184009775302466, - -0.00458792284209219, 0.0359393555418547, -6.05121024079603e-05, -1.2288384024143e-05, - 8.55484605384438e-05, -0.000379241348638065}, - {-14.9594190080906, 1.79473182195746, -1.00830704063572, 0.000890685410857856, - 0.0408932029176081, -0.0165460857151619, -0.000170544299916973, -0.000370901607010145, - 0.000324089709129097, -9.33010240878062e-05}, - {0.867614491733251, 1.38248194737027, 0.233408537422123, -0.00772942878114575, - -0.00783126068079782, -0.000413713955432221, 4.5775750146291e-05, 6.97323029940275e-05, - 1.70664456940787e-05, 6.75517901233086e-06}, - {2.34474364146174, -0.777275400251477, 2.09531381577911, 0.0170780716714389, 0.102855060371092, - -0.1203441505925, 0.000187004964420911, -0.00141720441050986, -0.000336251285258365, - 0.00145175125888695}}; - susHandlingParameters.sus8coeffBeta = { - {28.3033101237397, 1.77504446792811, 1.70758838986317, 0.0307800697044683, 0.0598759344275936, - -0.014461432284373, 0.000128415617799076, 0.000664419128546701, 0.000312923304130995, - -0.000269026446641855}, - {7.73040563051023, 0.0267291479555493, 1.16189582308493, 0.000611047892976521, - -0.00213680506915073, -0.00517435586596902, -3.60304406049766e-06, -1.74452976404459e-05, - -3.95396925228538e-05, -7.01948519410633e-05}, - {-48.0766126130725, -3.77981206700298, 3.03482861087335, -0.0678496412519532, 0.115260678424016, - -0.0109681510065038, -0.000438011443691466, 0.00097230136258486, -0.000930875177732769, - -0.000203144239955507}, - {12.1881935626341, -0.234345089308583, 2.01134619426134, 0.000181529284001169, - -0.00642848065105061, 0.0243985799415726, 2.0224042581776e-05, 5.22503286757285e-06, - -4.75196303016323e-05, 0.000221160482364556}, - {3.49559433498742, -0.294995112674766, 1.07892379698257, 0.000861664794052587, - 0.00138978933062055, 0.000436385106465176, 0.000288095124755908, 0.000147259769247883, - 0.000256686898599516, 0.000198982412957039}, - {9.36663996178607, -0.171266136751803, 0.799869891484541, -0.000896305696610864, - 0.00477919972789653, 0.0077876110326094, 9.16475263625076e-06, 3.02461250100473e-05, - -3.63917701783264e-05, -0.000101376940843402}, - {9.93372683055145, 1.02056557854246, 3.01635426591734, -0.0477113881140277, -0.0280426434406976, - 0.0438813017696874, 0.000470431190169998, -7.55753674679743e-05, -0.000516444906323815, - 0.000253844418223843}, - {4.12868774589741, -0.305711790187688, 1.15739216407191, 0.00942395115281056, - 0.00264758462357433, 0.00227985850688479, -0.000107537164019682, -4.91396736189963e-05, - -5.3803493814502e-05, 6.80587059728718e-06}, - {64.9193383444005, -1.57724255547465, -3.82166532626293, 0.0104712238987591, 0.0898786950946473, - 0.128910456296131, -8.27123227422217e-05, -0.000143979624107479, -0.00146684876653306, - -0.00102226799570239}}; - susHandlingParameters.sus9orientationMatrix = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; - susHandlingParameters.sus9coeffAlpha = { - {65.8975109449121, 2.19115342242175, 6.11069527811832, -0.0219884864133703, 0.119985456538482, - 0.142746712551924, -0.000465882328687976, 0.000606525132125852, 0.00141667074621881, - 0.00109715845894006}, - {5.70337356029945, 1.86705636976809, 0.235584190291708, 0.0194937327615426, 0.00973291465247784, - -0.00155675297510773, 0.000147099297988423, 0.000115708967219349, -4.1462310493722e-05, - -9.80097031103588e-06}, - {138.221145997284, 6.07665575619595, -9.08085914250542, 0.0839801072927519, -0.143071750033303, - 0.237868300719915, 0.000626693630444932, -0.000579788170871402, 0.00181740650944343, - -0.00207086879728281}, - {-7.78295582666151, 1.37506685179192, -0.507596181420042, 0.00350118305456038, - 0.00380814310115541, -0.0174012437563343, -0.000124801268056815, 2.96314830184492e-05, - 6.3416992450033e-05, -0.000190177262510221}, - {0.13102597129751, 1.24228303845143, 0.328808873447393, 2.6858679536165e-05, - 0.000231428138164498, -0.000584089095259736, 5.5322167970451e-05, -0.000322205709821716, - 7.71348293209208e-05, -0.000393885990364776}, - {4.64571633968935, 1.2668223691397, -0.158952088650432, -0.0038344859267428, 0.0030051503726095, - 0.00455578826025588, -9.42520993914957e-05, 5.81633314412289e-05, -4.43545804544095e-05, - -4.83524454851519e-05}, - {99.2385930314563, -3.65569343617926, 5.54203926675588, 0.0975630395981933, -0.15701634159692, - 0.107834711298836, -0.000885326636237814, 0.000960753844480462, -0.00179894024848343, - 0.000583066757644971}, - {2.82671549736619, 1.11214198870501, 0.214735318432744, 0.00284415167563662, - -0.00743289575690122, 0.000382705440762292, -7.43232442872501e-05, 6.96994098083348e-05, - -4.15108111710131e-06, 1.33520085213482e-05}, - {36.9013743125415, -0.522392401546163, -1.52452843963663, 0.0261375433218879, 0.060573568610239, - 0.0182582125221054, -0.000244373383911157, -0.000271385147292484, -0.000723799969427732, - 6.76324880239196e-05}}; - susHandlingParameters.sus9coeffBeta = { - {128.70886435409, 7.27355509732751, 7.18142203531244, 0.1536100459329, 0.199455846541636, - 0.101824964939793, 0.00116666116789421, 0.00181595584079788, 0.00159271319494017, - 0.000556768406475719}, - {-7.07933839681024, -0.979062424441878, 1.21792546815617, -0.0295740143783226, - -0.00442780611714201, -0.00329612819203176, -0.000291373125216143, -7.47259350176359e-05, - -4.87265282482212e-05, -7.87490350444332e-05}, - {41.1357193180502, 2.75138456414254, -0.0160889117718198, 0.0274001112562423, - -0.145644717742057, -0.0316076203283094, -0.000136443337244472, -0.00153945199081365, - 0.000938960439977633, 0.000599987111822885}, - {2.7980384746608, -0.234741037383589, 1.5342193016705, -0.000993791566721689, - -0.00787533639513478, 0.00927468655141365, 2.63308697896639e-05, -3.42816267184975e-05, - -8.48879419798771e-05, 3.84043821333798e-05}, - {0.427687530667804, -0.346076633694936, 1.22968527483851, -4.95098138311122e-05, - 0.000298245372198029, 0.000332756250024796, 0.00040375986210644, 5.20675972504572e-05, - 0.000327042170278218, 5.93011568264671e-05}, - {4.50337810133314, -0.279364254817202, 0.945812187846199, 0.000116182663432306, - 0.0115646046622083, 0.00908289960302886, 1.90394667311541e-05, -4.4360223646434e-06, - -0.000131398914898614, -0.000145568992865512}, - {-36.3377213654193, 2.21047221783626, 0.0609982245149821, -0.0670546774988572, - 0.016827777144747, -0.0277834084058314, 0.000778301409125556, 0.000135846745194401, - 0.00043261858797068, -0.00021172728254561}, - {-0.737678205841529, -0.217352122193475, 1.23494846329297, 0.00748173441779792, - 0.0019595873704705, 0.00567253723266176, -8.34768773292938e-05, -3.50608394184873e-05, - -0.000107500091550635, -5.1379722947632e-07}, - {-36.6150844777671, 3.24952006904945, 1.7222457840185, -0.0846362445435584, -0.0625549615377418, - 0.019178365782485, 0.000664877496455304, 0.000942971403881222, 0.000190754698755098, - -0.000372226659190439}}; - susHandlingParameters.sus10orientationMatrix = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; - susHandlingParameters.sus10coeffAlpha = { - {14.4562393748324, 0.669162330324919, 2.13895255446541, -0.0161997097021299, - 0.00185995785065838, 0.0621351118528379, -0.000278999272493087, 0.000238469666491965, - -0.000279407497782961, 0.000726904943739837}, - {-4.45678285887022, 0.92869611919737, 0.186752102727282, -0.00706160758952316, - 0.00532680276723634, -0.00119102617674229, -0.000105283880098953, 3.90673052334419e-05, - -3.13338277344246e-05, 5.32977236959767e-06}, - {30.4255268053197, 3.00991076401191, -1.4855621363519, 0.033934286288413, -0.0553588742704929, - 0.0299275582316466, 0.000167915322354466, -0.00050925078118232, 0.000463662961330962, - -0.000232919143454163}, - {2.45076465343337, 1.30206564388838, 0.635121046212765, 0.00517109639797675, - 0.00360579544364496, 0.0198490668911362, -9.31556816982662e-05, 6.7313653707875e-05, - 6.4669137025142e-05, 0.000209727581169138}, - {-0.784841314851562, 1.10058314980836, 0.314063830836532, 0.000583003703415889, - 0.000312635453606579, -0.000183738114552387, 0.000214096205760617, -0.000286744686021244, - 0.000159157597180407, -0.00032235099420715}, - {7.19568036510586, 1.33307479701657, -0.465585141952456, -0.0031910726544199, - -0.00546273504371797, 0.0145494754402526, -7.9863949693769e-05, 4.83681329120104e-05, - 8.85844309936609e-05, -0.000143217870916994}, - {-12.8344546267449, 1.36023633150143, -0.728527724854506, 0.019982118403416, 0.0385056413989437, - -0.00468598272326268, -0.000303957957649245, -6.37783846968216e-05, 0.000514049116643205, - 0.000112015427600697}, - {-2.58279031298065, 1.42167821629586, 0.208769467511292, -0.00640190372145885, - -0.0056405289717473, 0.000509611313918708, 2.23310562107823e-05, 3.23685469522147e-05, - -7.55982776243849e-06, 2.78417756661088e-06}, - {-29.7178996143914, 2.636972251183, 1.97316329325243, -0.03983524158327, -0.0193152048730234, - -0.0600902798379509, 0.00031786916010672, 0.000162178988605602, 0.000224550786416246, - 0.000614337977361927}}; - susHandlingParameters.sus10coeffBeta = { - {12.4771349792459, 1.44317849705414, 0.975637226331561, 0.0430284146301043, 0.0220810531548995, - -0.0220926906772, 0.000310052324529521, 0.000658151808869523, -0.000288026365111098, - -0.000214619731807045}, - {-0.113203260140131, -0.272424061092191, 1.27704377191184, -0.00791746619331075, - 0.00278646694862191, -0.00398881099259934, -8.09569694307212e-05, 5.99617384829016e-05, - -5.4550919751855e-05, -8.6314530565085e-05}, - {-48.585664295448, -2.04899787231903, 4.48757129623549, -0.0226180460431321, 0.090326735447661, - -0.0722998813632622, -6.77623771415477e-05, 0.000562585419036509, -0.000956171370931993, - 0.000491554402311223}, - {-1.20986884955482, -0.215604107185474, 1.22123198786617, 0.000256508527822089, - -0.00625056735692847, 0.00262961582224303, 2.27433984698861e-05, 1.60471509861372e-05, - -4.85061736834262e-05, -1.8387092782907e-06}, - {-0.250205907903409, -0.315819331560782, 1.09018364376391, -0.000521787614293089, - -0.000500747760913489, 2.48184307342838e-05, 0.000313799238640988, 0.000136669146368744, - 0.000278914324565192, 0.000218512838469476}, - {-1.38512578184076, -0.240456589364121, 1.34170304231345, 0.00017499230372669, - 0.0070862275911073, -0.00460640844814105, 1.27594111036696e-05, -4.73855624902052e-06, - -5.41141037812903e-05, 8.50767021818388e-06}, - {58.9917559342216, -2.28705697628345, 5.35995190407842, 0.0214721399750612, -0.112195722921667, - 0.0890150265857542, -0.000100675657768708, 0.000493488022135339, -0.00137672908303878, - 0.000518683157694955}, - {3.18905073365834, -0.633376549706314, 1.17190259811174, 0.0188817945597344, - 0.00107470708915782, 0.00400880471375267, -0.000197312295539118, -2.46543035998379e-05, - -6.07871064300252e-05, 1.91822310311955e-05}, - {-21.6881499304099, -0.563186103920008, 3.70747028664292, 0.021112883967427, - -0.00650020689049325, -0.0778533644688476, -0.000131921888670268, -0.000402754836445439, - 0.000551249824375055, 0.00062236627391337}}; - susHandlingParameters.sus11orientationMatrix = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; - susHandlingParameters.sus11coeffAlpha = { - {-5.23569698615548, -1.45500092391928, 2.7643243644756, -0.0762912296128707, - -0.0201645929971608, 0.0997226845779083, -0.000741669441569556, -0.000485368004931528, - 0.000166230212359982, 0.00103455037278067}, - {-7.7405077383712, 0.892040861541276, 0.39014957203484, -0.00952030929935314, - 0.0185577462685363, 0.000500600568760257, -0.000151227821554572, 0.000245334737283439, - 1.89380065823205e-05, 1.83481122973969e-07}, - {-27.462143709831, -1.68192474287299, 0.689411302961069, -0.0146021086710062, 0.227153492753605, - 0.0815806579791421, 2.92919049429149e-05, 0.00153760357651792, -0.00247865821192621, - -0.00166333309739387}, - {-6.74664748624368, 1.43279156053015, 0.0212787292914553, 0.00764792230365732, - 0.00796410301290615, 0.0014384998868733, -8.95239151813685e-05, 9.55245417090909e-05, - 0.000127251739461239, 3.26943341606363e-05}, - {-2.20391533717674, 1.32902400478083, 0.38633027011889, 0.00104660852197061, - 0.00105228824412283, -0.00242067551428214, -6.98346290136652e-05, -0.000369075232184835, - -1.59510520000704e-05, -0.000448565104826966}, - {-5.29476778147188, 1.4549556336236, 0.561334186252557, -0.00260896342687109, - -0.00855934179001141, -0.0182515354646452, -8.79385828606048e-05, 5.98357681659175e-05, - 0.000146570207542509, 0.000201596912834554}, - {-45.7906613832612, 3.77630104475902, -1.67494598155515, -0.0545433897761635, 0.047897938410221, - -0.0355687158405231, 0.000374738707508583, -0.000448453494537518, 0.000377784972619365, - -0.000276573228333836}, - {-9.11681182090372, 2.06933872940742, 0.26131496122122, -0.0259534033367855, - -0.00777266937872862, -0.00262135395843891, 0.000223790782655445, 6.40488537928934e-05, - 7.75581514100296e-05, -9.25934285039627e-06}, - {183.243883340634, -8.02281039502717, -10.0756951652703, 0.168750521462303, 0.314006821405967, - 0.200264755034566, -0.0011895153717447, -0.00253812476819896, -0.00291324393641628, - -0.00140062522117514}}; - susHandlingParameters.sus11coeffBeta = { - {34.4499366074013, -0.438583698052091, 4.72111001451028, -0.041810050989433, 0.0562461093661426, - 0.0856849892524893, -0.000477813051406167, -3.16404257494464e-05, 0.00102633196865105, - 0.000552974013759876}, - {7.6366298088699, 0.150314752958302, 1.31364679484924, 0.00557696667395871, 0.00163731860604376, - -0.00454759608980269, 5.83979683674572e-05, 4.45944881220665e-05, -4.27874746147066e-05, - -8.77418673597557e-05}, - {130.156215011893, 1.85759000444524, -10.986892391833, -0.00686275191260681, -0.188837138116058, - 0.346177462085361, -0.000183276946352264, -0.000702183496893294, 0.00293145272693857, - -0.00318194442670715}, - {-1.67854820161036, -0.358899332859806, 0.956690839640595, -4.93862910503803e-05, - -0.0136134783014874, -0.00848731301504507, 3.75950499927045e-05, 1.35374694383289e-06, - -0.000156596507890443, -0.000123254220377897}, - {3.67569209537767, -0.387260959713287, 1.31343215605952, -0.00206444615206506, - 0.00145334813110285, -0.00151259497696238, 0.000449492568365603, 6.95883968949488e-07, - 0.000368585523744765, -6.3420715525635e-05}, - {14.3296323024886, -0.182979476956897, 0.306817119309235, -0.00022212115978293, - 0.00463485302909649, 0.0277574953550035, 1.1422454625565e-05, 1.06053257479502e-05, - -2.05720000720608e-05, -0.000338584671430337}, - {-18.7534921817754, 1.14272710923224, 0.460498062012866, -0.00995826989278202, - 0.0658502318647112, 0.00616942819937029, -7.70857153768402e-05, -0.000641755741925561, - 0.00047849204592989, 0.000158509018296766}, - {1.26543621388607, -0.176674379740481, 1.38814920935488, 0.00545485262295305, - -0.00499775616702264, 0.0038057039142173, -6.59604252054511e-05, 6.40211116049053e-05, - -6.74778593434431e-05, -2.81973589469059e-05}, - {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, - 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, - -0.00168429567131815}}; - - - rwHandlingParameters.inertiaWheel = 0.000028198; - rwHandlingParameters.maxTrq = 0.0032; //3.2 [mNm] - - //Geometry frame - rwMatrices.alignmentMatrix = { - { 0.9205, 0.0000, -0.9205, 0.0000}, - { 0.0000, -0.9205, 0.0000, 0.9205}, - { 0.3907, 0.3907, 0.3907, 0.3907}}; - - rwMatrices.pseudoInverse = { - { 0.4434, -0.2845, 0.3597}, - { 0.2136, -0.3317, 1.0123}, - { -0.8672, -0.1406, 0.1778}, - { 0.6426, 0.4794, 1.3603}}; - - rwMatrices.nullspace = { -0.7358, 0.5469, -0.3637, -0.1649}; - - kalmanFilterParameters.sensorNoiseSS = 8 * Math::PI / 180; - kalmanFilterParameters.sensorNoiseMAG = 4 * Math::PI / 180; - kalmanFilterParameters.sensorNoiseSTR = 0.1 * Math::PI / 180; - - sunModelParameters.domega = 36000.771; - sunModelParameters.omega_0 = 282.94 * Math::PI / 180.; - sunModelParameters.dm = 35999.049; - sunModelParameters.m_0 = 357.5256; - sunModelParameters.e = 23.4392911 * Math::PI / 180.; - sunModelParameters.e = 0.74508 * Math::PI / 180.; - sunModelParameters.p1 = 6892. / 3600. * Math::PI / 180.; - sunModelParameters.p2 = 72. / 3600. * Math::PI / 180.; - - safeModeControllerParameters.k_rate_mekf = 0.00059437; - safeModeControllerParameters.k_align_mekf = 0.000056875; - - safeModeControllerParameters.sunTargetDir = {1,0,0}; - - detumbleCtrlParameters.gainD = pow(10.0,-3.3); - - // Stuttgart - groundStationParameters.latitudeGs = 48.7495 * Math::PI / 180.; - groundStationParameters.longitudeGs = 9.10384 * Math::PI / 180.; - groundStationParameters.altitudeGs = 500; - - groundStationParameters.earthRadiusEquat = 6378137; - groundStationParameters.earthRadiusPolar = 6356752.314; - -// Geometry frame - targetModeControllerParameters.refDirection = {1,0,0}; - targetModeControllerParameters.refRotRate = {0,0,0}; - targetModeControllerParameters.quatRef = {0,0,0,1}; - - targetModeControllerParameters.avoidBlindStr = true; - targetModeControllerParameters.blindAvoidStart = 1.5; - targetModeControllerParameters.blindAvoidStop = 2.5; - targetModeControllerParameters.blindRotRate = 1 * Math::PI /180; - - targetModeControllerParameters.zeta = 0.3; - targetModeControllerParameters.om = 0.3; - targetModeControllerParameters.qiMin = 0.1; - targetModeControllerParameters.omMax = 1 * Math::PI / 180; - targetModeControllerParameters.gainNullspace = 0.01; - - targetModeControllerParameters.desatMomentumRef = {0,0,0}; - targetModeControllerParameters.deSatGainFactor = 1000; - targetModeControllerParameters.desatOn = true; - - targetModeControllerParameters.omegaEarth = 0.000072921158553; - - strParameters.boresightAxis = { 0.7593, 0,-0.6508}; - strParameters.exclusionAngle = 20 * Math::PI /180; - - magnetorquesParameter.mtq0orientationMatrix = { - { 1, 0, 0}, - { 0, 0, 1}, - { 0,-1, 0}}; - magnetorquesParameter.mtq1orientationMatrix = { - { 1, 0, 0}, - { 0, 1, 0}, - { 0, 0, 1}}; - magnetorquesParameter.mtq2orientationMatrix = { - { 0, 0, 1}, - { 0, 1, 0}, - {-1, 0, 0}}; - magnetorquesParameter.alignmentMatrixMtq = { - { 0, 0,-1}, - {-1, 0, 0}, - { 0, 1, 0}}; - magnetorquesParameter.inverseAlignment = { - { 0,-1, 0}, - { 0, 0, 1}, - {-1, 0, 0}}; - magnetorquesParameter.DipolMax = 0.2; - -} - -AcsParameters::~AcsParameters() { -} +AcsParameters::~AcsParameters() {} /* ReturnValue_t AcsParameters::getParameter(uint8_t domainId, diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 494ddb68..d0d67691 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -6,6 +6,7 @@ #ifndef ACSPARAMETERS_H_ #define ACSPARAMETERS_H_ +#include #include typedef unsigned char uint8_t; @@ -21,60 +22,762 @@ public: const ParameterWrapper *newValues, uint16_t startAtIndex); struct OnBoardParams { - double sampleTime; // [s] + double sampleTime = 0.1; // [s] } onBoardParams; struct InertiaEIVE { - double inertiaMatrix[3][3]; + 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]; } inertiaEIVE; struct MgmHandlingParameters { - float mgm0orientationMatrix[3][3]; - float mgm1orientationMatrix[3][3]; - float mgm2orientationMatrix[3][3]; - float mgm3orientationMatrix[3][3]; - float mgm4orientationMatrix[3][3]; + 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 mgm4orientationMatrix[3][3] = { + { 0, 0,-1}, + {-1, 0, 0}, + { 0, 1, 0}}; } mgmHandlingParameters; struct SusHandlingParameters { - float sus0orientationMatrix[3][3]; - float sus1orientationMatrix[3][3]; - float sus2orientationMatrix[3][3]; - float sus3orientationMatrix[3][3]; - float sus4orientationMatrix[3][3]; - float sus5orientationMatrix[3][3]; - float sus6orientationMatrix[3][3]; - float sus7orientationMatrix[3][3]; - float sus8orientationMatrix[3][3]; - float sus9orientationMatrix[3][3]; - float sus10orientationMatrix[3][3]; - float sus11orientationMatrix[3][3]; + float sus0orientationMatrix[3][3] = { + { 0, 0, 1}, + { 1, 0, 0}, + { 0, 1, 0}}; // FM07 + 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 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 + float sus10orientationMatrix[3][3] = { + { 0,-1, 0}, + { 1, 0, 0}, + { 0, 0, 1}}; // FM09 + float sus11orientationMatrix[3][3] = { + { 1, 0, 0}, + { 0,-1, 0}, + { 0, 0,-1}}; // FM08 - float sus0coeffAlpha[9][10]; // FM07 - float sus0coeffBeta[9][10]; - float sus1coeffAlpha[9][10]; // FM06 - float sus1coeffBeta[9][10]; - float sus2coeffAlpha[9][10]; // FM13 - float sus2coeffBeta[9][10]; - float sus3coeffAlpha[9][10]; // FM14 - float sus3coeffBeta[9][10]; - float sus4coeffAlpha[9][10]; // FM05 - float sus4coeffBeta[9][10]; - float sus5coeffAlpha[9][10]; // FM02 - float sus5coeffBeta[9][10]; - float sus6coeffAlpha[9][10]; // FM10 - float sus6coeffBeta[9][10]; - float sus7coeffAlpha[9][10]; // FM01 - float sus7coeffBeta[9][10]; - float sus8coeffAlpha[9][10]; // FM03 - float sus8coeffBeta[9][10]; - float sus9coeffAlpha[9][10]; // FM11 - float sus9coeffBeta[9][10]; - float sus10coeffAlpha[9][10]; // FM09 - float sus10coeffBeta[9][10]; - float sus11coeffAlpha[9][10]; // FM08 - float sus11coeffBeta[9][10]; + 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}}; + 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}}; + float sus1coeffAlpha[9][10] = { + {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, + -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, + -0.000178087038629721, 4.09208968678946e-05}, + {2.24803085641869, 1.42938692406645, 0.30104994020693, 0.00756499999397385, 0.0117765927439368, + -0.000743685980641362, 4.69920803836194e-05, 0.000129815636957956, -9.10792250542345e-06, + -2.03870119873411e-05}, + {26.9943033817917, 0.147791175366868, -3.48256070200564, -0.0303332422478656, + 0.0183377266255394, 0.124593616125966, -0.000466003049304431, -0.000272000698791331, + -0.00063621309529853, -0.00158363678978767}, + {-0.221893380318465, 1.29919955307083, 0.21872487901019, 0.0049448219667127, + 0.00291224091529189, 0.00654651987282984, -9.86842469311185e-05, 8.20057454706638e-05, + 6.42331081725944e-05, 7.11656918299053e-05}, + {1.40178843964621, 1.1733111455249, 0.287485528779234, -0.000793970428759834, + 0.000170529273905818, -0.00268807864923086, 9.09553964483881e-05, -0.000271892733575409, + 8.52016306311741e-05, -0.000291797625433646}, + {0.65549617899457, 1.25716478394514, 0.301396415134214, -0.00357289640403958, + -0.000473416364133431, -0.010760332636205, -9.77220176481185e-05, 4.40798040046875e-05, + 2.84958344955681e-05, 0.000128583400693359}, + {6.20958048145025, 1.9528406481596, 1.32915657614139, -0.0326944423378284, -0.0158258335207969, + 0.0328249756354635, 0.00027113042931131, -0.000133980867173428, -0.000357964552318811, + 0.000224235061786191}, + {2.46222812180944, 1.1731834908026, 0.17440330925151, -0.00132279581980401, + -0.00447202005426964, -0.000804321602550913, -1.59526570766446e-05, 2.62946483533391e-05, + 3.28466749016414e-05, -6.63837547601294e-06}, + {42.615758859473, 2.46617281707273, -5.742515881283, -0.131942799763164, 0.20250702826603, + 0.0981562802911027, 0.00189939440077981, -0.0018591621618441, -0.00161121179693977, + -0.00058814458116749}}; + float sus1coeffBeta[9][10] = { + {-12.300032617206, -1.06640894101328, 0.33950802247214, -0.00890867870617722, -0.04872758086642, + -0.0114263851027856, 0.000141061196404012, -0.000675469545483099, -0.000138249928781575, + -0.000138871036200597}, + {10.1631114109768, 0.261654603839785, 1.2376413405181, 0.00888558138614535, 0.00151674939001532, + -0.00534577602313027, 9.19430013005559e-05, 5.39804599087081e-05, -4.15760162347772e-05, + -7.60797902457032e-05}, + {-30.142329062199, 1.26939195100229, 6.14467186367471, 0.0464163689935328, 0.00379001947505376, + -0.165444163648109, 0.000516545385538741, 1.56053219154647e-05, -5.58651971370719e-05, + 0.00173185063955313}, + {12.1454103989862, -0.243589095509132, 2.02543716988677, -0.000857989774598331, + -0.00705278543432513, 0.0250580538307654, 3.50683653081847e-05, -2.63093897408875e-05, + -5.67352645830913e-05, 0.000232270832022029}, + {4.4338108906594, -0.305276965994378, 1.17293558142526, 0.000152618994429577, + 0.00134432642920902, -0.00104036813342885, 0.000334476082056995, 6.74826804343671e-05, + 0.000275311897725414, 7.58157740577916e-05}, + {3.47680700379043, -0.154163381023597, 1.389579838768, 0.000799705880026268, + 0.00401980026462874, -0.00915311817354667, -2.54817301605075e-06, -2.27422984169921e-05, + -2.61224817848938e-05, 6.00381132540332e-05}, + {29.469181543703, -0.722888948550437, 3.3623377135197, 0.00148445490093232, -0.0474780142430845, + 0.0486755575785462, 0.000126295091963757, 0.000526632230895258, -0.000259305985126003, + 0.000412751148048724}, + {2.67029041722834, -0.0837968038501666, 1.37628504937018, 0.00165061312885753, + -0.00953813055064273, 0.0032433005486936, -1.6522452172598e-05, 0.000144574078261271, + -8.47348746872376e-05, -1.92509604512729e-06}, + {-20.959201441285, -2.23605897639125, 5.73044624806043, 0.0354141964763815, 0.0887545371234514, + -0.193862330062381, 0.000216532998121618, -0.00207707610520973, 0.000552928905346826, + 0.00190182163597828}}; + float sus2coeffAlpha[9][10] = { + {6.51602979328333, 0.690575501042577, 1.18185457002269, -0.0153161662266588, + 0.00145972227341484, 0.0351496474730776, -0.000172645571366945, -6.04213053580018e-05, + 9.74494676304114e-05, 0.000334122888261002}, + {0.954398509323963, 1.10996214782069, 0.313314231563221, -0.00367553051112208, + 0.0110290193380194, 0.000240079475656232, -6.93444423181303e-05, 0.000107433381295167, + 1.30750132315838e-05, -2.43580795300515e-05}, + {-55.1159841655056, -1.47449655191106, 3.40106264596874, -0.0621428271456258, + 0.0659788065633613, -0.0791732068323335, -0.000524264070592741, 0.000582093651418709, + -0.000586102213707195, 0.000658133691098817}, + {1.98614148820353, 1.32058724763677, 0.156843003413303, 0.002748082456053, 0.00202677073171519, + 0.00382360695862248, -0.000122364309010211, 5.33354637965168e-05, 3.93641210098335e-05, + 4.06398431916703e-05}, + {3.41223117010734, 1.1597568029329, 0.31881674291653, -0.000382400010917784, + -0.000754945672515052, -0.00079200882313927, 0.000145713118224563, -0.00026910957285589, + 0.000137876961532787, -0.000326798596746712}, + {6.23333031852853, 1.24902998148103, -0.0162317540018123, -0.00338184464699201, + 0.000420329743164687, 0.00202038442335185, -7.10435889754986e-05, -6.04039458988991e-06, + 7.25318569569788e-06, -2.5930447720704e-05}, + {191.759784636909, -10.5228276216193, 8.48306234734519, 0.243240262512846, -0.344226468125615, + 0.126267158197535, -0.00186612281541009, 0.00304415728817747, -0.00304958575196089, + 0.000457236034569107}, + {5.61375025356727, 1.1692295110657, 0.224665256727786, -0.00230481633344849, + -0.00746693012026367, -0.00172583925345173, -7.00823444553058e-06, 7.31362778266959e-05, + 5.81988007269583e-05, 1.3723604109425e-05}, + {98.0250669452855, -2.18500123986039, -6.68238707939385, 0.000754807832106659, + 0.256133336978808, 0.110826583415768, 0.000457663127670018, -0.00197655629847616, + -0.00254305206375073, -0.000466731538082995}}; + float sus2coeffBeta[9][10] = { + {41.1102358678699, 2.3034699186519, 2.74551448799899, 0.061701310929235, 0.0317074142089495, + 0.0308171492962288, 0.00049453042200054, 0.000519222896270701, 2.85420168881716e-05, + 0.000259197384126413}, + {4.46821725251333, 0.0125273331991983, 1.32640678842532, 0.000543566569079156, + 0.00396616601484022, -0.00488408099728387, -3.05734704054868e-06, 7.3424831303621e-05, + -5.49439160235527e-05, -8.30708110469922e-05}, + {64.773396165255, 2.97057686090134, -1.90770757709096, 0.062747116236773, -0.077990648565002, + 0.0613989204238974, 0.00055512113297293, -0.000347045533958329, 0.00104059576098392, + -0.000348638726253297}, + {3.10352939390402, -0.2376108554276, 1.60523925160222, 0.00116454605680723, -0.0067958260462381, + 0.0136561370875238, 2.59929059167486e-05, 3.33825895937897e-05, -5.55828531601728e-05, + 0.000109833374761172}, + {0.156052891975873, -0.320721597024578, 1.15208488414874, 0.00164743688819939, + 0.000534718891498932, 0.000469870758457642, 0.000308432468885207, 0.00011789470679678, + 0.000292373398965513, 0.000183599033441813}, + {2.84967971406268, -0.21374251183113, 1.09938586447269, 2.34894704600407e-05, + 0.00588345375399262, 0.00296966835738407, 8.42707308834155e-06, 2.81870099202641e-06, + -3.56732787246631e-05, -7.04534663356379e-05}, + {-7.59892007483895, 0.358662160515702, 0.805137646978357, 0.00222144376998348, + 0.0464438387809707, 0.00847551828841782, 3.24805702347551e-05, 5.45500807838332e-05, + 0.000941378089367713, 0.000353137737023192}, + {-4.65367165487109, 0.201306010390421, 1.19135575710523, -0.00692801521395975, + 0.00394118754078443, 0.00426360093528599, 6.297683536736e-05, -7.15794236895102e-05, + -7.47076172176468e-05, -1.94516917836346e-05}, + {-59.5882618930651, 3.84530212586425, 3.50497032358686, -0.116100453177197, -0.0380997421813177, + -0.0581898335691109, 0.00111464935006159, 0.000559313074537689, 0.000168067749764069, + 0.000563224178849256}}; + float sus3coeffAlpha[9][10] = { + {-174.687021034355, -7.53454036765748, -9.33798316371397, -0.18212338430986, -0.242523652239734, + -0.202086838965846, -0.00138648793335223, -0.00225430176012882, -0.00198887215340364, + -0.00160678535160774}, + {6.92009692410602, 1.8192037428209, 0.254908171908415, 0.0179273243472017, 0.00894059238779664, + -0.000436952529644, 0.000138070523903458, 9.22759645920339e-05, -9.4312261303588e-06, + -1.76647897892869e-05}, + {-17.9720006944368, 0.230510201259892, 1.10751755772907, -0.00973621304161327, + 0.0554450499198677, -0.00590970792122449, -2.92393772526592e-05, 0.000444329929586969, + -0.000436055839773919, -9.5869891049503e-05}, + {-4.9880829382985, 1.33627775121504, -0.330382157073243, 0.00306744056311184, + 0.00376353074674973, -0.0107453978169225, -0.00010680477021693, 5.17225535432745e-05, + 7.4423443938376e-05, -0.000107927900087035}, + {0.952867982900728, 1.14513280899596, 0.307744203675505, 0.000404669974219378, + -0.000737988606997615, 0.00120218232577844, 0.000191147653645603, -0.000275058867995882, + 0.000137187356620739, -0.000320202731145004}, + {8.076706574364, 1.31338618710295, -0.334634356394277, -0.00209719438033295, + -0.00381753503582303, 0.0100347823323616, -7.00550548221671e-05, -1.97626956996069e-05, + 7.80079707003333e-05, -8.95904360920744e-05}, + {-82.4748312650249, 8.63074484663009, -0.949295700187556, -0.178618807265278, 0.130143669167547, + 0.0284326533865768, 0.00149831261351137, -0.0011583692969717, 0.0010560778729661, + 0.000635404380970666}, + {3.34457857521978, 1.09242517408071, 0.241722402244944, 0.00381629887587041, + -0.00863580122530851, 0.00137050492069702, -8.91046701171713e-05, 8.44169683308007e-05, + -3.54608413548779e-05, 8.54042677832451e-06}, + {78.1540457908649, -1.30266922193303, -5.33605443700115, 0.0184226131926499, 0.146629920899062, + 0.110698519952472, 6.64041537651749e-05, -0.00120174584530713, -0.00133177694921411, + -0.000796422644338886}}; + float sus3coeffBeta[9][10] = { + {-31.5704266802979, -5.10700699133189, 2.84549700473812, -0.122701561048957, -0.11257100034746, + 0.102120576206517, -0.000796645106694696, -0.00192211266325167, -4.99981232866237e-05, + 0.00104036677004523}, + {-0.734294938181273, -0.0694317595592039, 1.34746975389878, -0.00103465544451119, + 0.00389798465946559, -0.00308561832194191, -2.91843250099708e-06, 7.59634622232999e-05, + -6.54571602919161e-05, -0.000104146832644606}, + {24.2649069708536, 3.08145095664586, 1.88975821636026, 0.0767528234206466, -0.0526971951753399, + -0.0477053831942802, 0.000613806533422364, -0.000631628059238499, 0.00026217621127941, + 0.000555307997961608}, + {0.62884078560034, -0.152668817824194, 1.70304497205574, 0.000894387499536142, + -0.00306495168098874, 0.0180087418010658, 1.74990847586174e-05, 3.1263263531046e-05, + -7.1643235604579e-06, 0.000147876621100347}, + {-3.05400297018165, -0.316256447664344, 1.14841722699638, 0.000671621084688467, + -0.000906765726598906, 0.000687041032077189, 0.000323419818039841, 0.000128019308781935, + 0.000286018723737538, 0.000192248693306256}, + {-4.39855066935163, -0.322858945262125, 1.44405016355615, -4.93181749911261e-05, + 0.0127396834052722, -0.00523149676786941, 2.56561922352657e-05, 7.61202764874326e-06, + -0.00014623717850039, 8.12219846932013e-06}, + {110.820397525173, -10.9497307382094, 2.48939759290446, 0.296585618718034, -0.142611297893517, + -0.0141810186612052, -0.00275127095595919, 0.00160686698368569, -0.000872029428758877, + -0.000410522437887563}, + {-7.15740446281205, 0.104233532313688, 1.13155893729292, -0.00350418544400852, + 0.00532058598508803, 0.00459314980222008, 3.09155436939349e-05, -7.60935741692174e-05, + -5.87922606348196e-05, 2.56146268588382e-05}, + {44.8818060495112, -7.94729992210875, 3.59286389225051, 0.217944601088562, 0.108087933176612, + -0.116711715153385, -0.00194260120960441, -0.0015752762498594, -0.000331129410732722, + 0.00125896996438418}}; + float sus4coeffAlpha[9][10] = { + {-12.4581187126738, 0.398038572289047, -0.438887880988151, -0.00965382887938283, + -0.0309322349328842, -0.00359106522420111, -7.79546112299913e-06, -0.000432733997178497, + -9.79031907635314e-05, -1.49299384451257e-05}, + {8.41054378583447, 1.87462327360707, 0.266809999719952, 0.0216455385250676, 0.00879426079919981, + -0.00142295319820553, 0.000194819780653264, 8.57549705064449e-05, -3.56478452552367e-05, + -1.65680920554434e-05}, + {16.4141780945815, 2.57697842088604, 0.373972171754278, 0.0498264199400303, 0.0183175817756131, + -0.008545409848878, 0.000422696533006382, -0.000268245978898508, -0.000663188021815416, + -7.51144017137827e-05}, + {0.796692054977593, 1.26773229735266, 0.247715261673662, 0.00358183885438128, + 0.00216435175662881, 0.00713732829335305, -0.000110129715615857, 3.56051594182427e-05, + 5.03074365340535e-05, 8.40279146176271e-05}, + {2.37491588500165, 1.05997969088519, 0.309540461340971, -0.000405047711742513, + 0.000462224730316111, -0.00201887171945793, 0.000260159805167265, -0.000282867209803598, + 0.000201613303652666, -0.000277796442847579}, + {6.36749007598708, 1.31659760017973, -0.122724934153231, -0.00328808937096891, + -0.00577347207798776, 0.00403172074457999, -7.45676459772001e-05, 1.79838644222274e-05, + 0.000104552066440564, -2.78115121929346e-05}, + {-47.9667098848496, 3.97703197139796, -1.96403894754299, -0.0577989657406978, + 0.0634225576208007, -0.0346023445055141, 0.00045886475369098, -0.000326132951996844, + 0.000716490441845967, -0.000136132038635483}, + {6.21505474256094, 0.871830486201601, 0.286906473833627, 0.007875292606045, + -0.00974634725746389, 0.00128416935792136, -0.000111796743751489, 0.000102016719989187, + -3.3503088289589e-05, -1.03874407813931e-05}, + {102.09801265482, -4.12715152309748, -5.04594403360339, 0.075499959116996, 0.216574192561683, + 0.0750031215784663, -0.000147358932612646, -0.0023710703422108, -0.00143310719642393, + -0.000431914403446768}}; + float sus4coeffBeta[9][10] = { + {-21.5077132684032, -1.60004839699939, -0.0298995033958561, -0.0315563250430659, + -0.0424403625879891, -0.0245426225510417, -0.000209861203016225, -0.000422150973104431, + -0.00030514398458781, -0.000211986731019738}, + {9.07644247897601, 0.207457289788099, 1.26735366597312, 0.00768477352180427, + 0.00429230749575816, -0.00514802326062087, 7.56149591998578e-05, 8.42794730840662e-05, + -3.62215715492783e-05, -5.24384190165239e-05}, + {-33.5225408043693, -3.11167857248829, 1.91760591695775, -0.0963752386435729, + 0.00026620241534153, -0.0256680391021823, -0.00102188712837393, 2.63753563968978e-05, + 0.000113172463974702, 0.000271939918507175}, + {19.1379025029401, -0.225979661987912, 2.72337120022998, -0.00136982412154458, + -0.00447301210555274, 0.046496718064139, 2.09123846958985e-05, -4.30383094864847e-05, + -1.22808643520768e-05, 0.000440555709696048}, + {2.957867714783, -0.316069593806939, 1.06379930645214, 0.00103244713047271, 0.00148059212230411, + 0.000557885068990542, 0.000288633931072557, 0.000172775380291659, 0.000269738457990237, + 0.000254577019084984}, + {2.04155199929521, -0.318303488378033, 1.37820715117028, 0.00114788656817743, + 0.0130051117909245, -0.00743109928493789, 1.22403390396844e-05, -3.19245785131217e-05, + -0.000156735218010879, 3.81458400945988e-05}, + {27.314954181241, -1.43916155634084, 2.48967706992348, 0.0278695408478388, -0.0341141456915131, + 0.0281959785297513, -0.000252996164135396, 0.000163365679366542, -0.000380129463154642, + 0.000159350154429114}, + {-0.274693278266294, 0.0199711721436635, 1.26676843352524, -0.0006713759238817, + -0.00389715205101059, 0.00294298337610857, -9.58643121413979e-06, 6.30700938550725e-05, + -6.07188867796123e-05, 7.72199861279611e-06}, + {-74.1601853968901, 2.55641628908672, 6.38533530714782, -0.0582345132980647, + -0.0653804553172819, -0.138850555683872, 0.000489364157827405, 0.000469559629292745, + 0.000698140692952438, 0.00123017528239406}}; + float sus5coeffAlpha[9][10] = { + {-12.1398741236355, 1.99425442858125, -1.9303044815802, 0.0418421279520049, -0.0309683799946315, + -0.0562201123081437, 0.000522607299552916, -0.000375386573815007, -0.000183899715035788, + -0.000600349486293698}, + {4.51862054729553, 1.72396080253297, 0.274562680698765, 0.0162681383591035, 0.0108410181586673, + -0.000272215427359511, 0.000124164068046579, 0.000125586897851351, -1.24082224214974e-05, + -1.63339067540159e-05}, + {63.0100748193658, 7.78014670478172, 0.327263471268564, 0.181264302704374, -0.0652454854214506, + -0.03906716801285, 0.00166924078925478, -0.000749939315526625, 0.000320696101132374, + 0.000499934751180042}, + {-2.14377722994325, 1.33617641673436, 0.0973465660282871, 0.00389526886867845, + 0.00526064997381395, 0.00244964888333519, -8.59416490903541e-05, 4.58871931007681e-05, + 8.6123353128647e-05, 2.85447259858337e-05}, + {0.164792977301912, 1.17541977248641, 0.348838798760518, -0.000180865118417534, + 0.000331789515553421, -0.000734333865631793, 9.76677859410759e-05, -0.000324347075049525, + 8.66683396011167e-05, -0.000385839566009832}, + {-0.228934187493575, 1.30552820143752, 0.306779576899158, -0.00508763741184706, + -0.00318524263093038, -0.00878095392529144, -6.59040013073836e-05, 8.69122529321691e-05, + 5.73853071731283e-05, 8.56628414466758e-05}, + {22.6047744510684, -0.591739857860868, 0.566728856847393, 0.0498124268150265, + -0.0214126910277926, 0.00538091942017912, -0.000391517685229849, 0.000554321668236216, + 0.000191004410219065, 0.000102775124022018}, + {4.54704081104052, 0.844841244606025, 0.181355971462193, 0.0109743851006749, + -0.00363467884122547, 0.00108873046814694, -0.000153236888951059, 3.14623342713789e-06, + -2.78503202185463e-05, 3.99983788680736e-06}, + {-30.878359404848, 5.20536009886854, -0.674455093700773, -0.10801865891189, -0.0514805639475938, + 0.0503660452068572, 0.00072776817295273, 0.00120288537038655, -0.000301602375634166, + -0.000477098479809266}}; + float sus5coeffBeta[9][10] = { + {16.8155737032787, 0.65475660868259, 1.95532810363711, 0.000295624718662669, 0.0426379914736747, + 0.00192544771588337, -4.94534888281508e-05, 8.32299142575155e-05, 0.000645497238623369, + -0.000234155227840799}, + {9.48268090632318, 0.528942263930744, 1.34030963800712, 0.0173605129814363, 0.00581086655972212, + -0.00365006277801141, 0.000180048140973223, 0.000102002650672644, -4.10833110241736e-05, + -8.7810396165556e-05}, + {-47.8325489165383, -4.78262055949503, 1.66912859871505, -0.143518014673292, 0.0288441527062856, + -0.00322823115861497, -0.00148509910480755, 0.000284265179004289, -0.000175299737313045, + -7.04175618676909e-05}, + {3.70510151312723, -0.272200626024415, 1.5527519845099, 0.000589727630962265, + -0.00889682554869096, 0.0109857452472628, 3.05876215574877e-05, 2.09194236165814e-05, + -8.33769024439277e-05, 6.90991113575066e-05}, + {0.820199776906695, -0.355683467192776, 1.17142130858009, -0.000160174871610729, + 4.09723480153701e-05, 0.000209103751629257, 0.000390331989170637, 6.45642836249667e-05, + 0.000318092703362044, 0.000107158633760141}, + {5.52084497768914, -0.227775345312466, 0.845897282556327, 0.00157426476122436, + 0.00657189797805861, 0.0103797665963117, 2.51479848048895e-05, -4.78371400399983e-05, + -5.20221896473413e-05, -0.000143840492906166}, + {-33.4875689683454, 0.937557276329106, -1.02741065470967, -0.0140023273976314, + 0.0401908729477037, -0.0512457211360142, 7.05537967426573e-05, -0.00027521752411122, + 0.000407657552700476, -0.000458411000693613}, + {0.931346887326171, -0.320804452025793, 1.28866325376154, 0.00912456151698805, + -0.00404367403569981, 0.00477543659981282, -9.43987917474817e-05, 4.66464249533497e-05, + -7.89362487264572e-05, -1.0951496495443e-05}, + {-38.3689359928435, 3.8540516906071, 1.26391725545116, -0.108584643500539, -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}}; + 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}}; + float sus7coeffAlpha[9][10] = { + {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, + -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, + -0.00119021101127241, -0.000460110780350978}, + {1.60822506792345, 1.12993931449931, 0.300781032865641, -0.00405149856360946, + 0.0116663280665617, -0.000746071920075153, -8.36092173253351e-05, 0.000126762041147563, + -1.57820750462019e-05, -2.13840141586661e-05}, + {-151.403952985468, -5.77049222793992, 9.71132757422642, -0.113259116970462, 0.284142453949027, + -0.198625061659164, -0.000836450164210354, 0.00174062771509636, -0.00323746390757859, + 0.00124721932086258}, + {3.47391964888809, 1.28788318973591, 0.358380140281919, 0.0033863520864927, 0.00154601909793475, + 0.0103457296050314, -9.56426572270873e-05, 5.48838958555808e-05, 2.97537427220847e-05, + 0.000104735911514185}, + {3.32650947866065, 1.16701012685798, 0.293514063672376, -0.00065850791542434, + -8.61746510464303e-05, -0.00212038990772211, 0.00010377123197, -0.000262818127593837, + 0.000103360882478383, -0.000296739688930329}, + {-0.440176043435378, 1.18923278867097, 0.519516382652818, -0.00138846714677511, + 0.00266491699926247, -0.014254675949624, -4.20279929822439e-05, -5.49260281515447e-05, + -1.00328708454487e-05, 0.000138142092498215}, + {9.54962966738358, 1.83809145920811, 1.82162819067959, -0.0116786627338505, + -0.00496037444422313, 0.0590883547819332, 7.48465315787857e-05, 0.000221693951602584, + 7.96466345174136e-06, 0.000638822537725177}, + {7.04862901290925, 0.876813777672465, 0.16368093989381, 0.00928717461441627, + -0.00276538956293246, 0.00117995419940653, -0.000141511492474493, -6.09796031786385e-06, + -2.62114930414747e-05, -2.88713611443788e-06}, + {135.349147631811, -7.21933296299596, -6.02379024934871, 0.19557354282067, 0.207680233512614, + 0.12880101618361, -0.00169832076532024, -0.00192216719797732, -0.00188763612041332, + -0.00103101801961442}}; + float sus7coeffBeta[9][10] = { + {-12.7115487367622, -1.08890790360556, 0.0579616268854079, -0.0212303293514951, + -0.0395948453851818, -0.0275564242614342, -0.000228652851842222, -0.000148106159109458, + -0.000555136649469199, -0.000198260004582737}, + {-0.988147625946871, -0.759018567468546, 1.20998292002818, -0.0241231836977845, + -0.000572110443300516, -0.00294835038249426, -0.00026533039022186, 6.82250069765274e-06, + 7.21038415209318e-06, -6.54881435118179e-05}, + {98.0979345921564, 4.27381413621355, -4.39956005193548, 0.0709109587666745, -0.172774236139236, + 0.107243391488741, 0.000421832640471043, -0.00140450884710288, 0.00158019019392239, + -0.00078512547169536}, + {4.10892685652543, -0.229301778557857, 1.33380992987117, -0.000250095848720304, + -0.00555205065514645, 0.00355052914398176, 1.62727119770752e-05, -1.26026527654764e-05, + -3.25505031810898e-05, 5.79970895921158e-06}, + {3.09432502337258, -0.300556003790433, 1.17085811008124, 0.00128679594824324, + 0.00148229981422985, 9.15267474159147e-05, 0.000300497843413856, 6.31378865575566e-05, + 0.000258447032558814, 9.79142983264352e-05}, + {8.92336134924575, -0.197306981784312, 0.659908505354084, 0.00175572239373996, + 0.006801023678097, 0.0189775987436792, 9.2187857727721e-06, -4.8706332690626e-05, + -6.887009887486e-05, -0.000266455617735054}, + {-52.0734887320227, 2.64822385560272, -1.72387600304694, -0.0383944891609251, 0.110873671161269, + -0.0475247245070445, 0.000194652401328063, -0.000697307928990137, 0.00124021816001, + -0.000194213899980878}, + {2.08203985879155, -0.127503525368396, 1.17628056094647, 0.00283288065938444, + 0.00394668214608305, 0.00314868636161131, -2.99504350569853e-05, -7.11070816314279e-05, + -6.30148122529749e-05, 2.28114298989664e-05}, + {191.321181158032, -12.2449557187473, -7.21933741885107, 0.267954293388644, 0.331529493933124, + 0.149867703984027, -0.00222279201444128, -0.00284724570619954, -0.00298774060233964, + -0.000988903783752156}}; + float sus8coeffAlpha[9][10] = { + {5.46354311880959, 1.15370126035432, 0.568432485840475, -0.00105094692478431, + -0.000472899673842554, 0.015581320536192, 2.26460844314248e-05, -0.000254397947062058, + 0.000198938007250408, 0.000102026690279006}, + {8.8976133108173, 1.89502416095352, 0.268670471819199, 0.0217013413241972, 0.00973925295182384, + -0.00116357269193765, 0.000185865842232419, 0.000103311614912702, -2.46539447920969e-05, + -2.06292928734686e-05}, + {-45.4550803910752, 1.27220123406993, 5.21483855848504, 0.0315791081623634, 0.0725172355124129, + -0.13947591535243, 0.000412577580637848, 0.000434545096994917, -0.000840043932292312, + 0.00126857487044307}, + {1.81302768546433, 1.20563501267535, 0.344815267182167, 0.00546879453241056, + -0.00115382996865884, 0.010597876132341, -7.75885604486581e-05, 8.99568815949154e-05, + -2.98129544974679e-06, 0.000108913239345604}, + {2.19111439539173, 1.06951675598148, 0.283707798607213, 0.00016478588207518, + 0.000196086067268121, -0.00214980231173703, 0.000237820475654357, -0.000256402967908595, + 0.000165966620658577, -0.000268394081675921}, + {15.0858674915897, 1.27922724811168, -1.0803137812576, -0.00184009775302466, + -0.00458792284209219, 0.0359393555418547, -6.05121024079603e-05, -1.2288384024143e-05, + 8.55484605384438e-05, -0.000379241348638065}, + {-14.9594190080906, 1.79473182195746, -1.00830704063572, 0.000890685410857856, + 0.0408932029176081, -0.0165460857151619, -0.000170544299916973, -0.000370901607010145, + 0.000324089709129097, -9.33010240878062e-05}, + {0.867614491733251, 1.38248194737027, 0.233408537422123, -0.00772942878114575, + -0.00783126068079782, -0.000413713955432221, 4.5775750146291e-05, 6.97323029940275e-05, + 1.70664456940787e-05, 6.75517901233086e-06}, + {2.34474364146174, -0.777275400251477, 2.09531381577911, 0.0170780716714389, 0.102855060371092, + -0.1203441505925, 0.000187004964420911, -0.00141720441050986, -0.000336251285258365, + 0.00145175125888695}}; + float sus8coeffBeta[9][10] = { + {28.3033101237397, 1.77504446792811, 1.70758838986317, 0.0307800697044683, 0.0598759344275936, + -0.014461432284373, 0.000128415617799076, 0.000664419128546701, 0.000312923304130995, + -0.000269026446641855}, + {7.73040563051023, 0.0267291479555493, 1.16189582308493, 0.000611047892976521, + -0.00213680506915073, -0.00517435586596902, -3.60304406049766e-06, -1.74452976404459e-05, + -3.95396925228538e-05, -7.01948519410633e-05}, + {-48.0766126130725, -3.77981206700298, 3.03482861087335, -0.0678496412519532, 0.115260678424016, + -0.0109681510065038, -0.000438011443691466, 0.00097230136258486, -0.000930875177732769, + -0.000203144239955507}, + {12.1881935626341, -0.234345089308583, 2.01134619426134, 0.000181529284001169, + -0.00642848065105061, 0.0243985799415726, 2.0224042581776e-05, 5.22503286757285e-06, + -4.75196303016323e-05, 0.000221160482364556}, + {3.49559433498742, -0.294995112674766, 1.07892379698257, 0.000861664794052587, + 0.00138978933062055, 0.000436385106465176, 0.000288095124755908, 0.000147259769247883, + 0.000256686898599516, 0.000198982412957039}, + {9.36663996178607, -0.171266136751803, 0.799869891484541, -0.000896305696610864, + 0.00477919972789653, 0.0077876110326094, 9.16475263625076e-06, 3.02461250100473e-05, + -3.63917701783264e-05, -0.000101376940843402}, + {9.93372683055145, 1.02056557854246, 3.01635426591734, -0.0477113881140277, -0.0280426434406976, + 0.0438813017696874, 0.000470431190169998, -7.55753674679743e-05, -0.000516444906323815, + 0.000253844418223843}, + {4.12868774589741, -0.305711790187688, 1.15739216407191, 0.00942395115281056, + 0.00264758462357433, 0.00227985850688479, -0.000107537164019682, -4.91396736189963e-05, + -5.3803493814502e-05, 6.80587059728718e-06}, + {64.9193383444005, -1.57724255547465, -3.82166532626293, 0.0104712238987591, 0.0898786950946473, + 0.128910456296131, -8.27123227422217e-05, -0.000143979624107479, -0.00146684876653306, + -0.00102226799570239}}; + float sus9coeffAlpha[9][10] = { + {65.8975109449121, 2.19115342242175, 6.11069527811832, -0.0219884864133703, 0.119985456538482, + 0.142746712551924, -0.000465882328687976, 0.000606525132125852, 0.00141667074621881, + 0.00109715845894006}, + {5.70337356029945, 1.86705636976809, 0.235584190291708, 0.0194937327615426, 0.00973291465247784, + -0.00155675297510773, 0.000147099297988423, 0.000115708967219349, -4.1462310493722e-05, + -9.80097031103588e-06}, + {138.221145997284, 6.07665575619595, -9.08085914250542, 0.0839801072927519, -0.143071750033303, + 0.237868300719915, 0.000626693630444932, -0.000579788170871402, 0.00181740650944343, + -0.00207086879728281}, + {-7.78295582666151, 1.37506685179192, -0.507596181420042, 0.00350118305456038, + 0.00380814310115541, -0.0174012437563343, -0.000124801268056815, 2.96314830184492e-05, + 6.3416992450033e-05, -0.000190177262510221}, + {0.13102597129751, 1.24228303845143, 0.328808873447393, 2.6858679536165e-05, + 0.000231428138164498, -0.000584089095259736, 5.5322167970451e-05, -0.000322205709821716, + 7.71348293209208e-05, -0.000393885990364776}, + {4.64571633968935, 1.2668223691397, -0.158952088650432, -0.0038344859267428, 0.0030051503726095, + 0.00455578826025588, -9.42520993914957e-05, 5.81633314412289e-05, -4.43545804544095e-05, + -4.83524454851519e-05}, + {99.2385930314563, -3.65569343617926, 5.54203926675588, 0.0975630395981933, -0.15701634159692, + 0.107834711298836, -0.000885326636237814, 0.000960753844480462, -0.00179894024848343, + 0.000583066757644971}, + {2.82671549736619, 1.11214198870501, 0.214735318432744, 0.00284415167563662, + -0.00743289575690122, 0.000382705440762292, -7.43232442872501e-05, 6.96994098083348e-05, + -4.15108111710131e-06, 1.33520085213482e-05}, + {36.9013743125415, -0.522392401546163, -1.52452843963663, 0.0261375433218879, 0.060573568610239, + 0.0182582125221054, -0.000244373383911157, -0.000271385147292484, -0.000723799969427732, + 6.76324880239196e-05}}; + float sus9coeffBeta[9][10] = { + {128.70886435409, 7.27355509732751, 7.18142203531244, 0.1536100459329, 0.199455846541636, + 0.101824964939793, 0.00116666116789421, 0.00181595584079788, 0.00159271319494017, + 0.000556768406475719}, + {-7.07933839681024, -0.979062424441878, 1.21792546815617, -0.0295740143783226, + -0.00442780611714201, -0.00329612819203176, -0.000291373125216143, -7.47259350176359e-05, + -4.87265282482212e-05, -7.87490350444332e-05}, + {41.1357193180502, 2.75138456414254, -0.0160889117718198, 0.0274001112562423, + -0.145644717742057, -0.0316076203283094, -0.000136443337244472, -0.00153945199081365, + 0.000938960439977633, 0.000599987111822885}, + {2.7980384746608, -0.234741037383589, 1.5342193016705, -0.000993791566721689, + -0.00787533639513478, 0.00927468655141365, 2.63308697896639e-05, -3.42816267184975e-05, + -8.48879419798771e-05, 3.84043821333798e-05}, + {0.427687530667804, -0.346076633694936, 1.22968527483851, -4.95098138311122e-05, + 0.000298245372198029, 0.000332756250024796, 0.00040375986210644, 5.20675972504572e-05, + 0.000327042170278218, 5.93011568264671e-05}, + {4.50337810133314, -0.279364254817202, 0.945812187846199, 0.000116182663432306, + 0.0115646046622083, 0.00908289960302886, 1.90394667311541e-05, -4.4360223646434e-06, + -0.000131398914898614, -0.000145568992865512}, + {-36.3377213654193, 2.21047221783626, 0.0609982245149821, -0.0670546774988572, + 0.016827777144747, -0.0277834084058314, 0.000778301409125556, 0.000135846745194401, + 0.00043261858797068, -0.00021172728254561}, + {-0.737678205841529, -0.217352122193475, 1.23494846329297, 0.00748173441779792, + 0.0019595873704705, 0.00567253723266176, -8.34768773292938e-05, -3.50608394184873e-05, + -0.000107500091550635, -5.1379722947632e-07}, + {-36.6150844777671, 3.24952006904945, 1.7222457840185, -0.0846362445435584, -0.0625549615377418, + 0.019178365782485, 0.000664877496455304, 0.000942971403881222, 0.000190754698755098, + -0.000372226659190439}}; + float sus10coeffAlpha[9][10] = { + {14.4562393748324, 0.669162330324919, 2.13895255446541, -0.0161997097021299, + 0.00185995785065838, 0.0621351118528379, -0.000278999272493087, 0.000238469666491965, + -0.000279407497782961, 0.000726904943739837}, + {-4.45678285887022, 0.92869611919737, 0.186752102727282, -0.00706160758952316, + 0.00532680276723634, -0.00119102617674229, -0.000105283880098953, 3.90673052334419e-05, + -3.13338277344246e-05, 5.32977236959767e-06}, + {30.4255268053197, 3.00991076401191, -1.4855621363519, 0.033934286288413, -0.0553588742704929, + 0.0299275582316466, 0.000167915322354466, -0.00050925078118232, 0.000463662961330962, + -0.000232919143454163}, + {2.45076465343337, 1.30206564388838, 0.635121046212765, 0.00517109639797675, + 0.00360579544364496, 0.0198490668911362, -9.31556816982662e-05, 6.7313653707875e-05, + 6.4669137025142e-05, 0.000209727581169138}, + {-0.784841314851562, 1.10058314980836, 0.314063830836532, 0.000583003703415889, + 0.000312635453606579, -0.000183738114552387, 0.000214096205760617, -0.000286744686021244, + 0.000159157597180407, -0.00032235099420715}, + {7.19568036510586, 1.33307479701657, -0.465585141952456, -0.0031910726544199, + -0.00546273504371797, 0.0145494754402526, -7.9863949693769e-05, 4.83681329120104e-05, + 8.85844309936609e-05, -0.000143217870916994}, + {-12.8344546267449, 1.36023633150143, -0.728527724854506, 0.019982118403416, 0.0385056413989437, + -0.00468598272326268, -0.000303957957649245, -6.37783846968216e-05, 0.000514049116643205, + 0.000112015427600697}, + {-2.58279031298065, 1.42167821629586, 0.208769467511292, -0.00640190372145885, + -0.0056405289717473, 0.000509611313918708, 2.23310562107823e-05, 3.23685469522147e-05, + -7.55982776243849e-06, 2.78417756661088e-06}, + {-29.7178996143914, 2.636972251183, 1.97316329325243, -0.03983524158327, -0.0193152048730234, + -0.0600902798379509, 0.00031786916010672, 0.000162178988605602, 0.000224550786416246, + 0.000614337977361927}}; + float sus10coeffBeta[9][10] = { + {12.4771349792459, 1.44317849705414, 0.975637226331561, 0.0430284146301043, 0.0220810531548995, + -0.0220926906772, 0.000310052324529521, 0.000658151808869523, -0.000288026365111098, + -0.000214619731807045}, + {-0.113203260140131, -0.272424061092191, 1.27704377191184, -0.00791746619331075, + 0.00278646694862191, -0.00398881099259934, -8.09569694307212e-05, 5.99617384829016e-05, + -5.4550919751855e-05, -8.6314530565085e-05}, + {-48.585664295448, -2.04899787231903, 4.48757129623549, -0.0226180460431321, 0.090326735447661, + -0.0722998813632622, -6.77623771415477e-05, 0.000562585419036509, -0.000956171370931993, + 0.000491554402311223}, + {-1.20986884955482, -0.215604107185474, 1.22123198786617, 0.000256508527822089, + -0.00625056735692847, 0.00262961582224303, 2.27433984698861e-05, 1.60471509861372e-05, + -4.85061736834262e-05, -1.8387092782907e-06}, + {-0.250205907903409, -0.315819331560782, 1.09018364376391, -0.000521787614293089, + -0.000500747760913489, 2.48184307342838e-05, 0.000313799238640988, 0.000136669146368744, + 0.000278914324565192, 0.000218512838469476}, + {-1.38512578184076, -0.240456589364121, 1.34170304231345, 0.00017499230372669, + 0.0070862275911073, -0.00460640844814105, 1.27594111036696e-05, -4.73855624902052e-06, + -5.41141037812903e-05, 8.50767021818388e-06}, + {58.9917559342216, -2.28705697628345, 5.35995190407842, 0.0214721399750612, -0.112195722921667, + 0.0890150265857542, -0.000100675657768708, 0.000493488022135339, -0.00137672908303878, + 0.000518683157694955}, + {3.18905073365834, -0.633376549706314, 1.17190259811174, 0.0188817945597344, + 0.00107470708915782, 0.00400880471375267, -0.000197312295539118, -2.46543035998379e-05, + -6.07871064300252e-05, 1.91822310311955e-05}, + {-21.6881499304099, -0.563186103920008, 3.70747028664292, 0.021112883967427, + -0.00650020689049325, -0.0778533644688476, -0.000131921888670268, -0.000402754836445439, + 0.000551249824375055, 0.00062236627391337}}; + float sus11coeffAlpha[9][10] = { + {-5.23569698615548, -1.45500092391928, 2.7643243644756, -0.0762912296128707, + -0.0201645929971608, 0.0997226845779083, -0.000741669441569556, -0.000485368004931528, + 0.000166230212359982, 0.00103455037278067}, + {-7.7405077383712, 0.892040861541276, 0.39014957203484, -0.00952030929935314, + 0.0185577462685363, 0.000500600568760257, -0.000151227821554572, 0.000245334737283439, + 1.89380065823205e-05, 1.83481122973969e-07}, + {-27.462143709831, -1.68192474287299, 0.689411302961069, -0.0146021086710062, 0.227153492753605, + 0.0815806579791421, 2.92919049429149e-05, 0.00153760357651792, -0.00247865821192621, + -0.00166333309739387}, + {-6.74664748624368, 1.43279156053015, 0.0212787292914553, 0.00764792230365732, + 0.00796410301290615, 0.0014384998868733, -8.95239151813685e-05, 9.55245417090909e-05, + 0.000127251739461239, 3.26943341606363e-05}, + {-2.20391533717674, 1.32902400478083, 0.38633027011889, 0.00104660852197061, + 0.00105228824412283, -0.00242067551428214, -6.98346290136652e-05, -0.000369075232184835, + -1.59510520000704e-05, -0.000448565104826966}, + {-5.29476778147188, 1.4549556336236, 0.561334186252557, -0.00260896342687109, + -0.00855934179001141, -0.0182515354646452, -8.79385828606048e-05, 5.98357681659175e-05, + 0.000146570207542509, 0.000201596912834554}, + {-45.7906613832612, 3.77630104475902, -1.67494598155515, -0.0545433897761635, 0.047897938410221, + -0.0355687158405231, 0.000374738707508583, -0.000448453494537518, 0.000377784972619365, + -0.000276573228333836}, + {-9.11681182090372, 2.06933872940742, 0.26131496122122, -0.0259534033367855, + -0.00777266937872862, -0.00262135395843891, 0.000223790782655445, 6.40488537928934e-05, + 7.75581514100296e-05, -9.25934285039627e-06}, + {183.243883340634, -8.02281039502717, -10.0756951652703, 0.168750521462303, 0.314006821405967, + 0.200264755034566, -0.0011895153717447, -0.00253812476819896, -0.00291324393641628, + -0.00140062522117514}}; + float sus11coeffBeta[9][10] = { + {34.4499366074013, -0.438583698052091, 4.72111001451028, -0.041810050989433, 0.0562461093661426, + 0.0856849892524893, -0.000477813051406167, -3.16404257494464e-05, 0.00102633196865105, + 0.000552974013759876}, + {7.6366298088699, 0.150314752958302, 1.31364679484924, 0.00557696667395871, 0.00163731860604376, + -0.00454759608980269, 5.83979683674572e-05, 4.45944881220665e-05, -4.27874746147066e-05, + -8.77418673597557e-05}, + {130.156215011893, 1.85759000444524, -10.986892391833, -0.00686275191260681, -0.188837138116058, + 0.346177462085361, -0.000183276946352264, -0.000702183496893294, 0.00293145272693857, + -0.00318194442670715}, + {-1.67854820161036, -0.358899332859806, 0.956690839640595, -4.93862910503803e-05, + -0.0136134783014874, -0.00848731301504507, 3.75950499927045e-05, 1.35374694383289e-06, + -0.000156596507890443, -0.000123254220377897}, + {3.67569209537767, -0.387260959713287, 1.31343215605952, -0.00206444615206506, + 0.00145334813110285, -0.00151259497696238, 0.000449492568365603, 6.95883968949488e-07, + 0.000368585523744765, -6.3420715525635e-05}, + {14.3296323024886, -0.182979476956897, 0.306817119309235, -0.00022212115978293, + 0.00463485302909649, 0.0277574953550035, 1.1422454625565e-05, 1.06053257479502e-05, + -2.05720000720608e-05, -0.000338584671430337}, + {-18.7534921817754, 1.14272710923224, 0.460498062012866, -0.00995826989278202, + 0.0658502318647112, 0.00616942819937029, -7.70857153768402e-05, -0.000641755741925561, + 0.00047849204592989, 0.000158509018296766}, + {1.26543621388607, -0.176674379740481, 1.38814920935488, 0.00545485262295305, + -0.00499775616702264, 0.0038057039142173, -6.59604252054511e-05, 6.40211116049053e-05, + -6.74778593434431e-05, -2.81973589469059e-05}, + {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, 0.167666815691513, + 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, + -0.00168429567131815}}; float filterAlpha; float sunThresh; @@ -91,101 +794,110 @@ public: double rw1orientationMatrix[3][3]; double rw2orientationMatrix[3][3]; double rw3orientationMatrix[3][3]; - double inertiaWheel; - double maxTrq; + double inertiaWheel = 0.000028198; + double maxTrq = 0.0032; //3.2 [mNm] } rwHandlingParameters; + struct RwMatrices { + 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}; + } rwMatrices; + struct SafeModeControllerParameters { - double k_rate_mekf; - double k_align_mekf; + 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 sunTargetDir[3]; //Body frame + double sunTargetDir[3] = { 1, 0, 0}; //Body frame double satRateRef[3]; //Body frame } safeModeControllerParameters; struct DetumbleCtrlParameters { - double gainD; + 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]; - double refRotRate[3]; - double quatRef[4]; - bool avoidBlindStr; - double blindAvoidStart; - double blindAvoidStop; - double blindRotRate; + double refDirection[3] = { 1, 0, 0}; + double refRotRate[3] = { 0, 0, 0}; + double quatRef[4] = { 0, 0, 0, 1}; + bool avoidBlindStr = true; + double blindAvoidStart = 1.5; + double blindAvoidStop = 2.5; + double blindRotRate = 1 * Math::PI /180; - double zeta; + double zeta = 0.3; double zetaLow; - double om; + double om = 0.3; double omLow; - double qiMin; - double omMax; - double gainNullspace; + double omMax = 1 * Math::PI / 180; + double qiMin = 0.1; + double gainNullspace = 0.01; - double desatMomentumRef[3]; - double deSatGainFactor; - bool desatOn; + double desatMomentumRef[3] = {0,0,0}; + double deSatGainFactor = 1000; + bool desatOn = true; - double omegaEarth; + double omegaEarth = 0.000072921158553; } inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; - struct RWMatrices { - double alignmentMatrix[3][4]; - double pseudoInverse[4][3]; - double without0[4][3]; - double without1[4][3]; - double without2[4][3]; - double without3[4][3]; - double nullspace[4]; - } rwMatrices; - struct StrParameters { - double exclusionAngle; + double exclusionAngle = 20 * Math::PI /180; // double strOrientationMatrix[3][3]; - double boresightAxis[3]; //in body/geometry frame + double boresightAxis[3] = { 0.7593, 0.0000,-0.6508}; //in body/geometry frame } strParameters; struct GpsParameters { } gpsParameters; struct GroundStationParameters { - double latitudeGs; // [rad] Latitude - double longitudeGs; // [rad] Longitude - double altitudeGs; // [m] Altitude - double earthRadiusEquat; // [m] - double earthRadiusPolar; // [m] - } groundStationParameters; + double latitudeGs = 48.7495 * Math::PI / 180.; // [rad] Latitude + double longitudeGs = 9.10384 * Math::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; - float omega_0; //Rektaszension des Aufsteigenden Knotens plus Argument des Perigäums - float m_0; //coefficients for mean anomaly - float dm; //coefficients for mean anomaly - float e; //angle of earth's rotation axis - float e1; - float p1; //some parameter - float p2; //some parameter + float domega = 36000.771; + float omega_0 = 282.94 * Math::PI / 180.; //RAAN plus argument of perigee + float m_0 = 357.5256; //coefficients for mean anomaly + float dm = 35999.049; //coefficients for mean anomaly + // ToDo: check correct assignment of e and e1. Both were assigned to e before + float e = 23.4392911 * Math::PI / 180.; //angle of earth's rotation axis + float e1 = 0.74508 * Math::PI / 180.; + // + float p1 = 6892. / 3600. * Math::PI / 180.; //some parameter + float p2 = 72. / 3600. * Math::PI / 180.; //some parameter } sunModelParameters; struct KalmanFilterParameters { @@ -195,9 +907,9 @@ public: double processNoiseOmega[3]; double processNoiseQuaternion[4]; - double sensorNoiseSTR; - double sensorNoiseSS; - double sensorNoiseMAG; + double sensorNoiseSTR = 0.1 * Math::PI / 180; + double sensorNoiseSS = 8 * Math::PI / 180; + double sensorNoiseMAG = 4 * Math::PI / 180; double sensorNoiseRMU[3]; double sensorNoiseArwRmu; //Angular Random Walk @@ -206,12 +918,27 @@ public: struct MagnetorquesParameter { - double mtq0orientationMatrix[3][3]; - double mtq1orientationMatrix[3][3]; - double mtq2orientationMatrix[3][3]; - double alignmentMatrixMtq[3][3]; - double inverseAlignment[3][3]; - double DipolMax; // [Am^2] + double mtq0orientationMatrix[3][3] = { + { 1, 0, 0}, + { 0, 0, 1}, + { 0,-1, 0}}; + double mtq1orientationMatrix[3][3] = { + { 1, 0, 0}, + { 0, 1, 0}, + { 0, 0, 1}}; + double mtq2orientationMatrix[3][3] = { + { 0, 0, 1}, + { 0, 1, 0}, + {-1, 0, 0}}; + double alignmentMatrixMtq[3][3] = { + { 0, 0,-1}, + {-1, 0, 0}, + { 0, 1, 0}}; + double inverseAlignment[3][3] = { + { 0,-1, 0}, + { 0, 0, 1}, + {-1, 0, 0}}; + double DipolMax = 0.2; // [Am^2] } magnetorquesParameter; From 5f17f365e3458d5d7ccb23eb22fe8fcc3ba1c668 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 27 Sep 2022 11:06:11 +0200 Subject: [PATCH 031/244] fixed local includes --- mission/controller/acs/ActuatorCmd.cpp | 6 +- mission/controller/acs/ActuatorCmd.h | 10 +- mission/controller/acs/Guidance.cpp | 10 +- mission/controller/acs/Guidance.h | 6 +- mission/controller/acs/Igrf13Model.cpp | 10 +- mission/controller/acs/Igrf13Model.h | 2 +- .../acs/MultiplicativeKalmanFilter.cpp | 12 +- .../acs/MultiplicativeKalmanFilter.h | 2 +- mission/controller/acs/Navigation.cpp | 11 +- mission/controller/acs/Navigation.h | 10 +- mission/controller/acs/OutputValues.cpp | 2 +- mission/controller/acs/SensorProcessing.cpp | 6 +- mission/controller/acs/SensorProcessing.h | 11 +- mission/controller/acs/SensorValues.cpp | 5 +- mission/controller/acs/SusConverter.cpp | 263 +++++++++--------- mission/controller/acs/SusConverter.h | 39 ++- mission/controller/acs/control/Detumble.cpp | 6 +- mission/controller/acs/control/Detumble.h | 11 +- mission/controller/acs/control/PtgCtrl.cpp | 12 +- mission/controller/acs/control/PtgCtrl.h | 10 +- mission/controller/acs/control/SafeCtrl.cpp | 11 +- mission/controller/acs/control/SafeCtrl.h | 11 +- 22 files changed, 235 insertions(+), 231 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 3b4ed71e..261277a0 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -6,14 +6,14 @@ */ -#include +#include "ActuatorCmd.h" +#include "util/MathOperations.h" +#include "util/CholeskyDecomposition.h" #include -#include #include #include #include #include -#include ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 85ef1532..820f1f00 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -9,11 +9,11 @@ #define ACTUATORCMD_H_ -#include -#include -#include -#include -#include +#include "AcsParameters.h" +#include "SensorProcessing.h" +#include "MultiplicativeKalmanFilter.h" +#include "SensorValues.h" +#include "OutputValues.h" class ActuatorCmd{ public: diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 158ee9f0..a6a91a81 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -8,12 +8,12 @@ #include "Guidance.h" #include "string.h" +#include "util/MathOperations.h" +#include "util/CholeskyDecomposition.h" #include -#include -#include -#include -#include -#include +#include +#include +#include Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 736d8b9b..bf53d767 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -9,9 +9,9 @@ #define GUIDANCE_H_ -#include -#include -#include +#include "AcsParameters.h" +#include "SensorValues.h" +#include "OutputValues.h" #include diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index 91bfad15..75d3c9a1 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -6,15 +6,15 @@ */ #include "Igrf13Model.h" +#include "util/MathOperations.h" #include #include #include //#include -#include -#include -#include -#include -#include +#include +#include +#include +#include Igrf13Model::Igrf13Model(){ diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index 3ea2cb40..d89865ca 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -16,11 +16,11 @@ #ifndef IGRF13MODEL_H_ #define IGRF13MODEL_H_ -#include #include #include #include #include +#include // Output should be transformed to [T] instead of [nT] diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index b2c8a963..5f7facd2 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -5,14 +5,14 @@ * Author: rooob */ +#include "MultiplicativeKalmanFilter.h" +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" #include #include -#include -#include -#include -#include "MultiplicativeKalmanFilter.h" -#include -#include +#include +#include +#include /*Initialisation of values for parameters in constructor*/ MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) : diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index f972ab5a..542db996 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -14,9 +14,9 @@ #ifndef MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_ +#include "config/classIds.h" #include //uint8_t #include /*purpose, timeval ?*/ -#include "acs/config/classIds.h" //#include <_timeval.h> #include "AcsParameters.h" diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 2cb70389..a2e3aa5d 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -6,12 +6,13 @@ */ #include "Navigation.h" +#include "util/MathOperations.h" +#include "util/CholeskyDecomposition.h" #include -#include -#include -#include -#include -#include +#include +#include +#include + Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){ acsParameters = *acsParameters_; diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 75bbade5..6691b8aa 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -9,11 +9,11 @@ #define NAVIGATION_H_ -#include -#include -#include -#include -#include +#include "AcsParameters.h" +#include "SensorProcessing.h" +#include "MultiplicativeKalmanFilter.h" +#include "SensorValues.h" +#include "OutputValues.h" class Navigation{ public: diff --git a/mission/controller/acs/OutputValues.cpp b/mission/controller/acs/OutputValues.cpp index 3deb4043..d035f621 100644 --- a/mission/controller/acs/OutputValues.cpp +++ b/mission/controller/acs/OutputValues.cpp @@ -4,7 +4,7 @@ * Created on: 30 Mar 2022 * Author: rooob */ -#include +#include "OutputValues.h" namespace ACS { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 7b89daf1..61f8092b 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -6,14 +6,14 @@ */ #include "SensorProcessing.h" +#include "Igrf13Model.h" +#include "util/MathOperations.h" +#include #include #include #include #include #include -#include -#include -#include using namespace Math; // Thought: Maybe separate file for transforming of sensor values diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 23abcc40..aa4dc3bd 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -5,13 +5,14 @@ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ +#include "AcsParameters.h" +#include "SensorValues.h" +#include "OutputValues.h" +#include "config/classIds.h" #include //uint8_t #include /*purpose, timeval ?*/ -#include -#include -#include -#include -#include +#include + /*Planned: * - Fusion of Sensor Measurements - diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 414c93ef..11720aeb 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -4,11 +4,10 @@ * Created on: 30 Mar 2022 * Author: rooob */ -#include - +#include "SensorValues.h" #include #include -#include +#include namespace ACS { diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 151648ed..c2d65133 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -5,13 +5,15 @@ * Author: Timon Schwarz */ +#include "SusConverter.h" #include //for atan2 #include -#include #include +#include +#include void SunSensor::setSunSensorData() { - // Creates dummy sensordata, replace with SUS devicehandler / channel readout + // ToDo: exchange dummy values with DataPool susChannelValues[0] = {3913, 3912, 3799, 4056}; susChannelValues[1] = {3913, 3912, 3799, 4056}; susChannelValues[2] = {3913, 3912, 3799, 4056}; @@ -26,71 +28,73 @@ void SunSensor::setSunSensorData() { susChannelValues[11] = {3913, 3912, 3799, 4056}; } -void SunSensor::checkSunSensorData(uint8_t Sensornumber) { - uint16_t ChannelValueSum; +void SunSensor::checkSunSensorData(uint8_t susNumber) { + uint16_t channelValueSum; // Check individual channel values for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters - if (susChannelValues[Sensornumber][k] <= ChannelValueCheckLow || - susChannelValues[Sensornumber][k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS + if (susChannelValues[susNumber][k] <= channelValueCheckLow || + susChannelValues[susNumber][k] > channelValueCheckHigh) { // Channel values out of range for 12 bit SUS // channel measurement range? - ValidityNumber[Sensornumber] = false; // false --> Data not valid + validFlag[susNumber] = returnvalue::FAILED; /*printf( "The value of channel %i from sun sensor %i is not inside the borders of valid data with " "a value of %i \n", - k, Sensornumber, ChannelValue[k]);*/ - } else if (susChannelValues[Sensornumber][k] > - susChannelValues[Sensornumber][4]) { // Channel values higher than zero current threshold GNDREF? - ValidityNumber[Sensornumber] = false; + k, susNumber, ChannelValue[k]);*/ + } else if (susChannelValues[susNumber][k] > + susChannelValues[susNumber][4]) { // Channel values higher than zero current threshold GNDREF? + validFlag[susNumber] = returnvalue::FAILED; /*printf( "The value of channel %i from sun sensor %i is higher than the zero current threshold " "GNDREF\n", - k, Sensornumber);*/ + k, susNumber);*/ }; }; // check sum of all channel values to check if sun sensor is illuminated by the sun (sum is // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon // reflection or earth albedo) - ChannelValueSum = - 4 * susChannelValues[Sensornumber][4] - (susChannelValues[Sensornumber][0] + - susChannelValues[Sensornumber][1] + susChannelValues[Sensornumber][2] + - susChannelValues[Sensornumber][3]); - if ((ChannelValueSum < ChannelValueSumHigh) && (ChannelValueSum > ChannelValueSumLow)) { - ValidityNumber[Sensornumber] = false; - //printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); + channelValueSum = + 4 * susChannelValues[susNumber][4] - (susChannelValues[susNumber][0] + + susChannelValues[susNumber][1] + susChannelValues[susNumber][2] + + susChannelValues[susNumber][3]); + if ((channelValueSum < channelValueSumHigh) && (channelValueSum > channelValueSumLow)) { + validFlag[susNumber] = returnvalue::FAILED; + //printf("Sun sensor %i is not illuminated by the sun\n", susNumber); }; } -void SunSensor::AngleCalculation(uint8_t Sensornumber) { - float xout, yout, s = 0.03; // s=[mm] - uint8_t d = 5, h = 1; // d=[mm] h=[mm] +void SunSensor::calcAngle(uint8_t susNumber) { + float xout, yout; + float s = 0.03; // s=[mm] gap between diodes + uint8_t d = 5; // d=[mm] edge length of the quadratic aperture + uint8_t h = 1; // h=[mm] distance between diodes and aperture int ch0, ch1, ch2, ch3; // Substract measurement values from GNDREF zero current threshold - ch0 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][0]; - ch1 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][1]; - ch2 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][2]; - ch3 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][3]; + ch0 = susChannelValues[susNumber][4] - susChannelValues[susNumber][0]; + ch1 = susChannelValues[susNumber][4] - susChannelValues[susNumber][1]; + ch2 = susChannelValues[susNumber][4] - susChannelValues[susNumber][2]; + ch3 = susChannelValues[susNumber][4] - susChannelValues[susNumber][3]; // Calculation of x and y xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] // Calculation of the angles - AlphaBetaRaw[Sensornumber][0] = atan2(xout, h) * (180 / M_PI); //[°] - AlphaBetaRaw[Sensornumber][1] = atan2(yout, h) * (180 / M_PI); //[°] + alphaBetaRaw[susNumber][0] = atan2(xout, h) * (180 / M_PI); //[°] + alphaBetaRaw[susNumber][1] = atan2(yout, h) * (180 / M_PI); //[°] } -void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { - switch (Sensornumber) { // search for the correct calibration coefficients for each SUS +void SunSensor::setCalibrationCoefficients(uint8_t susNumber) { + switch (susNumber) { // search for the correct calibration coefficients for each SUS case 0: for (uint8_t row = 0; row < 9; row++) { // save the correct coefficients in the right SUS class for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; } } break; @@ -98,8 +102,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 1: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; } } break; @@ -107,8 +111,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 2: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; } } break; @@ -116,8 +120,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 3: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; } } break; @@ -125,8 +129,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 4: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; } } break; @@ -134,8 +138,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 5: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; } } break; @@ -143,8 +147,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 6: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; } } break; @@ -152,8 +156,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 7: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; } } break; @@ -161,8 +165,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 8: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; } } break; @@ -170,8 +174,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 9: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; } } break; @@ -179,8 +183,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 10: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; } } break; @@ -188,20 +192,20 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 11: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; } } break; } } -void SunSensor::Calibration(uint8_t Sensornumber) { +void SunSensor::Calibration(uint8_t susNumber) { float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; uint8_t index; - alpha_m = AlphaBetaRaw[Sensornumber][0]; //[°] - beta_m = AlphaBetaRaw[Sensornumber][1]; //[°] + alpha_m = alphaBetaRaw[susNumber][0]; //[°] + beta_m = alphaBetaRaw[susNumber][1]; //[°] // while loop iterates above all calibration cells to use the different calibration functions in // each cell @@ -212,47 +216,47 @@ void SunSensor::Calibration(uint8_t Sensornumber) { while (l < 3) { l = l + 1; // if-condition to check in which cell the data point has to be - if ((alpha_m > ((CompleteCellWidth * ((k - 1) / 3)) - HalfCellWidth) && - alpha_m < ((CompleteCellWidth * (k / 3)) - HalfCellWidth)) && - (beta_m > ((CompleteCellWidth * ((l - 1) / 3)) - HalfCellWidth) && - beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) { + if ((alpha_m > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && + alpha_m < ((completeCellWidth * (k / 3)) - halfCellWidth)) && + (beta_m > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) && + beta_m < ((completeCellWidth * (l / 3)) - halfCellWidth))) { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell // -> first cell has number 0 - AlphaBetaCalibrated[Sensornumber][0] = - CoeffAlpha[Sensornumber][index][0] + CoeffAlpha[Sensornumber][index][1] * alpha_m + CoeffAlpha[Sensornumber][index][2] * beta_m + - CoeffAlpha[Sensornumber][index][3] * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][4] * alpha_m * beta_m + - CoeffAlpha[Sensornumber][index][5] * beta_m * beta_m + - CoeffAlpha[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + - CoeffAlpha[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + - CoeffAlpha[Sensornumber][index][8] * alpha_m * beta_m * beta_m + - CoeffAlpha[Sensornumber][index][9] * beta_m * beta_m * beta_m; //[°] - AlphaBetaCalibrated[Sensornumber][1] = - CoeffBeta[Sensornumber][index][0] + CoeffBeta[Sensornumber][index][1] * alpha_m + - CoeffBeta[Sensornumber][index][2] * beta_m + CoeffBeta[Sensornumber][index][3] * alpha_m * alpha_m + - CoeffBeta[Sensornumber][index][4] * alpha_m * beta_m + - CoeffBeta[Sensornumber][index][5] * beta_m * beta_m + - CoeffBeta[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + - CoeffBeta[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + - CoeffBeta[Sensornumber][index][8] * alpha_m * beta_m * beta_m + - CoeffBeta[Sensornumber][index][9] * beta_m * beta_m * beta_m; //[°] + alphaBetaCalibrated[susNumber][0] = + coeffAlpha[susNumber][index][0] + coeffAlpha[susNumber][index][1] * alpha_m + coeffAlpha[susNumber][index][2] * beta_m + + coeffAlpha[susNumber][index][3] * alpha_m * alpha_m + coeffAlpha[susNumber][index][4] * alpha_m * beta_m + + coeffAlpha[susNumber][index][5] * beta_m * beta_m + + coeffAlpha[susNumber][index][6] * alpha_m * alpha_m * alpha_m + + coeffAlpha[susNumber][index][7] * alpha_m * alpha_m * beta_m + + coeffAlpha[susNumber][index][8] * alpha_m * beta_m * beta_m + + coeffAlpha[susNumber][index][9] * beta_m * beta_m * beta_m; //[°] + alphaBetaCalibrated[susNumber][1] = + coeffBeta[susNumber][index][0] + coeffBeta[susNumber][index][1] * alpha_m + + coeffBeta[susNumber][index][2] * beta_m + coeffBeta[susNumber][index][3] * alpha_m * alpha_m + + coeffBeta[susNumber][index][4] * alpha_m * beta_m + + coeffBeta[susNumber][index][5] * beta_m * beta_m + + coeffBeta[susNumber][index][6] * alpha_m * alpha_m * alpha_m + + coeffBeta[susNumber][index][7] * alpha_m * alpha_m * beta_m + + coeffBeta[susNumber][index][8] * alpha_m * beta_m * beta_m + + coeffBeta[susNumber][index][9] * beta_m * beta_m * beta_m; //[°] } } } } -void SunSensor::CalculateSunVector(uint8_t Sensornumber) { +void SunSensor::CalculateSunVector(uint8_t susNumber) { float alpha, beta; - alpha = AlphaBetaCalibrated[Sensornumber][0]; //[°] - beta = AlphaBetaCalibrated[Sensornumber][1]; //[°] + alpha = alphaBetaCalibrated[susNumber][0]; //[°] + beta = alphaBetaCalibrated[susNumber][1]; //[°] // Calculate the normalized Sun Vector - SunVectorBodyFrame[Sensornumber][0] = + sunVectorBodyFrame[susNumber][0] = (tan(alpha * (M_PI / 180)) / (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - SunVectorBodyFrame[Sensornumber][1] = + sunVectorBodyFrame[susNumber][1] = (tan(beta * (M_PI / 180)) / (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - SunVectorBodyFrame[Sensornumber][2] = + sunVectorBodyFrame[susNumber][2] = (-1 / (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); } @@ -262,79 +266,79 @@ float* SunSensor::getSunVectorBodyFrame() { float* SunVectorBodyFrameReturn = 0; SunVectorBodyFrameReturn = new float[3]; - SunVectorBodyFrameReturn[0] = SunVectorBodyFrame[0]; - SunVectorBodyFrameReturn[1] = SunVectorBodyFrame[1]; - SunVectorBodyFrameReturn[2] = SunVectorBodyFrame[2]; + SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[0]; + SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[1]; + SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[2]; return SunVectorBodyFrameReturn; } -bool SunSensor::getValidityNumber(uint8_t Sensornumber) { - return ValidityNumber[Sensornumber]; +bool SunSensor::getValidFlag(uint8_t susNumber) { + return validFlag[susNumber]; } float* SunSensor::TransferSunVector() { - float* SunVectorEIVE = 0; - SunVectorEIVE = new float[3]; + float* sunVectorEIVE = 0; + sunVectorEIVE = new float[3]; - uint8_t counter = 0; - int8_t BasisMatrixUse[3][3]; - float SunVectorMatrixEIVE[3][12] = {0}, sum; - float SunVectorMatrixBodyFrame[3][12]; + uint8_t susAvail = 12; + int8_t basisMatrixUse[3][3]; + float sunVectorMatrixEIVE[3][12] = {0}; + float sunVectorMatrixBodyFrame[3][12]; - for (uint8_t Sensornumber = 0; Sensornumber < 12; - Sensornumber++) { // save the sun vector of each SUS in their body frame into an array for + for (uint8_t susNumber = 0; susNumber < 12; + susNumber++) { // save the sun vector of each SUS in their body frame into an array for // further processing - float* SunVectorBodyFrame = SunVectorBodyFrame[Sensornumber]; - SunVectorMatrixBodyFrame[0][Sensornumber] = SunVectorBodyFrame[0]; - SunVectorMatrixBodyFrame[1][Sensornumber] = SunVectorBodyFrame[1]; - SunVectorMatrixBodyFrame[2][Sensornumber] = SunVectorBodyFrame[2]; + float* SunVectorBodyFrame = SunVectorBodyFrame[susNumber]; + sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0]; + sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1]; + sunVectorMatrixBodyFrame[2][susNumber] = SunVectorBodyFrame[2]; } - for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { - if (getValidityNumber(Sensornumber) == false) { - counter = counter + 1; + for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { + if (getValidFlag(susNumber) == returnvalue::FAILED) { + susAvail -= 1; } // if the SUS data is not valid -> for (uint8_t c1 = 0; c1 < 3; c1++) { for (uint8_t c2 = 0; c2 < 3; c2++) { - switch (Sensornumber) { // find right basis matrix for each SUS + switch (susNumber) { case 0: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; break; case 1: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; break; case 2: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; break; case 3: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; break; case 4: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; break; case 5: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; break; case 6: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; break; case 7: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; break; case 8: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; break; case 9: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; break; case 10: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; break; case 11: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; break; } } @@ -344,34 +348,31 @@ float* SunSensor::TransferSunVector() { for (uint8_t p = 0; p < 3; p++) { for (uint8_t q = 0; q < 3; q++) { // normal matrix multiplication - SunVectorMatrixEIVE[p][Sensornumber] += - (BasisMatrixUse[p][q] * SunVectorMatrixBodyFrame[q][Sensornumber]); + sunVectorMatrixEIVE[p][susNumber] += + (basisMatrixUse[p][q] * sunVectorMatrixBodyFrame[q][susNumber]); } } } - if (counter < 12) { // Calculate one sun vector out of all sun vectors from the different SUS + if (susAvail > 0) { // Calculate one sun vector out of all sun vectors from the different SUS for (uint8_t i = 0; i < 3; i++) { - sum = 0; - for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { - if (getValidityNumber(Sensornumber)){ - sum += SunVectorMatrixEIVE[i][Sensornumber]; - //printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]); + float sum = 0; + for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { + if (getValidFlag(susNumber) == returnvalue::OK){ + sum += sunVectorMatrixEIVE[i][susNumber]; + //printf("%f\n", SunVectorMatrixEIVE[i][susNumber]); } } // ToDo: decide on length on sun vector - SunVectorEIVE[i] = - sum/* / (12 - counter)*/; // FLAG Ergebnis ist falsch, kann an einem Fehler im Programm - // liegen, vermutlich aber an den falschen ChannelValues da die - // transformierten Sonnenvektoren jedes SUS plausibel sind + sunVectorEIVE[i] = sum; } - VectorOperations::normalize(SunVectorEIVE, SunVectorEIVE, 3); + VectorOperations::normalize(sunVectorEIVE, sunVectorEIVE, 3); } else { // No sus is valid throw std::invalid_argument("No sun sensor is valid"); // throw error } - return SunVectorEIVE; + return sunVectorEIVE; } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 4012bb5a..3a5662ca 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -8,49 +8,48 @@ #ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ #define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ - +#include "AcsParameters.h" #include -#include class SunSensor { public: SunSensor() {} void setSunSensorData(); - void checkSunSensorData(uint8_t Sensornumber); - void AngleCalculation(uint8_t Sensornumber); - void setCalibrationCoefficients(uint8_t Sensornumber); - void Calibration(uint8_t Sensornumber); - void CalculateSunVector(uint8_t Sensornumber); + void checkSunSensorData(uint8_t susNumber); + void calcAngle(uint8_t susNumber); + void setCalibrationCoefficients(uint8_t susNumber); + void Calibration(uint8_t susNumber); + void CalculateSunVector(uint8_t susNumber); - bool getValidityNumber(uint8_t Sensornumber); + bool getValidFlag(uint8_t susNumber); float* getSunVectorBodyFrame(); float* TransferSunVector(); private: uint16_t susChannelValues[12][4]; //[Bit] - float AlphaBetaRaw[12][2]; //[°] - float AlphaBetaCalibrated[12][2]; //[°] - float SunVectorBodyFrame[12][3]; //[-] + float alphaBetaRaw[12][2]; //[°] + float alphaBetaCalibrated[12][2]; //[°] + float sunVectorBodyFrame[12][3]; //[-] - bool ValidityNumber[12] = true; + bool validFlag[12] = returnvalue::OK; - uint16_t ChannelValueCheckHigh = + uint16_t channelValueCheckHigh = 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check - uint8_t ChannelValueCheckLow = + uint8_t channelValueCheckLow = 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check - uint16_t ChannelValueSumHigh = + uint16_t channelValueSumHigh = 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by // the reflection of sunlight from the moon/earth - uint8_t ChannelValueSumLow = + uint8_t channelValueSumLow = 0; //[Bit]low borderline for check if the sun sensor is illuminated // by the sun or by the reflection of sunlight from the moon/earth - uint8_t CompleteCellWidth = 140, - HalfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in + uint8_t completeCellWidth = 140, + halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in // which cell a data point should be - float CoeffAlpha[12][9][10]; - float CoeffBeta[12][9][10]; + float coeffAlpha[12][9][10]; + float coeffBeta[12][9][10]; AcsParameters acsParameters; }; diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 8a15aa8a..67903c7c 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,13 +7,13 @@ */ -#include +#include "Detumble.h" +#include "../util/MathOperations.h" +#include #include #include -#include #include #include -#include #include diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 618be6c6..835c32b0 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -8,14 +8,15 @@ #ifndef ACS_CONTROL_DETUMBLE_H_ #define ACS_CONTROL_DETUMBLE_H_ +#include "../SensorValues.h" +#include "../OutputValues.h" +#include "../AcsParameters.h" +#include "../config/classIds.h" #include #include -#include -#include -#include -#include -#include #include +#include + class Detumble{ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index eaffd909..999df912 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -8,12 +8,12 @@ #include "PtgCtrl.h" -#include -#include -#include -#include -#include -#include +#include "../util/MathOperations.h" +#include +#include +#include +#include +#include #include PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fe5d0e96..87a66f2d 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -14,12 +14,12 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ +#include "../SensorValues.h" +#include "../OutputValues.h" +#include "../AcsParameters.h" +#include "../config/classIds.h" #include #include -#include -#include -#include -#include #include class PtgCtrl{ @@ -53,7 +53,7 @@ private: AcsParameters::PointingModeControllerParameters* pointingModeControllerParameters; AcsParameters::RwHandlingParameters* rwHandlingParameters; AcsParameters::InertiaEIVE* inertiaEIVE; - AcsParameters::RWMatrices* rwMatrices; + AcsParameters::RwMatrices* rwMatrices; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 55772ae4..ff619f35 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -6,12 +6,13 @@ */ #include "SafeCtrl.h" -#include -#include -#include -#include -#include +#include "../util/MathOperations.h" #include +#include +#include +#include +#include + SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){ loadAcsParameters(acsParameters_); diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 36807bcc..c6d47324 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -8,14 +8,15 @@ #ifndef SAFECTRL_H_ #define SAFECTRL_H_ +#include "../SensorValues.h" +#include "../OutputValues.h" +#include "../AcsParameters.h" +#include "../config/classIds.h" #include #include -#include -#include -#include -#include "acs/config/classIds.h" -#include #include +#include + class SafeCtrl{ From 4a1cce19c4bb5dc3e53f052c9cde1f288aa691b7 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 27 Sep 2022 11:57:15 +0200 Subject: [PATCH 032/244] fixed minor bugs --- mission/controller/acs/AcsParameters.h | 26 ++++++++++----------- mission/controller/acs/OutputValues.h | 2 +- mission/controller/acs/SensorProcessing.h | 2 +- mission/controller/acs/SusConverter.cpp | 25 ++++---------------- mission/controller/acs/SusConverter.h | 26 +++++++++++++++++---- mission/controller/acs/config/classIds.h | 6 +++-- mission/controller/acs/control/Detumble.h | 2 +- mission/controller/acs/control/SafeCtrl.cpp | 2 +- 8 files changed, 48 insertions(+), 43 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index d0d67691..cc1fd7b1 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -845,13 +845,13 @@ public: bool avoidBlindStr = true; double blindAvoidStart = 1.5; double blindAvoidStop = 2.5; - double blindRotRate = 1 * Math::PI /180; + double blindRotRate = 1 * M_PI /180; double zeta = 0.3; double zetaLow; double om = 0.3; double omLow; - double omMax = 1 * Math::PI / 180; + double omMax = 1 * M_PI / 180; double qiMin = 0.1; double gainNullspace = 0.01; @@ -867,7 +867,7 @@ public: struct StrParameters { - double exclusionAngle = 20 * Math::PI /180; + double exclusionAngle = 20 * M_PI /180; // double strOrientationMatrix[3][3]; double boresightAxis[3] = { 0.7593, 0.0000,-0.6508}; //in body/geometry frame } strParameters; @@ -876,8 +876,8 @@ public: } gpsParameters; struct GroundStationParameters { - double latitudeGs = 48.7495 * Math::PI / 180.; // [rad] Latitude - double longitudeGs = 9.10384 * Math::PI / 180.; // [rad] Longitude + 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] @@ -889,15 +889,15 @@ public: }; uint8_t useSunModel; float domega = 36000.771; - float omega_0 = 282.94 * Math::PI / 180.; //RAAN plus argument of perigee + float omega_0 = 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 // ToDo: check correct assignment of e and e1. Both were assigned to e before - float e = 23.4392911 * Math::PI / 180.; //angle of earth's rotation axis - float e1 = 0.74508 * Math::PI / 180.; + float e = 23.4392911 * M_PI / 180.; //angle of earth's rotation axis + float e1 = 0.74508 * M_PI / 180.; // - float p1 = 6892. / 3600. * Math::PI / 180.; //some parameter - float p2 = 72. / 3600. * Math::PI / 180.; //some parameter + float p1 = 6892. / 3600. * M_PI / 180.; //some parameter + float p2 = 72. / 3600. * M_PI / 180.; //some parameter } sunModelParameters; struct KalmanFilterParameters { @@ -907,9 +907,9 @@ public: double processNoiseOmega[3]; double processNoiseQuaternion[4]; - double sensorNoiseSTR = 0.1 * Math::PI / 180; - double sensorNoiseSS = 8 * Math::PI / 180; - double sensorNoiseMAG = 4 * Math::PI / 180; + double sensorNoiseSTR = 0.1 * M_PI / 180; + double sensorNoiseSS = 8 * M_PI / 180; + double sensorNoiseMAG = 4 * M_PI / 180; double sensorNoiseRMU[3]; double sensorNoiseArwRmu; //Angular Random Walk diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h index a73dc43c..c9fc71f1 100644 --- a/mission/controller/acs/OutputValues.h +++ b/mission/controller/acs/OutputValues.h @@ -48,4 +48,4 @@ public: } -#endif OUTPUTVALUES_H_ +#endif /*OUTPUTVALUES_H_*/ diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index aa4dc3bd..097c88ed 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -86,5 +86,5 @@ protected: }; -#endif SENSORPROCESSING_H_ +#endif /*SENSORPROCESSING_H_*/ diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index c2d65133..13f23808 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -12,21 +12,6 @@ #include #include -void SunSensor::setSunSensorData() { - // ToDo: exchange dummy values with DataPool - susChannelValues[0] = {3913, 3912, 3799, 4056}; - susChannelValues[1] = {3913, 3912, 3799, 4056}; - susChannelValues[2] = {3913, 3912, 3799, 4056}; - susChannelValues[3] = {3913, 3912, 3799, 4056}; - susChannelValues[4] = {3913, 3912, 3799, 4056}; - susChannelValues[5] = {3913, 3912, 3799, 4056}; - susChannelValues[6] = {3913, 3912, 3799, 4056}; - susChannelValues[7] = {3913, 3912, 3799, 4056}; - susChannelValues[8] = {3913, 3912, 3799, 4056}; - susChannelValues[9] = {3913, 3912, 3799, 4056}; - susChannelValues[10] = {3913, 3912, 3799, 4056}; - susChannelValues[11] = {3913, 3912, 3799, 4056}; -} void SunSensor::checkSunSensorData(uint8_t susNumber) { uint16_t channelValueSum; @@ -261,14 +246,14 @@ void SunSensor::CalculateSunVector(uint8_t susNumber) { (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); } -float* SunSensor::getSunVectorBodyFrame() { +float* SunSensor::getSunVectorBodyFrame(uint8_t susNumber) { // return function for the sun vector in the body frame float* SunVectorBodyFrameReturn = 0; SunVectorBodyFrameReturn = new float[3]; - SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[0]; - SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[1]; - SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[2]; + SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[susNumber][0]; + SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[susNumber][1]; + SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[susNumber][2]; return SunVectorBodyFrameReturn; } @@ -289,7 +274,7 @@ float* SunSensor::TransferSunVector() { for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { // save the sun vector of each SUS in their body frame into an array for // further processing - float* SunVectorBodyFrame = SunVectorBodyFrame[susNumber]; + float* SunVectorBodyFrame = &SunVectorBodyFrame[susNumber]; sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0]; sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1]; sunVectorMatrixBodyFrame[2][susNumber] = SunVectorBodyFrame[2]; diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 3a5662ca..bfec1dfe 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -15,7 +15,6 @@ class SunSensor { public: SunSensor() {} - void setSunSensorData(); void checkSunSensorData(uint8_t susNumber); void calcAngle(uint8_t susNumber); void setCalibrationCoefficients(uint8_t susNumber); @@ -23,16 +22,35 @@ class SunSensor { void CalculateSunVector(uint8_t susNumber); bool getValidFlag(uint8_t susNumber); - float* getSunVectorBodyFrame(); + float* getSunVectorBodyFrame(uint8_t susNumber); float* TransferSunVector(); private: - uint16_t susChannelValues[12][4]; //[Bit] + // ToDo: remove statics and replace with actual data + uint16_t susChannelValues[12][4] = { + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}, + {3913, 3912, 3799, 4056}}; //[Bit] float alphaBetaRaw[12][2]; //[°] float alphaBetaCalibrated[12][2]; //[°] float sunVectorBodyFrame[12][3]; //[-] - bool validFlag[12] = returnvalue::OK; + bool validFlag[12] = {returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK,returnvalue::OK, + returnvalue::OK}; uint16_t channelValueCheckHigh = 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h index 0c03b408..7db1ea72 100644 --- a/mission/controller/acs/config/classIds.h +++ b/mission/controller/acs/config/classIds.h @@ -7,11 +7,13 @@ #ifndef ACS_CONFIG_CLASSIDS_H_ #define ACS_CONFIG_CLASSIDS_H_ -#include "bsp_hosted/fsfwconfig/returnvalues/classIds.h" + +#include +#include namespace CLASS_ID { enum eiveclassIds: uint8_t { - EIVE_CLASS_ID_START = CLASS_ID_END, + EIVE_CLASS_ID_START = COMMON_CLASS_ID_END, KALMAN, SAFE, PTG, diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 835c32b0..b85aaf86 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -43,5 +43,5 @@ private: AcsParameters::MagnetorquesParameter* magnetorquesParameter; }; -#endif ACS_CONTROL_DETUMBLE_H_ +#endif /*ACS_CONTROL_DETUMBLE_H_*/ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index ff619f35..32029ad7 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -85,7 +85,7 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid, VectorOperations::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3); *outputValid = true; - return RETURN_OK; + return returnvalue::OK; } From eb08d8849b631e9e5b08f496ddd571902da0a7b0 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 28 Sep 2022 15:27:51 +0200 Subject: [PATCH 033/244] added SUS datapool to AcsController --- mission/controller/AcsController.cpp | 83 ++++++++++++++++++- mission/controller/AcsController.h | 18 ++++ .../AcsCtrlDefinitions.h | 17 +++- 3 files changed, 115 insertions(+), 3 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 09e91e26..bfce1d48 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -3,7 +3,7 @@ #include AcsController::AcsController(object_id_t objectId) - : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this) {} + : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this), susData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return returnvalue::OK; @@ -35,6 +35,12 @@ void AcsController::performControlOperation() { copyMgmData(); } } + { + PoolReadGuard pg(&susData); + if (pg.getReadResult() == returnvalue::OK) { + copySusData(); + } + } } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, @@ -94,3 +100,78 @@ void AcsController::copyMgmData() { } } } + +void AcsController::copySusData() { + { + PoolReadGuard pg(&susSets[0]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus0.value, susSets[0].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[1]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus1.value, susSets[1].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[2]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus2.value, susSets[2].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[3]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus3.value, susSets[3].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[4]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus4.value, susSets[4].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[5]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus5.value, susSets[5].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[6]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus6.value, susSets[6].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[7]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus7.value, susSets[7].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[8]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus8.value, susSets[8].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[9]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[10]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus10.value, susSets[10].channels.value, 6 * sizeof(uint16_t)); + } + } + { + PoolReadGuard pg(&susSets[11]); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susData.sus11.value, susSets[11].channels.value, 6 * sizeof(uint16_t)); + } + } +} diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 14b710a2..210b1c04 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -55,6 +55,9 @@ class AcsController : public ExtendedControllerBase { void copyMgmData(); // Sun Sensors + + acsctrl::SusDataRaw susData; + std::array susSets{ SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), @@ -70,6 +73,21 @@ class AcsController : public ExtendedControllerBase { SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), }; + PoolEntry sus0PoolVec = PoolEntry(6); + PoolEntry sus1PoolVec = PoolEntry(6); + PoolEntry sus2PoolVec = PoolEntry(6); + PoolEntry sus3PoolVec = PoolEntry(6); + PoolEntry sus4PoolVec = PoolEntry(6); + PoolEntry sus5PoolVec = PoolEntry(6); + PoolEntry sus6PoolVec = PoolEntry(6); + PoolEntry sus7PoolVec = PoolEntry(6); + PoolEntry sus8PoolVec = PoolEntry(6); + PoolEntry sus9PoolVec = PoolEntry(6); + PoolEntry sus10PoolVec = PoolEntry(6); + PoolEntry sus11PoolVec = PoolEntry(6); + + void copySusData(); + // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 9a2b1abd..1b38e033 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -60,9 +60,22 @@ class MgmDataRaw : public StaticLocalDataSet { private: }; -class SusData : public StaticLocalDataSet { +class SusDataRaw : public StaticLocalDataSet { public: - SusData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {} + SusDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {} + + lp_vec_t sus0 = lp_vec_t(sid.objectId, SUS_0_N_LOC_XFYFZM_PT_XF, this); + lp_vec_t sus1 = lp_vec_t(sid.objectId, SUS_1_N_LOC_XBYFZM_PT_XB, this); + lp_vec_t sus2 = lp_vec_t(sid.objectId, SUS_2_N_LOC_XFYBZB_PT_YB, this); + lp_vec_t sus3 = lp_vec_t(sid.objectId, SUS_3_N_LOC_XFYBZF_PT_YF, this); + lp_vec_t sus4 = lp_vec_t(sid.objectId, SUS_4_N_LOC_XMYFZF_PT_ZF, this); + lp_vec_t sus5 = lp_vec_t(sid.objectId, SUS_5_N_LOC_XFYMZB_PT_ZB, this); + lp_vec_t sus6 = lp_vec_t(sid.objectId, SUS_6_R_LOC_XFYBZM_PT_XF, this); + lp_vec_t sus7 = lp_vec_t(sid.objectId, SUS_7_R_LOC_XBYBZM_PT_XB, this); + lp_vec_t sus8 = lp_vec_t(sid.objectId, SUS_8_R_LOC_XBYBZB_PT_YB, this); + lp_vec_t sus9 = lp_vec_t(sid.objectId, SUS_9_R_LOC_XBYBZB_PT_YF, this); + lp_vec_t sus10 = lp_vec_t(sid.objectId, SUS_10_N_LOC_XMYBZF_PT_ZF, this); + lp_vec_t sus11 = lp_vec_t(sid.objectId, SUS_11_R_LOC_XBYMZB_PT_ZB, this); private: }; From 16429009b4013ba67c3f6109eec47191445eb36f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 30 Sep 2022 10:13:27 +0200 Subject: [PATCH 034/244] added LocalDataPool entries --- dummies/ImtqDummy.cpp | 2 ++ dummies/MgmLIS3MDLDummy.cpp | 1 + dummies/MgmRm3100Dummy.cpp | 3 ++- mission/controller/AcsController.cpp | 8 ++++++-- 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 6dc451b3..63d05c1e 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -39,5 +39,7 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0,0.0,0.0})); + localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.cpp b/dummies/MgmLIS3MDLDummy.cpp index 7aa5fdf7..9c521559 100644 --- a/dummies/MgmLIS3MDLDummy.cpp +++ b/dummies/MgmLIS3MDLDummy.cpp @@ -41,5 +41,6 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry({0.0,0.0,0.0})); return returnvalue::OK; } diff --git a/dummies/MgmRm3100Dummy.cpp b/dummies/MgmRm3100Dummy.cpp index ad97e7be..cb27209c 100644 --- a/dummies/MgmRm3100Dummy.cpp +++ b/dummies/MgmRm3100Dummy.cpp @@ -36,5 +36,6 @@ uint32_t MgmRm3100Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmRm3100Dummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - return OK; + localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, new PoolEntry({0.0, 0.0, 0.0})); + return returnvalue::OK; } diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index bfce1d48..896af482 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -2,6 +2,8 @@ #include +#include + AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this), susData(this) {} @@ -29,7 +31,7 @@ void AcsController::performControlOperation() { break; } - { + /*{ PoolReadGuard pg(&mgmData); if (pg.getReadResult() == returnvalue::OK) { copyMgmData(); @@ -40,7 +42,7 @@ void AcsController::performControlOperation() { if (pg.getReadResult() == returnvalue::OK) { copySusData(); } - } + }*/ } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, @@ -58,6 +60,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { if (sid == mgmData.getSid()) { return &mgmData; + } else if (sid == susData.getSid()) { + return &susData; } return nullptr; } From abe5f43ae95d7eadf9e958f9e7ff4b3bf7682a18 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 30 Sep 2022 11:06:17 +0200 Subject: [PATCH 035/244] added performDetumble and implemented ckeckModeCommand --- mission/controller/AcsController.cpp | 61 ++++++++++++++++++++++++++-- mission/controller/AcsController.h | 21 ++++++++++ 2 files changed, 78 insertions(+), 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 896af482..26f6a1ee 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,11 +1,14 @@ #include "AcsController.h" #include - -#include +#include AcsController::AcsController(object_id_t objectId) - : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this), susData(this) {} + : ExtendedControllerBase(objectId, objects::NO_OBJECT), + mgmData(this), + susData(this), + sensorProcessing(&acsParameters), + detumbleCounter{0} {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return returnvalue::OK; @@ -45,6 +48,43 @@ void AcsController::performControlOperation() { }*/ } +void AcsController::performDetumble() { + ACS::SensorValues sensorValues; + ACS::OutputValues outputValues; + + // sensorValues.read(); + // outputValues.read(); + + timeval now; // = {0,0}; + Clock::getClock_timeval(&now); + + sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &outputValues, &validMekf); + double magMomMtq[3] = {0, 0, 0}; + detumble.bDotLaw(outputValues.magneticFieldVectorDerivative, + &outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst, + &outputValues.magFieldEstValid, magMomMtq); + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + + if (outputValues.satRateMekfValid && VectorOperations::norm(outputValues.satRateMekf, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } + + else if (outputValues.satRateEstValid && + VectorOperations::norm(outputValues.satRateEst, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { + detumbleCounter++; + } + + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_SAFE; + detumbleCounter = 0; + } +} + ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec); @@ -68,7 +108,20 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) { - return returnvalue::OK; + 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 > 5) || (submode < 2)) { + return INVALID_SUBMODE; + } else { + return returnvalue::OK; + } + } + return INVALID_MODE; } void AcsController::copyMgmData() { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 210b1c04..21c77683 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -3,7 +3,12 @@ #include #include +#include +#include "acs/ActuatorCmd.h" +#include "acs/Navigation.h" +#include "acs/SensorProcessing.h" +#include "acs/control/Detumble.h" #include "controllerdefinitions/AcsCtrlDefinitions.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" @@ -16,7 +21,23 @@ class AcsController : public ExtendedControllerBase { AcsController(object_id_t objectId); + 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_NADIR = 5; + + protected: + void performDetumble(); + private: + AcsParameters acsParameters; + SensorProcessing sensorProcessing; + Navigation navigation; + ActuatorCmd actuatorCmd; + + Detumble detumble; + uint8_t detumbleCounter; + enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; From 9d0e10eb1ed11b6f998a03dd22696af0a6e2c084 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 4 Oct 2022 13:45:13 +0200 Subject: [PATCH 036/244] fixed dummies, scheduled dummies as part of acsTask --- bsp_q7s/core/InitMission.cpp | 19 ++++++++ dummies/ImtqDummy.cpp | 2 + dummies/MgmLIS3MDLDummy.cpp | 2 +- dummies/SusDummy.cpp | 3 +- mission/controller/AcsController.cpp | 61 ++++++++++++++++++++---- mission/controller/AcsController.h | 9 +++- mission/controller/acs/AcsParameters.cpp | 15 +++--- mission/controller/acs/AcsParameters.h | 2 +- 8 files changed, 92 insertions(+), 21 deletions(-) diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index e60c2caf..5bac228b 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -139,6 +139,25 @@ void initmission::initTasks() { initmission::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); } #endif +#if OBSW_Q7S_EM == 1 + acsTask->addComponent(objects::MGM_0_LIS3_HANDLER); + acsTask->addComponent(objects::MGM_1_RM3100_HANDLER); + acsTask->addComponent(objects::MGM_2_LIS3_HANDLER); + acsTask->addComponent(objects::MGM_3_RM3100_HANDLER); + acsTask->addComponent(objects::IMTQ_HANDLER); + acsTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF); + acsTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF); + acsTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB); + acsTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB); + acsTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB); + acsTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB); + acsTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF); + acsTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF); + acsTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF); + acsTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); + acsTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); + acsTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); +#endif PeriodicTaskIF* sysTask = factory->createPeriodicTask( "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 63d05c1e..8765e978 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -41,5 +41,7 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0,0.0,0.0})); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry({0.0,0.0,0.0})); + localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.cpp b/dummies/MgmLIS3MDLDummy.cpp index 9c521559..b89d74b8 100644 --- a/dummies/MgmLIS3MDLDummy.cpp +++ b/dummies/MgmLIS3MDLDummy.cpp @@ -41,6 +41,6 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); - localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry({0.0,0.0,0.0})); + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry({0.0, 0.0, 0.0})); return returnvalue::OK; } diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 2a2a1bda..10368a9a 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -54,7 +54,8 @@ void SusDummy::performControlOperation() { ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); - localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, new PoolEntry({0})); + localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, + new PoolEntry({0, 0, 0, 0, 0, 0})); return returnvalue::OK; } diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 26f6a1ee..8c92d968 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,14 +1,17 @@ #include "AcsController.h" #include -#include +//#include AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId, objects::NO_OBJECT), + /*sensorProcessing(&acsParameters), + navigation(&acsParameters), + actuatorCmd(&acsParameters), + detumble(&acsParameters), + detumbleCounter{0},*/ mgmData(this), - susData(this), - sensorProcessing(&acsParameters), - detumbleCounter{0} {} + susData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return returnvalue::OK; @@ -28,13 +31,28 @@ void AcsController::performControlOperation() { return; } case InternalState::READY: { + if (mode != MODE_OFF) { + switch (submode) { + case SUBMODE_SAFE: + // performSafe(); + break; + + case SUBMODE_DETUMBLE: + // performDetumble(); + break; + + case SUBMODE_PTG_GS: + // performPointingCtrl(); + break; + } + } break; } default: break; } - /*{ + { PoolReadGuard pg(&mgmData); if (pg.getReadResult() == returnvalue::OK) { copyMgmData(); @@ -45,11 +63,15 @@ void AcsController::performControlOperation() { if (pg.getReadResult() == returnvalue::OK) { copySusData(); } - }*/ + } +} + +void AcsController::performSafe(){ + } void AcsController::performDetumble() { - ACS::SensorValues sensorValues; + /*ACS::SensorValues sensorValues; ACS::OutputValues outputValues; // sensorValues.read(); @@ -82,17 +104,36 @@ void AcsController::performDetumble() { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { submode = SUBMODE_SAFE; detumbleCounter = 0; - } + }*/ +} + +void AcsController::performPointingCtrl(){ + } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + // MGM localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); + // SUS + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec); + localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec); + poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); return returnvalue::OK; } @@ -124,6 +165,10 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_MODE; } +void AcsController::modeChanged(Mode_t mode, Submode_t submode) {} + +void AcsController::announceMode(bool recursive) {} + void AcsController::copyMgmData() { { PoolReadGuard pg(&mgm0Lis3Set); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 21c77683..054f2b8f 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -27,16 +27,19 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_PTG_NADIR = 5; protected: + + void performSafe(); void performDetumble(); + void performPointingCtrl(); private: - AcsParameters acsParameters; + /*AcsParameters acsParameters; SensorProcessing sensorProcessing; Navigation navigation; ActuatorCmd actuatorCmd; Detumble detumble; - uint8_t detumbleCounter; + uint8_t detumbleCounter;*/ enum class InternalState { STARTUP, INITIAL_DELAY, READY }; @@ -52,6 +55,8 @@ 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); // MGMs acsctrl::MgmDataRaw mgmData; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index c22e8ede..0b516c13 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -1,18 +1,17 @@ -#include #include "AcsParameters.h" +#include #include + #include AcsParameters::AcsParameters() {} AcsParameters::~AcsParameters() {} - /* -ReturnValue_t AcsParameters::getParameter(uint8_t domainId, - uint16_t parameterId, ParameterWrapper* parameterWrapper, - const ParameterWrapper *newValues, uint16_t startAtIndex) { - - return 0; - +ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, + uint16_t startAtIndex) { + return returnvalue::OK; }*/ diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index cc1fd7b1..39f2c3c6 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -11,7 +11,7 @@ typedef unsigned char uint8_t; -class AcsParameters/*:public HasParametersIF*/{ +class AcsParameters /*: public HasParametersIF*/{ public: AcsParameters(); From 1844df2195a32373cc305d4f6c015c1d336b13fd Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 6 Oct 2022 15:33:01 +0200 Subject: [PATCH 037/244] SusDummy output matched to student test case --- dummies/SusDummy.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 10368a9a..ba267d61 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -48,6 +48,10 @@ void SusDummy::performControlOperation() { } else { susSet.setValidity(true, true); } + susSet.channels[0] = 3913; + susSet.channels[1] = 3912; + susSet.channels[2] = 3799; + susSet.channels[3] = 4056; susSet.commit(); } From 84e960a9efc55789773db4f2a43d40d749507562 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 6 Oct 2022 15:37:41 +0200 Subject: [PATCH 038/244] enabled performSafe --- mission/controller/AcsController.cpp | 30 ++++++++++++++-------------- mission/controller/AcsController.h | 4 ++-- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8c92d968..2b0689c7 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -5,11 +5,11 @@ AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId, objects::NO_OBJECT), - /*sensorProcessing(&acsParameters), - navigation(&acsParameters), - actuatorCmd(&acsParameters), - detumble(&acsParameters), - detumbleCounter{0},*/ + sensorProcessing(&acsParameters), + navigation(&acsParameters), + actuatorCmd(&acsParameters), + detumble(&acsParameters), + detumbleCounter{0}, mgmData(this), susData(this) {} @@ -64,14 +64,16 @@ void AcsController::performControlOperation() { copySusData(); } } + sif::debug << susData.sus0.value[0] << "," << susData.sus0.value[1] << "," + << susData.sus0.value[2] << "," << susData.sus0.value[3] << "," + << susData.sus0.value[4] << "," << susData.sus0.value[5] << std::endl; + sif::debug << susData.sus0.isValid() << std::endl; } -void AcsController::performSafe(){ - -} +void AcsController::performSafe() {} void AcsController::performDetumble() { - /*ACS::SensorValues sensorValues; + ACS::SensorValues sensorValues; ACS::OutputValues outputValues; // sensorValues.read(); @@ -104,12 +106,10 @@ void AcsController::performDetumble() { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { submode = SUBMODE_SAFE; detumbleCounter = 0; - }*/ + } } -void AcsController::performPointingCtrl(){ - -} +void AcsController::performPointingCtrl() {} ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { @@ -120,6 +120,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus); + poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); // SUS localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec); @@ -133,8 +134,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec); localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec); - - poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0}); + poolManager.subscribeForRegularPeriodicPacket({susData.getSid(), false, 5.0}); return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 054f2b8f..fa4ba88e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -33,13 +33,13 @@ class AcsController : public ExtendedControllerBase { void performPointingCtrl(); private: - /*AcsParameters acsParameters; + AcsParameters acsParameters; SensorProcessing sensorProcessing; Navigation navigation; ActuatorCmd actuatorCmd; Detumble detumble; - uint8_t detumbleCounter;*/ + uint8_t detumbleCounter; enum class InternalState { STARTUP, INITIAL_DELAY, READY }; From 3079dabc20b609a442ea685d803ca6e48a69ed2a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 6 Oct 2022 15:38:23 +0200 Subject: [PATCH 039/244] combined SensorProcessing and SusConverter --- mission/controller/acs/SensorProcessing.cpp | 737 +++++++++++--------- mission/controller/acs/SensorProcessing.h | 104 ++- mission/controller/acs/SusConverter.cpp | 340 +++------ mission/controller/acs/SusConverter.h | 69 +- 4 files changed, 582 insertions(+), 668 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 61f8092b..933800e2 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -6,382 +6,441 @@ */ #include "SensorProcessing.h" -#include "Igrf13Model.h" -#include "util/MathOperations.h" -#include + #include #include #include #include #include +#include + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "Igrf13Model.h" +#include "util/MathOperations.h" using namespace Math; // Thought: Maybe separate file for transforming of sensor values // into geometry frame and body frame -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : - savedMagFieldEst { 0, 0, 0 }{ - validMagField = false; - validGcLatitude = false; +SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} { + validMagField = false; + validGcLatitude = false; } -SensorProcessing::~SensorProcessing() { +SensorProcessing::~SensorProcessing() {} + +bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, + bool mgm1valid, const float *mgm2Value, bool mgm2valid, + const float *mgm3Value, bool mgm3valid, const float *mgm4Value, + bool mgm4valid, timeval timeOfMgmMeasurement, + const AcsParameters::MgmHandlingParameters *mgmParameters, + const double gpsLatitude, const double gpsLongitude, + const double gpsAltitude, bool gpsValid, double *magFieldEst, + bool *outputValid, double *magFieldModel, + bool *magFieldModelValid, double *magneticFieldVectorDerivative, + bool *magneticFieldVectorDerivativeValid) { + if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { + *outputValid = false; + validMagField = false; + return false; + } + // Transforming Values to the Body Frame (actually it is the geometry frame atm) + float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0}, + mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0}; + + bool validUnit[5] = {false, false, false, false, false}; + uint8_t validCount = 0; + if (mgm0valid) { + MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, + mgm0ValueBody, 3, 3, 1); + validCount += 1; + validUnit[0] = true; + } + if (mgm1valid) { + MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, + mgm1ValueBody, 3, 3, 1); + validCount += 1; + validUnit[1] = true; + } + if (mgm2valid) { + MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, + mgm2ValueBody, 3, 3, 1); + validCount += 1; + validUnit[2] = true; + } + if (mgm3valid) { + MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, + mgm3ValueBody, 3, 3, 1); + validCount += 1; + validUnit[3] = true; + } + if (mgm4valid) { + MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value, + mgm4ValueBody, 3, 3, 1); + validCount += 1; + validUnit[4] = true; + } + + /* -------- MagFieldEst: Middle Value ------- */ + float mgmValues[3][5] = { + {mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], mgm3ValueBody[0], mgm4ValueBody[0]}, + {mgm0ValueBody[1], mgm1ValueBody[1], mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1]}, + {mgm0ValueBody[2], mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2]}}; + double mgmValidValues[3][validCount]; + uint8_t j = 0; + for (uint8_t i = 0; i < validCount; i++) { + if (validUnit[i]) { + mgmValidValues[0][j] = mgmValues[0][i]; + mgmValidValues[1][j] = mgmValues[1][i]; + mgmValidValues[2][j] = mgmValues[2][i]; + j += 1; + } + } + // Selection Sort + double mgmValidValuesSort[3][validCount]; + MathOperations::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount); + + uint8_t n = ceil(validCount / 2); + magFieldEst[0] = mgmValidValuesSort[0][n]; + magFieldEst[1] = mgmValidValuesSort[1][n]; + magFieldEst[2] = mgmValidValuesSort[2][n]; + validMagField = true; + + //-----------------------Mag Rate Computation --------------------------------------------------- + double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); + for (uint8_t i = 0; i < 3; i++) { + magneticFieldVectorDerivative[i] = (magFieldEst[i] - savedMagFieldEst[i]) / timeDiff; + savedMagFieldEst[i] = magFieldEst[i]; + } + + *magneticFieldVectorDerivativeValid = true; + if (timeOfSavedMagFieldEst.tv_sec == 0) { + magneticFieldVectorDerivative[0] = 0; + magneticFieldVectorDerivative[1] = 0; + magneticFieldVectorDerivative[2] = 0; + *magneticFieldVectorDerivativeValid = false; + } + + timeOfSavedMagFieldEst = timeOfMgmMeasurement; + + *outputValid = true; + + // ---------------- IGRF- 13 Implementation here ------------------------------------------------ + if (!gpsValid) { + *magFieldModelValid = false; + } else { + // 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.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(gpsLongitude, gpsLatitude, gpsAltitude, timeOfMgmMeasurement, + magFieldModel); + *magFieldModelValid = false; + } + + return true; } -bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, - const float *mgm1Value, bool mgm1valid, const float *mgm2Value, - bool mgm2valid, const float *mgm3Value, bool mgm3valid, - const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, - const AcsParameters::MgmHandlingParameters *mgmParameters, - const double gpsLatitude, const double gpsLongitude, - const double gpsAltitude, bool gpsValid, double *magFieldEst, bool *outputValid, - double *magFieldModel, bool*magFieldModelValid, - double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid) { +void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, + const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, + double *sunDirEst, bool *sunDirEstValid, + double *sunVectorInertial, bool *sunVectorInertialValid, + double *sunVectorDerivative, bool *sunVectorDerivativeValid) { + susData.sus0.setValid(susConverter.checkSunSensorData(susData.sus0)); + susData.sus1.setValid(susConverter.checkSunSensorData(susData.sus1)); + susData.sus2.setValid(susConverter.checkSunSensorData(susData.sus2)); + susData.sus3.setValid(susConverter.checkSunSensorData(susData.sus3)); + susData.sus4.setValid(susConverter.checkSunSensorData(susData.sus4)); + susData.sus5.setValid(susConverter.checkSunSensorData(susData.sus5)); + susData.sus6.setValid(susConverter.checkSunSensorData(susData.sus6)); + susData.sus7.setValid(susConverter.checkSunSensorData(susData.sus7)); + susData.sus8.setValid(susConverter.checkSunSensorData(susData.sus8)); + susData.sus9.setValid(susConverter.checkSunSensorData(susData.sus9)); + susData.sus10.setValid(susConverter.checkSunSensorData(susData.sus10)); + susData.sus11.setValid(susConverter.checkSunSensorData(susData.sus11)); - if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { - *outputValid = false; - validMagField = false; - return false; - } - // Transforming Values to the Body Frame (actually it is the geometry frame atm) - float mgm0ValueBody[3] = {0,0,0}, mgm1ValueBody[3] = {0,0,0}, - mgm2ValueBody[3] = {0,0,0}, mgm3ValueBody[3] = {0,0,0}, - mgm4ValueBody[3] = {0,0,0}; + if (!susData.sus0.isValid() && !susData.sus1.isValid() && !susData.sus2.isValid() && + !susData.sus3.isValid() && !susData.sus4.isValid() && !susData.sus5.isValid() && + !susData.sus6.isValid() && !susData.sus7.isValid() && !susData.sus8.isValid() && + !susData.sus9.isValid() && !susData.sus10.isValid() && !susData.sus11.isValid()) { + *sunDirEstValid = false; + return; + } else { + // WARNING: NOT TRANSFORMED IN BODY FRAME YET + // Transformation into Geomtry Frame + float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, + sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, + sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, + sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - bool validUnit[5] = {false, false, false, false, false}; - uint8_t validCount = 0; - if (mgm0valid) { - MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, mgm0ValueBody, 3, 3, 1); - validCount += 1; - validUnit[0] = true; - } - if (mgm1valid) { - MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, mgm1ValueBody, 3, 3, 1); - validCount += 1; - validUnit[1] = true; - } - if (mgm2valid) { - MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, mgm2ValueBody, 3, 3, 1); - validCount += 1; - validUnit[2] = true; - } - if (mgm3valid) { - MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, mgm3ValueBody, 3, 3, 1); - validCount += 1; - validUnit[3] = true; - } - if (mgm4valid) { - MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value, mgm4ValueBody, 3, 3, 1); - validCount += 1; - validUnit[4] = true; - } + if (susData.sus0.isValid()) { + MatrixOperations::multiply( + susParameters->sus0orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus0, susParameters->sus0coeffAlpha, + susParameters->sus0coeffBeta), + sus0VecBody, 3, 3, 1); + } + if (susData.sus1.isValid()) { + MatrixOperations::multiply( + susParameters->sus1orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus1, susParameters->sus1coeffAlpha, + susParameters->sus1coeffBeta), + sus1VecBody, 3, 3, 1); + } + if (susData.sus2.isValid()) { + MatrixOperations::multiply( + susParameters->sus2orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus2, susParameters->sus2coeffAlpha, + susParameters->sus2coeffBeta), + sus2VecBody, 3, 3, 1); + } + if (susData.sus3.isValid()) { + MatrixOperations::multiply( + susParameters->sus3orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus3, susParameters->sus3coeffAlpha, + susParameters->sus3coeffBeta), + sus3VecBody, 3, 3, 1); + } + if (susData.sus4.isValid()) { + MatrixOperations::multiply( + susParameters->sus4orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus4, susParameters->sus4coeffAlpha, + susParameters->sus4coeffBeta), + sus4VecBody, 3, 3, 1); + } + if (susData.sus5.isValid()) { + MatrixOperations::multiply( + susParameters->sus5orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus5, susParameters->sus5coeffAlpha, + susParameters->sus5coeffBeta), + sus5VecBody, 3, 3, 1); + } + if (susData.sus6.isValid()) { + MatrixOperations::multiply( + susParameters->sus6orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus6, susParameters->sus6coeffAlpha, + susParameters->sus6coeffBeta), + sus6VecBody, 3, 3, 1); + } + if (susData.sus7.isValid()) { + MatrixOperations::multiply( + susParameters->sus7orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus7, susParameters->sus7coeffAlpha, + susParameters->sus7coeffBeta), + sus7VecBody, 3, 3, 1); + } + if (susData.sus8.isValid()) { + MatrixOperations::multiply( + susParameters->sus8orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus8, susParameters->sus8coeffAlpha, + susParameters->sus8coeffBeta), + sus8VecBody, 3, 3, 1); + } + if (susData.sus9.isValid()) { + MatrixOperations::multiply( + susParameters->sus9orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus9, susParameters->sus9coeffAlpha, + susParameters->sus9coeffBeta), + sus9VecBody, 3, 3, 1); + } + if (susData.sus10.isValid()) { + MatrixOperations::multiply( + susParameters->sus10orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus10, susParameters->sus10coeffAlpha, + susParameters->sus10coeffBeta), + sus10VecBody, 3, 3, 1); + } + if (susData.sus11.isValid()) { + MatrixOperations::multiply( + susParameters->sus11orientationMatrix[0], + susConverter.getSunVectorSensorFrame(susData.sus11, susParameters->sus11coeffAlpha, + susParameters->sus11coeffBeta), + sus11VecBody, 3, 3, 1); + } - /* -------- MagFieldEst: Middle Value ------- */ - float mgmValues[3][5] = { { mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], - mgm3ValueBody[0], mgm4ValueBody[0] }, { mgm0ValueBody[1], mgm1ValueBody[1], - mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1] }, { mgm0ValueBody[2], - mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2] } }; - double mgmValidValues[3][validCount]; - uint8_t j = 0; - for (uint8_t i = 0; i < validCount; i++) { - if (validUnit[i]) { - mgmValidValues[0][j] = mgmValues[0][i]; - mgmValidValues[1][j] = mgmValues[1][i]; - mgmValidValues[2][j] = mgmValues[2][i]; - j += 1; - } - } - //Selection Sort - double mgmValidValuesSort[3][validCount]; - MathOperations::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount); + /* ------ Mean Value: susDirEst ------ */ + // Timo already done + bool validIds[12] = {susData.sus0.isValid(), susData.sus1.isValid(), susData.sus2.isValid(), + susData.sus3.isValid(), susData.sus4.isValid(), susData.sus5.isValid(), + susData.sus6.isValid(), susData.sus7.isValid(), susData.sus8.isValid(), + susData.sus9.isValid(), susData.sus10.isValid(), susData.sus11.isValid()}; + float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], + sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], + sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, + {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1], + sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1], + sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]}, + {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2], + sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], + sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; - uint8_t n = ceil(validCount/2); - magFieldEst[0] = mgmValidValuesSort[0][n]; - magFieldEst[1] = mgmValidValuesSort[1][n]; - magFieldEst[2] = mgmValidValuesSort[2][n]; - validMagField = true; + double susMeanValue[3] = {0, 0, 0}; + uint8_t validSusCounter = 0; + for (uint8_t i = 0; i < 12; i++) { + if (validIds[i]) { + susMeanValue[0] += susVecBody[0][i]; + susMeanValue[1] += susVecBody[1][i]; + susMeanValue[2] += susVecBody[2][i]; + validSusCounter += 1; + } + } + double divisor = 1 / validSusCounter; + VectorOperations::mulScalar(susMeanValue, divisor, sunDirEst, 3); + *sunDirEstValid = true; + } - //-----------------------Mag Rate Computation --------------------------------------------------- - double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); - for (uint8_t i = 0; i < 3; i++) { - magneticFieldVectorDerivative[i] = (magFieldEst[i] - - savedMagFieldEst[i]) / timeDiff; - savedMagFieldEst[i] = magFieldEst[i]; - } + /* -------- Sun Derivatiative --------------------- */ - *magneticFieldVectorDerivativeValid = true; - if (timeOfSavedMagFieldEst.tv_sec == 0) { - magneticFieldVectorDerivative[0] = 0; - magneticFieldVectorDerivative[1] = 0; - magneticFieldVectorDerivative[2] = 0; - *magneticFieldVectorDerivativeValid = false; - } + double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); + for (uint8_t i = 0; i < 3; i++) { + sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff; + savedSunVector[i] = sunDirEst[i]; + } - timeOfSavedMagFieldEst = timeOfMgmMeasurement; + *sunVectorDerivativeValid = true; + if (timeOfSavedSusDirEst.tv_sec == 0) { + sunVectorDerivative[0] = 0; + sunVectorDerivative[1] = 0; + sunVectorDerivative[2] = 0; + *sunVectorDerivativeValid = false; + } + timeOfSavedSusDirEst = timeOfSusMeasurement; - *outputValid = true; + /* -------- Sun Model Direction (IJK frame) ------- */ + // if (useSunModel) eventuell + double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); - // ---------------- IGRF- 13 Implementation here ------------------------------------------------ - if (!gpsValid){ - *magFieldModelValid = false; - } - else{ - // 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.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(gpsLongitude, gpsLatitude, gpsAltitude, - timeOfMgmMeasurement, magFieldModel); - *magFieldModelValid = false; - } + // Julean Centuries + double JC2000 = JD2000 / 36525; - return true; + 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; + + sunVectorInertial[0] = cos(eclipticLongitude); + sunVectorInertial[1] = sin(eclipticLongitude) * cos(epsilon); + sunVectorInertial[2] = sin(eclipticLongitude) * sin(epsilon); + + *sunVectorInertialValid = true; } -void SensorProcessing::processSus(const float sus0Value[], bool sus0valid, const float sus1Value[], bool sus1valid, - const float sus2Value[], bool sus2valid, const float sus3Value[], bool sus3valid, - const float sus4Value[], bool sus4valid, const float sus5Value[], bool sus5valid, - const float sus6Value[], bool sus6valid, const float sus7Value[], bool sus7valid, - const float sus8Value[], bool sus8valid, const float sus9Value[], bool sus9valid, - const float sus10Value[], bool sus10valid, const float sus11Value[], bool sus11valid, - timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, - const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, - double *sunVectorInertial, bool *sunVectorInertialValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid){ - - if(!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid - && !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid){ - *sunDirEstValid = false; - return; - } - else{ - // WARNING: NOT TRANSFORMED IN BODY FRAME YET - // Transformation into Geomtry Frame - float sus0ValueBody[3] = {0,0,0}, sus1ValueBody[3] = {0,0,0}, sus2ValueBody[3] = {0,0,0}, - sus3ValueBody[3] = {0,0,0}, sus4ValueBody[3] = {0,0,0}, sus5ValueBody[3] = {0,0,0}, - sus6ValueBody[3] = {0,0,0}, sus7ValueBody[3] = {0,0,0}, sus8ValueBody[3] = {0,0,0}, - sus9ValueBody[3] = {0,0,0}, sus10ValueBody[3] = {0,0,0}, sus11ValueBody[3] = {0,0,0}; - - if (sus0valid) { - MatrixOperations::multiply(susParameters->sus0orientationMatrix[0], sus0Value, sus0ValueBody, 3, 3, 1); - } - if (sus1valid) { - MatrixOperations::multiply(susParameters->sus1orientationMatrix[0], sus1Value, sus1ValueBody, 3, 3, 1); - } - if (sus2valid) { - MatrixOperations::multiply(susParameters->sus2orientationMatrix[0], sus2Value, sus2ValueBody, 3, 3, 1); - } - if (sus3valid) { - MatrixOperations::multiply(susParameters->sus3orientationMatrix[0], sus3Value, sus3ValueBody, 3, 3, 1); - } - if (sus4valid) { - MatrixOperations::multiply(susParameters->sus4orientationMatrix[0], sus4Value, sus4ValueBody, 3, 3, 1); - } - if (sus5valid) { - MatrixOperations::multiply(susParameters->sus5orientationMatrix[0], sus5Value, sus5ValueBody, 3, 3, 1); - } - if (sus6valid) { - MatrixOperations::multiply(susParameters->sus6orientationMatrix[0], sus6Value, sus6ValueBody, 3, 3, 1); - } - if (sus7valid) { - MatrixOperations::multiply(susParameters->sus7orientationMatrix[0], sus7Value, sus7ValueBody, 3, 3, 1); - } - if (sus8valid) { - MatrixOperations::multiply(susParameters->sus8orientationMatrix[0], sus8Value, sus8ValueBody, 3, 3, 1); - } - if (sus9valid) { - MatrixOperations::multiply(susParameters->sus9orientationMatrix[0], sus9Value, sus9ValueBody, 3, 3, 1); - } - if (sus10valid) { - MatrixOperations::multiply(susParameters->sus10orientationMatrix[0], sus10Value, sus10ValueBody, 3, 3, 1); - } - if (sus11valid) { - MatrixOperations::multiply(susParameters->sus11orientationMatrix[0], sus11Value, sus11ValueBody, 3, 3, 1); - } - - /* ------ Mean Value: susDirEst ------ */ - // Timo already done - bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, - sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; - float susValuesBody[3][12] = {{sus0ValueBody[0], sus1ValueBody[0], sus2ValueBody[0], sus3ValueBody[0], sus4ValueBody[0], - sus5ValueBody[0], sus6ValueBody[0], sus7ValueBody[0], sus8ValueBody[0], sus9ValueBody[0], sus10ValueBody[0], sus11ValueBody[0]}, - {sus0ValueBody[1], sus1ValueBody[1], sus2ValueBody[1], sus3ValueBody[1], sus4ValueBody[1], - sus5ValueBody[1], sus6ValueBody[1], sus7ValueBody[1], sus8ValueBody[1], sus9ValueBody[1], sus10ValueBody[1], sus11ValueBody[1]}, - {sus0ValueBody[2], sus1ValueBody[2], sus2ValueBody[2], sus3ValueBody[2], sus4ValueBody[2], - sus5ValueBody[2], sus6ValueBody[2], sus7ValueBody[2], sus8ValueBody[2], sus9ValueBody[2], sus10ValueBody[2], sus11ValueBody[2]}}; - - double susMeanValue[3] = {0,0,0}; - uint8_t validSusCounter = 0; - for (uint8_t i = 0; i < 12; i++){ - if (validIds[i]){ - susMeanValue[0]+=susValuesBody[0][i]; - susMeanValue[1]+=susValuesBody[1][i]; - susMeanValue[2]+=susValuesBody[2][i]; - validSusCounter+=1; - } - } - double divisor = 1/validSusCounter; - VectorOperations::mulScalar(susMeanValue, divisor, sunDirEst, 3); - *sunDirEstValid = true; - } - - /* -------- Sun Derivatiative --------------------- */ - - double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); - for (uint8_t i = 0; i < 3; i++) { - sunVectorDerivative[i] = (sunDirEst[i] - - savedSunVector[i]) / timeDiff; - savedSunVector[i] = sunDirEst[i]; - } - - *sunVectorDerivativeValid = true; - if (timeOfSavedSusDirEst.tv_sec == 0) { - sunVectorDerivative[0] = 0; - sunVectorDerivative[1] = 0; - sunVectorDerivative[2] = 0; - *sunVectorDerivativeValid = false; - } - - timeOfSavedSusDirEst = timeOfSusMeasurement; - - /* -------- Sun Model Direction (IJK frame) ------- */ - // if (useSunModel) eventuell - double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); - - //Julean Centuries - 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; - - - sunVectorInertial[0] = cos(eclipticLongitude); - sunVectorInertial[1] = sin(eclipticLongitude) - * cos(epsilon); - sunVectorInertial[2] = sin(eclipticLongitude) - * sin(epsilon); - - - *sunVectorInertialValid = true; -} - - void SensorProcessing::processRmu(const double rmu0Value[], bool rmu0valid, - const double rmu1Value[], bool rmu1valid, - const double rmu2Value[], bool rmu2valid, - timeval timeOfrmuMeasurement, const AcsParameters::RmuHandlingParameters *rmuParameters, - double *satRatEst, bool *satRateEstValid){ + const double rmu1Value[], bool rmu1valid, + const double rmu2Value[], bool rmu2valid, + timeval timeOfrmuMeasurement, + const AcsParameters::RmuHandlingParameters *rmuParameters, + double *satRatEst, bool *satRateEstValid) { + if (!rmu0valid && !rmu1valid && !rmu2valid) { + *satRateEstValid = false; + return; + } + // Transforming Values to the Body Frame (actually it is the geometry frame atm) + double rmu0ValueBody[3] = {0, 0, 0}, rmu1ValueBody[3] = {0, 0, 0}, rmu2ValueBody[3] = {0, 0, 0}; - if (!rmu0valid && !rmu1valid && !rmu2valid) { - *satRateEstValid = false; - return; - } - // Transforming Values to the Body Frame (actually it is the geometry frame atm) - double rmu0ValueBody[3] = {0,0,0}, rmu1ValueBody[3]= {0,0,0}, - rmu2ValueBody[3] = {0,0,0}; + bool validUnit[3] = {false, false, false}; + uint8_t validCount = 0; + if (rmu0valid) { + MatrixOperations::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value, + rmu0ValueBody, 3, 3, 1); + validCount += 1; + validUnit[0] = true; + } + if (rmu1valid) { + MatrixOperations::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value, + rmu1ValueBody, 3, 3, 1); + validCount += 1; + validUnit[1] = true; + } + if (rmu2valid) { + MatrixOperations::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value, + rmu2ValueBody, 3, 3, 1); + validCount += 1; + validUnit[2] = true; + } - bool validUnit[3] = {false, false, false}; - uint8_t validCount = 0; - if (rmu0valid) { - MatrixOperations::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value, rmu0ValueBody, 3, 3, 1); - validCount += 1; - validUnit[0] = true; - } - if (rmu1valid) { - MatrixOperations::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value, rmu1ValueBody, 3, 3, 1); - validCount += 1; - validUnit[1] = true; - } - if (rmu2valid) { - MatrixOperations::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value, rmu2ValueBody, 3, 3, 1); - validCount += 1; - validUnit[2] = true; - } - - /* -------- SatRateEst: Middle Value ------- */ - double rmuValues[3][3] = { { rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]}, { rmu0ValueBody[1], rmu1ValueBody[1], - rmu2ValueBody[1]}, { rmu0ValueBody[2], - rmu1ValueBody[2], rmu2ValueBody[2]} }; - double rmuValidValues[3][validCount]; - uint8_t j = 0; - for (uint8_t i = 0; i < validCount; i++) { - if (validUnit[i]) { - rmuValidValues[0][j] = rmuValues[0][i]; - rmuValidValues[1][j] = rmuValues[1][i]; - rmuValidValues[2][j] = rmuValues[2][i]; - j += 1; - } - } - //Selection Sort - double rmuValidValuesSort[3][validCount]; - MathOperations::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount); - - uint8_t n = ceil(validCount/2); - satRatEst[0] = rmuValidValuesSort[0][n]; - satRatEst[1] = rmuValidValuesSort[1][n]; - satRatEst[2] = rmuValidValuesSort[2][n]; - *satRateEstValid = true; + /* -------- SatRateEst: Middle Value ------- */ + double rmuValues[3][3] = {{rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]}, + {rmu0ValueBody[1], rmu1ValueBody[1], rmu2ValueBody[1]}, + {rmu0ValueBody[2], rmu1ValueBody[2], rmu2ValueBody[2]}}; + double rmuValidValues[3][validCount]; + uint8_t j = 0; + for (uint8_t i = 0; i < validCount; i++) { + if (validUnit[i]) { + rmuValidValues[0][j] = rmuValues[0][i]; + rmuValidValues[1][j] = rmuValues[1][i]; + rmuValidValues[2][j] = rmuValues[2][i]; + j += 1; + } + } + // Selection Sort + double rmuValidValuesSort[3][validCount]; + MathOperations::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount); + uint8_t n = ceil(validCount / 2); + satRatEst[0] = rmuValidValuesSort[0][n]; + satRatEst[1] = rmuValidValuesSort[1][n]; + satRatEst[2] = rmuValidValuesSort[2][n]; + *satRateEstValid = true; } void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, - const bool validGps, double *gcLatitude, double *gdLongitude){ - // name to convert not process - if (validGps) { - // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic - *gdLongitude = gps0longitude * PI / 180; - double latitudeRad = gps0latitude * PI / 180; - double eccentricityWgs84 = 0.0818195; - double factor = 1 - pow(eccentricityWgs84, 2); - *gcLatitude = atan(factor * tan(latitudeRad)); - validGcLatitude = true; - - } + const bool validGps, double *gcLatitude, double *gdLongitude) { + // name to convert not process + if (validGps) { + // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic + *gdLongitude = gps0longitude * PI / 180; + double latitudeRad = gps0latitude * PI / 180; + double eccentricityWgs84 = 0.0818195; + double factor = 1 - pow(eccentricityWgs84, 2); + *gcLatitude = atan(factor * tan(latitudeRad)); + validGcLatitude = true; + } } void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, - ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { + ACS::OutputValues *outputValues, + const AcsParameters *acsParameters) { + sensorValues->update(); + processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, + &outputValues->gcLatitude, &outputValues->gdLongitude); - sensorValues->update(); - processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, - sensorValues->gps0Valid, &outputValues->gcLatitude, &outputValues->gdLongitude); - - outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, - sensorValues->mgm1, sensorValues->mgm1Valid, - sensorValues->mgm2, sensorValues->mgm2Valid, - sensorValues->mgm3, sensorValues->mgm3Valid, - sensorValues->mgm4, sensorValues->mgm4Valid, now, - &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, - outputValues->gdLongitude, sensorValues->gps0altitude, - sensorValues->gps0Valid, - outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid, - outputValues->magneticFieldVectorDerivative, &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? - - processSus(sensorValues->sus0, sensorValues->sus0Valid, sensorValues->sus1, sensorValues->sus1Valid, - sensorValues->sus2, sensorValues->sus2Valid, sensorValues->sus3, sensorValues->sus3Valid, - sensorValues->sus4, sensorValues->sus4Valid, sensorValues->sus5, sensorValues->sus5Valid, - sensorValues->sus6, sensorValues->sus6Valid, sensorValues->sus7, sensorValues->sus7Valid, - sensorValues->sus8, sensorValues->sus8Valid, sensorValues->sus9, sensorValues->sus9Valid, - sensorValues->sus10, sensorValues->sus10Valid, sensorValues->sus11, sensorValues->sus11Valid, - now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, - outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // VALID outputs ? - - processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, sensorValues->rmu1Valid, - sensorValues->rmu2, sensorValues->rmu2Valid, now, &acsParameters->rmuHandlingParameters, - outputValues->satRateEst, &outputValues->satRateEstValid); + /*outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, + sensorValues->mgm1, sensorValues->mgm1Valid, + sensorValues->mgm2, sensorValues->mgm2Valid, + sensorValues->mgm3, sensorValues->mgm3Valid, + sensorValues->mgm4, sensorValues->mgm4Valid, now, + &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, + outputValues->gdLongitude, sensorValues->gps0altitude, + sensorValues->gps0Valid, + outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid, + outputValues->magneticFieldVectorDerivative, + &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? + processSus(sensorValues->sus0, sensorValues->sus0Valid, sensorValues->sus1, + sensorValues->sus1Valid, sensorValues->sus2, sensorValues->sus2Valid, sensorValues->sus3, + sensorValues->sus3Valid, sensorValues->sus4, sensorValues->sus4Valid, sensorValues->sus5, + sensorValues->sus5Valid, sensorValues->sus6, sensorValues->sus6Valid, sensorValues->sus7, + sensorValues->sus7Valid, sensorValues->sus8, sensorValues->sus8Valid, sensorValues->sus9, + sensorValues->sus9Valid, sensorValues->sus10, sensorValues->sus10Valid, sensorValues->sus11, + sensorValues->sus11Valid, now, &acsParameters->susHandlingParameters, + &acsParameters->sunModelParameters, outputValues->sunDirEst, &outputValues->sunDirEstValid, + outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // + VALID outputs ? +*/ + processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, + sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now, + &acsParameters->rmuHandlingParameters, outputValues->satRateEst, + &outputValues->satRateEstValid); } - diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 097c88ed..baa9c5b6 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -5,14 +5,16 @@ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ -#include "AcsParameters.h" -#include "SensorValues.h" -#include "OutputValues.h" -#include "config/classIds.h" -#include //uint8_t -#include /*purpose, timeval ?*/ #include +#include //uint8_t +#include /*purpose, timeval ?*/ +#include "SusConverter.h" +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "AcsParameters.h" +#include "OutputValues.h" +#include "SensorValues.h" +#include "config/classIds.h" /*Planned: * - Fusion of Sensor Measurements - @@ -26,65 +28,53 @@ * magField * SunDirEst*/ -class SensorProcessing{ -public: +class SensorProcessing { + public: + void reset(); - void reset(); + SensorProcessing(AcsParameters *acsParameters_); + virtual ~SensorProcessing(); - SensorProcessing(AcsParameters *acsParameters_); - virtual ~SensorProcessing(); + void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + const AcsParameters *acsParameters); // Will call protected functions + private: + protected: + // short description needed for every function + bool processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, + const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, + const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, + const AcsParameters::MgmHandlingParameters *mgmParameters, + const double gpsLatitude, const double gpsLongitude, const double gpsAltitude, + bool gpsValid, double *magFieldEst, bool *outputValid, double *magFieldModel, + bool *magFieldModelValid, double *magneticFieldVectorDerivative, + bool *magneticFieldVectorDerivativeValid); // Output - void process(timeval now, ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, - const AcsParameters *acsParameters); // Will call protected functions -private: + void processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, + const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, + bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, + double *sunVectorDerivative, bool *sunVectorDerivativeValid); -protected: - // short description needed for every function - bool processMgm(const float *mgm0Value, bool mgm0valid, - const float *mgm1Value, bool mgm1valid, - const float *mgm2Value, bool mgm2valid, - const float *mgm3Value, bool mgm3valid, - const float *mgm4Value, bool mgm4valid, - timeval timeOfMgmMeasurement, - const AcsParameters::MgmHandlingParameters *mgmParameters, - const double gpsLatitude, const double gpsLongitude, - const double gpsAltitude, bool gpsValid, - double *magFieldEst, bool *outputValid, - double *magFieldModel, bool*magFieldModelValid, - double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid); //Output + void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu + const double rmu1Value[], bool rmu1valid, const double rmu2Value[], + bool rmu2valid, timeval timeOfrmuMeasurement, + const AcsParameters::RmuHandlingParameters *rmuParameters, double *satRatEst, + bool *satRateEstValid); - void processSus(const float sus0Value[], bool sus0valid, const float sus1Value[], bool sus1valid, - const float sus2Value[], bool sus2valid, const float sus3Value[], bool sus3valid, - const float sus4Value[], bool sus4valid, const float sus5Value[], bool sus5valid, - const float sus6Value[], bool sus6valid, const float sus7Value[], bool sus7valid, - const float sus8Value[], bool sus8valid, const float sus9Value[], bool sus9valid, - const float sus10Value[], bool sus10valid, const float sus11Value[], bool sus11valid, - timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, - const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, - double *sunVectorModel, bool *sunVectorModelValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid); + void processStr(); - void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu - const double rmu1Value[], bool rmu1valid, - const double rmu2Value[], bool rmu2valid, - timeval timeOfrmuMeasurement, const AcsParameters::RmuHandlingParameters *rmuParameters, - double *satRatEst, bool *satRateEstValid); - - void processStr(); - - void processGps(const double gps0latitude, const double gps0longitude, - const bool validGps, double *gcLatitude, double *gdLongitude); - - - double savedMagFieldEst[3]; - timeval timeOfSavedMagFieldEst; - double savedSunVector[3]; - timeval timeOfSavedSusDirEst; - bool validMagField; - bool validGcLatitude; + void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, + double *gcLatitude, double *gdLongitude); + double savedMagFieldEst[3]; + timeval timeOfSavedMagFieldEst; + double savedSunVector[3]; + timeval timeOfSavedSusDirEst; + bool validMagField; + bool validGcLatitude; + SusConverter susConverter; + AcsParameters acsParameters; }; #endif /*SENSORPROCESSING_H_*/ - diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 13f23808..7d8f5bd2 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -6,191 +6,68 @@ */ #include "SusConverter.h" -#include //for atan2 -#include -#include + #include #include +#include +#include //for atan2 +#include -void SunSensor::checkSunSensorData(uint8_t susNumber) { - uint16_t channelValueSum; - - // Check individual channel values - for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters - - if (susChannelValues[susNumber][k] <= channelValueCheckLow || - susChannelValues[susNumber][k] > channelValueCheckHigh) { // Channel values out of range for 12 bit SUS - // channel measurement range? - validFlag[susNumber] = returnvalue::FAILED; - /*printf( - "The value of channel %i from sun sensor %i is not inside the borders of valid data with " - "a value of %i \n", - k, susNumber, ChannelValue[k]);*/ - } else if (susChannelValues[susNumber][k] > - susChannelValues[susNumber][4]) { // Channel values higher than zero current threshold GNDREF? - validFlag[susNumber] = returnvalue::FAILED; - /*printf( - "The value of channel %i from sun sensor %i is higher than the zero current threshold " - "GNDREF\n", - k, susNumber);*/ - }; +bool SusConverter::checkSunSensorData(lp_vec_t susChannel) { + if (susChannel.value[0] <= susChannelValueCheckLow || + susChannel.value[0] > susChannelValueCheckHigh || + susChannel.value[0] > susChannel.value[GNDREF]) { + return false; + } + if (susChannel.value[1] <= susChannelValueCheckLow || + susChannel.value[1] > susChannelValueCheckHigh || + susChannel.value[1] > susChannel.value[GNDREF]) { + return false; + }; + if (susChannel.value[2] <= susChannelValueCheckLow || + susChannel.value[2] > susChannelValueCheckHigh || + susChannel.value[2] > susChannel.value[GNDREF]) { + return false; + }; + if (susChannel.value[3] <= susChannelValueCheckLow || + susChannel.value[3] > susChannelValueCheckHigh || + susChannel.value[3] > susChannel.value[GNDREF]) { + return false; }; - // check sum of all channel values to check if sun sensor is illuminated by the sun (sum is - // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon - // reflection or earth albedo) - channelValueSum = - 4 * susChannelValues[susNumber][4] - (susChannelValues[susNumber][0] + - susChannelValues[susNumber][1] + susChannelValues[susNumber][2] + - susChannelValues[susNumber][3]); - if ((channelValueSum < channelValueSumHigh) && (channelValueSum > channelValueSumLow)) { - validFlag[susNumber] = returnvalue::FAILED; - //printf("Sun sensor %i is not illuminated by the sun\n", susNumber); + susChannelValueSum = 4 * susChannel.value[GNDREF] - (susChannel.value[0] + susChannel.value[1] + + susChannel.value[2] + susChannel.value[3]); + if ((susChannelValueSum < susChannelValueSumHigh) && + (susChannelValueSum > susChannelValueSumLow)) { + return false; }; + return true; } -void SunSensor::calcAngle(uint8_t susNumber) { +void SusConverter::calcAngle(lp_vec_t susChannel) { float xout, yout; float s = 0.03; // s=[mm] gap between diodes uint8_t d = 5; // d=[mm] edge length of the quadratic aperture uint8_t h = 1; // h=[mm] distance between diodes and aperture int ch0, ch1, ch2, ch3; // Substract measurement values from GNDREF zero current threshold - ch0 = susChannelValues[susNumber][4] - susChannelValues[susNumber][0]; - ch1 = susChannelValues[susNumber][4] - susChannelValues[susNumber][1]; - ch2 = susChannelValues[susNumber][4] - susChannelValues[susNumber][2]; - ch3 = susChannelValues[susNumber][4] - susChannelValues[susNumber][3]; + ch0 = susChannel.value[GNDREF] - susChannel.value[0]; + ch1 = susChannel.value[GNDREF] - susChannel.value[1]; + ch2 = susChannel.value[GNDREF] - susChannel.value[2]; + ch3 = susChannel.value[GNDREF] - susChannel.value[3]; // Calculation of x and y xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] // Calculation of the angles - alphaBetaRaw[susNumber][0] = atan2(xout, h) * (180 / M_PI); //[°] - alphaBetaRaw[susNumber][1] = atan2(yout, h) * (180 / M_PI); //[°] + alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°] + alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°] } -void SunSensor::setCalibrationCoefficients(uint8_t susNumber) { - switch (susNumber) { // search for the correct calibration coefficients for each SUS - - case 0: - for (uint8_t row = 0; row < 9; - row++) { // save the correct coefficients in the right SUS class - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; - } - } - break; - - case 1: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; - } - } - break; - - case 2: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; - } - } - break; - - case 3: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; - } - } - break; - - case 4: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; - } - } - break; - - case 5: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; - } - } - break; - - case 6: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; - } - } - break; - - case 7: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; - } - } - break; - - case 8: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; - } - } - break; - - case 9: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; - } - } - break; - - case 10: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; - } - } - break; - - case 11: - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t column = 0; column < 10; column++) { - coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; - coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; - } - } - break; - } -} - -void SunSensor::Calibration(uint8_t susNumber) { - float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; - uint8_t index; - - alpha_m = alphaBetaRaw[susNumber][0]; //[°] - beta_m = alphaBetaRaw[susNumber][1]; //[°] +void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { + uint8_t index, k, l; // while loop iterates above all calibration cells to use the different calibration functions in // each cell @@ -201,68 +78,62 @@ void SunSensor::Calibration(uint8_t susNumber) { while (l < 3) { l = l + 1; // if-condition to check in which cell the data point has to be - if ((alpha_m > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && - alpha_m < ((completeCellWidth * (k / 3)) - halfCellWidth)) && - (beta_m > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) && - beta_m < ((completeCellWidth * (l / 3)) - halfCellWidth))) { + if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && + alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) && + (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) && + alphaBetaRaw[1] < ((completeCellWidth * (l / 3)) - halfCellWidth))) { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell - // -> first cell has number 0 - alphaBetaCalibrated[susNumber][0] = - coeffAlpha[susNumber][index][0] + coeffAlpha[susNumber][index][1] * alpha_m + coeffAlpha[susNumber][index][2] * beta_m + - coeffAlpha[susNumber][index][3] * alpha_m * alpha_m + coeffAlpha[susNumber][index][4] * alpha_m * beta_m + - coeffAlpha[susNumber][index][5] * beta_m * beta_m + - coeffAlpha[susNumber][index][6] * alpha_m * alpha_m * alpha_m + - coeffAlpha[susNumber][index][7] * alpha_m * alpha_m * beta_m + - coeffAlpha[susNumber][index][8] * alpha_m * beta_m * beta_m + - coeffAlpha[susNumber][index][9] * beta_m * beta_m * beta_m; //[°] - alphaBetaCalibrated[susNumber][1] = - coeffBeta[susNumber][index][0] + coeffBeta[susNumber][index][1] * alpha_m + - coeffBeta[susNumber][index][2] * beta_m + coeffBeta[susNumber][index][3] * alpha_m * alpha_m + - coeffBeta[susNumber][index][4] * alpha_m * beta_m + - coeffBeta[susNumber][index][5] * beta_m * beta_m + - coeffBeta[susNumber][index][6] * alpha_m * alpha_m * alpha_m + - coeffBeta[susNumber][index][7] * alpha_m * alpha_m * beta_m + - coeffBeta[susNumber][index][8] * alpha_m * beta_m * beta_m + - coeffBeta[susNumber][index][9] * beta_m * beta_m * beta_m; //[°] + alphaBetaCalibrated[0] = + coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] + + coeffAlpha[index][2] * alphaBetaRaw[1] + + coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + + coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + + coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + + coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + + coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + + coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + + coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] + alphaBetaCalibrated[1] = + coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] + + coeffBeta[index][2] * alphaBetaRaw[1] + + coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] + + coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] + + coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] + + coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] + + coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] + + coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] + + coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°] } } } } -void SunSensor::CalculateSunVector(uint8_t susNumber) { - float alpha, beta; - alpha = alphaBetaCalibrated[susNumber][0]; //[°] - beta = alphaBetaCalibrated[susNumber][1]; //[°] - +float* SusConverter::calculateSunVector() { // Calculate the normalized Sun Vector - sunVectorBodyFrame[susNumber][0] = - (tan(alpha * (M_PI / 180)) / - (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - sunVectorBodyFrame[susNumber][1] = - (tan(beta * (M_PI / 180)) / - (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - sunVectorBodyFrame[susNumber][2] = - (-1 / - (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); + sunVectorBodyFrame[0] = (tan(alphaBetaCalibrated[0] * (M_PI / 180)) / + (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + + powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + sunVectorBodyFrame[1] = (tan(alphaBetaCalibrated[1] * (M_PI / 180)) / + (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + + powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + sunVectorBodyFrame[2] = + (-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + + powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + + return sunVectorBodyFrame; } -float* SunSensor::getSunVectorBodyFrame(uint8_t susNumber) { - // return function for the sun vector in the body frame - float* SunVectorBodyFrameReturn = 0; - SunVectorBodyFrameReturn = new float[3]; - - SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[susNumber][0]; - SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[susNumber][1]; - SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[susNumber][2]; - - return SunVectorBodyFrameReturn; +float* SusConverter::getSunVectorSensorFrame(lp_vec_t susChannel, + const float coeffAlpha[9][10], + const float coeffBeta[9][10]) { + calcAngle(susChannel); + calibration(coeffAlpha, coeffBeta); + return calculateSunVector(); } -bool SunSensor::getValidFlag(uint8_t susNumber) { - return validFlag[susNumber]; -} +bool SusConverter::getValidFlag(uint8_t susNumber) { return validFlag[susNumber]; } -float* SunSensor::TransferSunVector() { +float* SusConverter::TransferSunVector() { float* sunVectorEIVE = 0; sunVectorEIVE = new float[3]; @@ -273,7 +144,7 @@ float* SunSensor::TransferSunVector() { for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { // save the sun vector of each SUS in their body frame into an array for - // further processing + // further processing float* SunVectorBodyFrame = &SunVectorBodyFrame[susNumber]; sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0]; sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1]; @@ -288,42 +159,53 @@ float* SunSensor::TransferSunVector() { for (uint8_t c1 = 0; c1 < 3; c1++) { for (uint8_t c2 = 0; c2 < 3; c2++) { switch (susNumber) { - case 0: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; break; case 1: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; break; case 2: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; break; case 3: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; break; case 4: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; break; case 5: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; break; case 6: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; break; case 7: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; break; case 8: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; break; case 9: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; break; case 10: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; break; case 11: - basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = + acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; break; } } @@ -343,9 +225,9 @@ float* SunSensor::TransferSunVector() { for (uint8_t i = 0; i < 3; i++) { float sum = 0; for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { - if (getValidFlag(susNumber) == returnvalue::OK){ + if (getValidFlag(susNumber) == returnvalue::OK) { sum += sunVectorMatrixEIVE[i][susNumber]; - //printf("%f\n", SunVectorMatrixEIVE[i][susNumber]); + // printf("%f\n", SunVectorMatrixEIVE[i][susNumber]); } } // ToDo: decide on length on sun vector @@ -359,5 +241,3 @@ float* SunSensor::TransferSunVector() { return sunVectorEIVE; } - - diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index bfec1dfe..32f9e954 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -8,69 +8,54 @@ #ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ #define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ -#include "AcsParameters.h" +#include #include -class SunSensor { - public: - SunSensor() {} +#include "AcsParameters.h" - void checkSunSensorData(uint8_t susNumber); - void calcAngle(uint8_t susNumber); - void setCalibrationCoefficients(uint8_t susNumber); - void Calibration(uint8_t susNumber); - void CalculateSunVector(uint8_t susNumber); +class SusConverter { + public: + SusConverter() {} + + bool checkSunSensorData(lp_vec_t susChannel); + + void calcAngle(lp_vec_t susChannel); + void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]); + float* calculateSunVector(); bool getValidFlag(uint8_t susNumber); - float* getSunVectorBodyFrame(uint8_t susNumber); + float* getSunVectorSensorFrame(lp_vec_t susChannel, const float coeffAlpha[9][10], + const float coeffBeta[9][10]); float* TransferSunVector(); private: - // ToDo: remove statics and replace with actual data - uint16_t susChannelValues[12][4] = { - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}, - {3913, 3912, 3799, 4056}}; //[Bit] - float alphaBetaRaw[12][2]; //[°] - float alphaBetaCalibrated[12][2]; //[°] - float sunVectorBodyFrame[12][3]; //[-] + float alphaBetaRaw[2]; //[°] + // float coeffAlpha[9][10]; + // float coeffBeta[9][10]; + float alphaBetaCalibrated[2]; //[°] + float sunVectorBodyFrame[3]; //[-] - bool validFlag[12] = {returnvalue::OK, - returnvalue::OK,returnvalue::OK, - returnvalue::OK,returnvalue::OK, - returnvalue::OK,returnvalue::OK, - returnvalue::OK,returnvalue::OK, - returnvalue::OK,returnvalue::OK, - returnvalue::OK}; + bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, + returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, + returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK}; - uint16_t channelValueCheckHigh = + static const uint8_t GNDREF = 4; + uint16_t susChannelValueCheckHigh = 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check - uint8_t channelValueCheckLow = + uint8_t susChannelValueCheckLow = 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check - uint16_t channelValueSumHigh = + uint16_t susChannelValueSumHigh = 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by // the reflection of sunlight from the moon/earth - uint8_t channelValueSumLow = + uint8_t susChannelValueSumLow = 0; //[Bit]low borderline for check if the sun sensor is illuminated // by the sun or by the reflection of sunlight from the moon/earth uint8_t completeCellWidth = 140, halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in // which cell a data point should be - - float coeffAlpha[12][9][10]; - float coeffBeta[12][9][10]; + uint16_t susChannelValueSum = 0; AcsParameters acsParameters; }; - #endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */ From d726ce4e496972fa5f95e772fe2e837070c2195d Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 09:43:52 +0200 Subject: [PATCH 040/244] permanently activated ACS_CTRL --- bsp_q7s/OBSWConfig.h.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index c20db3f1..a5f4f420 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -30,7 +30,7 @@ #define OBSW_ADD_SUN_SENSORS @OBSW_ADD_SUN_SENSORS@ #define OBSW_ADD_SUS_BOARD_ASS @OBSW_ADD_SUS_BOARD_ASS@ #define OBSW_ADD_ACS_BOARD @OBSW_ADD_ACS_BOARD@ -#define OBSW_ADD_ACS_CTRL @OBSW_ADD_ACS_CTRL@ +#define OBSW_ADD_ACS_CTRL 1 #define OBSW_ADD_GPS_CTRL @OBSW_ADD_GPS_CTRL@ #define OBSW_ADD_RW @OBSW_ADD_RW@ #define OBSW_ADD_RTD_DEVICES @OBSW_ADD_RTD_DEVICES@ From 5cbbf4ceb2eb5eefab22baa664f72c6468ac2569 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 09:44:28 +0200 Subject: [PATCH 041/244] corrected SUS dummy outputs to match student project --- dummies/SusDummy.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index ba267d61..0c2feaac 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -51,7 +51,8 @@ void SusDummy::performControlOperation() { susSet.channels[0] = 3913; susSet.channels[1] = 3912; susSet.channels[2] = 3799; - susSet.channels[3] = 4056; + susSet.channels[3] = 3797; + susSet.channels[4] = 4056; susSet.commit(); } From bbf4be1cfee605ef257b68c8dc21dd5c4509c179 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 09:53:33 +0200 Subject: [PATCH 042/244] removed reduntant parts. fixed integers messing up divisions --- mission/controller/acs/SusConverter.cpp | 117 +----------------------- 1 file changed, 4 insertions(+), 113 deletions(-) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 7d8f5bd2..ea52f5a2 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -67,16 +67,17 @@ void SusConverter::calcAngle(lp_vec_t susChannel) { } void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) { - uint8_t index, k, l; + uint8_t index; + float k, l; // while loop iterates above all calibration cells to use the different calibration functions in // each cell k = 0; while (k < 3) { - k = k + 1; + k++; l = 0; while (l < 3) { - l = l + 1; + l++; // if-condition to check in which cell the data point has to be if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) && @@ -131,113 +132,3 @@ float* SusConverter::getSunVectorSensorFrame(lp_vec_t susChannel, return calculateSunVector(); } -bool SusConverter::getValidFlag(uint8_t susNumber) { return validFlag[susNumber]; } - -float* SusConverter::TransferSunVector() { - float* sunVectorEIVE = 0; - sunVectorEIVE = new float[3]; - - uint8_t susAvail = 12; - int8_t basisMatrixUse[3][3]; - float sunVectorMatrixEIVE[3][12] = {0}; - float sunVectorMatrixBodyFrame[3][12]; - - for (uint8_t susNumber = 0; susNumber < 12; - susNumber++) { // save the sun vector of each SUS in their body frame into an array for - // further processing - float* SunVectorBodyFrame = &SunVectorBodyFrame[susNumber]; - sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0]; - sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1]; - sunVectorMatrixBodyFrame[2][susNumber] = SunVectorBodyFrame[2]; - } - - for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { - if (getValidFlag(susNumber) == returnvalue::FAILED) { - susAvail -= 1; - } // if the SUS data is not valid -> - - for (uint8_t c1 = 0; c1 < 3; c1++) { - for (uint8_t c2 = 0; c2 < 3; c2++) { - switch (susNumber) { - case 0: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; - break; - case 1: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; - break; - case 2: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; - break; - case 3: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; - break; - case 4: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; - break; - case 5: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; - break; - case 6: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; - break; - case 7: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; - break; - case 8: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; - break; - case 9: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; - break; - case 10: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; - break; - case 11: - basisMatrixUse[c1][c2] = - acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; - break; - } - } - } - - // matrix multiplication for transition in EIVE coordinatesystem - for (uint8_t p = 0; p < 3; p++) { - for (uint8_t q = 0; q < 3; q++) { - // normal matrix multiplication - sunVectorMatrixEIVE[p][susNumber] += - (basisMatrixUse[p][q] * sunVectorMatrixBodyFrame[q][susNumber]); - } - } - } - - if (susAvail > 0) { // Calculate one sun vector out of all sun vectors from the different SUS - for (uint8_t i = 0; i < 3; i++) { - float sum = 0; - for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { - if (getValidFlag(susNumber) == returnvalue::OK) { - sum += sunVectorMatrixEIVE[i][susNumber]; - // printf("%f\n", SunVectorMatrixEIVE[i][susNumber]); - } - } - // ToDo: decide on length on sun vector - sunVectorEIVE[i] = sum; - } - VectorOperations::normalize(sunVectorEIVE, sunVectorEIVE, 3); - } else { - // No sus is valid - throw std::invalid_argument("No sun sensor is valid"); // throw error - } - - return sunVectorEIVE; -} From f77b3498ec5f66c8bf215ef566488df8c0d37f92 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 09:54:06 +0200 Subject: [PATCH 043/244] fixed inputs. removed outputs. minor fixes --- mission/controller/AcsController.cpp | 11 +- mission/controller/acs/AcsParameters.h | 4 +- mission/controller/acs/SensorProcessing.cpp | 155 ++++++++++---------- mission/controller/acs/SensorProcessing.h | 7 +- 4 files changed, 86 insertions(+), 91 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2b0689c7..91d38eff 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -38,7 +38,7 @@ void AcsController::performControlOperation() { break; case SUBMODE_DETUMBLE: - // performDetumble(); + performDetumble(); break; case SUBMODE_PTG_GS: @@ -64,10 +64,9 @@ void AcsController::performControlOperation() { copySusData(); } } - sif::debug << susData.sus0.value[0] << "," << susData.sus0.value[1] << "," - << susData.sus0.value[2] << "," << susData.sus0.value[3] << "," - << susData.sus0.value[4] << "," << susData.sus0.value[5] << std::endl; - sif::debug << susData.sus0.isValid() << std::endl; + + mode = MODE_ON; + submode = SUBMODE_DETUMBLE; } void AcsController::performSafe() {} @@ -82,7 +81,7 @@ void AcsController::performDetumble() { timeval now; // = {0,0}; Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(&susData, now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); double magMomMtq[3] = {0, 0, 0}; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 39f2c3c6..c342dabc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -16,11 +16,11 @@ public: AcsParameters(); virtual ~AcsParameters(); - +/* virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues, uint16_t startAtIndex); - +*/ struct OnBoardParams { double sampleTime = 0.1; // [s] } onBoardParams; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 933800e2..c5db92ad 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -144,29 +144,29 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const return true; } -void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, +void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, double *sunVectorDerivative, bool *sunVectorDerivativeValid) { - susData.sus0.setValid(susConverter.checkSunSensorData(susData.sus0)); - susData.sus1.setValid(susConverter.checkSunSensorData(susData.sus1)); - susData.sus2.setValid(susConverter.checkSunSensorData(susData.sus2)); - susData.sus3.setValid(susConverter.checkSunSensorData(susData.sus3)); - susData.sus4.setValid(susConverter.checkSunSensorData(susData.sus4)); - susData.sus5.setValid(susConverter.checkSunSensorData(susData.sus5)); - susData.sus6.setValid(susConverter.checkSunSensorData(susData.sus6)); - susData.sus7.setValid(susConverter.checkSunSensorData(susData.sus7)); - susData.sus8.setValid(susConverter.checkSunSensorData(susData.sus8)); - susData.sus9.setValid(susConverter.checkSunSensorData(susData.sus9)); - susData.sus10.setValid(susConverter.checkSunSensorData(susData.sus10)); - susData.sus11.setValid(susConverter.checkSunSensorData(susData.sus11)); + susData->sus0.setValid(susConverter.checkSunSensorData(susData->sus0)); + susData->sus1.setValid(susConverter.checkSunSensorData(susData->sus1)); + susData->sus2.setValid(susConverter.checkSunSensorData(susData->sus2)); + susData->sus3.setValid(susConverter.checkSunSensorData(susData->sus3)); + susData->sus4.setValid(susConverter.checkSunSensorData(susData->sus4)); + susData->sus5.setValid(susConverter.checkSunSensorData(susData->sus5)); + susData->sus6.setValid(susConverter.checkSunSensorData(susData->sus6)); + susData->sus7.setValid(susConverter.checkSunSensorData(susData->sus7)); + susData->sus8.setValid(susConverter.checkSunSensorData(susData->sus8)); + susData->sus9.setValid(susConverter.checkSunSensorData(susData->sus9)); + susData->sus10.setValid(susConverter.checkSunSensorData(susData->sus10)); + susData->sus11.setValid(susConverter.checkSunSensorData(susData->sus11)); - if (!susData.sus0.isValid() && !susData.sus1.isValid() && !susData.sus2.isValid() && - !susData.sus3.isValid() && !susData.sus4.isValid() && !susData.sus5.isValid() && - !susData.sus6.isValid() && !susData.sus7.isValid() && !susData.sus8.isValid() && - !susData.sus9.isValid() && !susData.sus10.isValid() && !susData.sus11.isValid()) { + if (!susData->sus0.isValid() && !susData->sus1.isValid() && !susData->sus2.isValid() && + !susData->sus3.isValid() && !susData->sus4.isValid() && !susData->sus5.isValid() && + !susData->sus6.isValid() && !susData->sus7.isValid() && !susData->sus8.isValid() && + !susData->sus9.isValid() && !susData->sus10.isValid() && !susData->sus11.isValid()) { *sunDirEstValid = false; return; } else { @@ -177,97 +177,97 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSus sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - if (susData.sus0.isValid()) { + if (susData->sus0.isValid()) { MatrixOperations::multiply( susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus0, susParameters->sus0coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus0, susParameters->sus0coeffAlpha, susParameters->sus0coeffBeta), sus0VecBody, 3, 3, 1); } - if (susData.sus1.isValid()) { + if (susData->sus1.isValid()) { MatrixOperations::multiply( susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus1, susParameters->sus1coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus1, susParameters->sus1coeffAlpha, susParameters->sus1coeffBeta), sus1VecBody, 3, 3, 1); } - if (susData.sus2.isValid()) { + if (susData->sus2.isValid()) { MatrixOperations::multiply( susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus2, susParameters->sus2coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus2, susParameters->sus2coeffAlpha, susParameters->sus2coeffBeta), sus2VecBody, 3, 3, 1); } - if (susData.sus3.isValid()) { + if (susData->sus3.isValid()) { MatrixOperations::multiply( susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus3, susParameters->sus3coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus3, susParameters->sus3coeffAlpha, susParameters->sus3coeffBeta), sus3VecBody, 3, 3, 1); } - if (susData.sus4.isValid()) { + if (susData->sus4.isValid()) { MatrixOperations::multiply( susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus4, susParameters->sus4coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus4, susParameters->sus4coeffAlpha, susParameters->sus4coeffBeta), sus4VecBody, 3, 3, 1); } - if (susData.sus5.isValid()) { + if (susData->sus5.isValid()) { MatrixOperations::multiply( susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus5, susParameters->sus5coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus5, susParameters->sus5coeffAlpha, susParameters->sus5coeffBeta), sus5VecBody, 3, 3, 1); } - if (susData.sus6.isValid()) { + if (susData->sus6.isValid()) { MatrixOperations::multiply( susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus6, susParameters->sus6coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus6, susParameters->sus6coeffAlpha, susParameters->sus6coeffBeta), sus6VecBody, 3, 3, 1); } - if (susData.sus7.isValid()) { + if (susData->sus7.isValid()) { MatrixOperations::multiply( susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus7, susParameters->sus7coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus7, susParameters->sus7coeffAlpha, susParameters->sus7coeffBeta), sus7VecBody, 3, 3, 1); } - if (susData.sus8.isValid()) { + if (susData->sus8.isValid()) { MatrixOperations::multiply( susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus8, susParameters->sus8coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus8, susParameters->sus8coeffAlpha, susParameters->sus8coeffBeta), sus8VecBody, 3, 3, 1); } - if (susData.sus9.isValid()) { + if (susData->sus9.isValid()) { MatrixOperations::multiply( susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus9, susParameters->sus9coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus9, susParameters->sus9coeffAlpha, susParameters->sus9coeffBeta), sus9VecBody, 3, 3, 1); } - if (susData.sus10.isValid()) { + if (susData->sus10.isValid()) { MatrixOperations::multiply( susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus10, susParameters->sus10coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus10, susParameters->sus10coeffAlpha, susParameters->sus10coeffBeta), sus10VecBody, 3, 3, 1); } - if (susData.sus11.isValid()) { + if (susData->sus11.isValid()) { MatrixOperations::multiply( susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus11, susParameters->sus11coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus11, susParameters->sus11coeffAlpha, susParameters->sus11coeffBeta), sus11VecBody, 3, 3, 1); } /* ------ Mean Value: susDirEst ------ */ - // Timo already done - bool validIds[12] = {susData.sus0.isValid(), susData.sus1.isValid(), susData.sus2.isValid(), - susData.sus3.isValid(), susData.sus4.isValid(), susData.sus5.isValid(), - susData.sus6.isValid(), susData.sus7.isValid(), susData.sus8.isValid(), - susData.sus9.isValid(), susData.sus10.isValid(), susData.sus11.isValid()}; + bool validIds[12] = { + susData->sus0.isValid(), susData->sus1.isValid(), susData->sus2.isValid(), + susData->sus3.isValid(), susData->sus4.isValid(), susData->sus5.isValid(), + susData->sus6.isValid(), susData->sus7.isValid(), susData->sus8.isValid(), + susData->sus9.isValid(), susData->sus10.isValid(), susData->sus11.isValid()}; float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, @@ -279,7 +279,7 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSus sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; double susMeanValue[3] = {0, 0, 0}; - uint8_t validSusCounter = 0; + float validSusCounter = 0; for (uint8_t i = 0; i < 12; i++) { if (validIds[i]) { susMeanValue[0] += susVecBody[0][i]; @@ -407,40 +407,35 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo } } -void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, - ACS::OutputValues *outputValues, +void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now, + ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { - sensorValues->update(); - processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, - &outputValues->gcLatitude, &outputValues->gdLongitude); + // sensorValues->update(); + // processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, + // &outputValues->gcLatitude, &outputValues->gdLongitude); - /*outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, - sensorValues->mgm1, sensorValues->mgm1Valid, - sensorValues->mgm2, sensorValues->mgm2Valid, - sensorValues->mgm3, sensorValues->mgm3Valid, - sensorValues->mgm4, sensorValues->mgm4Valid, now, - &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, - outputValues->gdLongitude, sensorValues->gps0altitude, - sensorValues->gps0Valid, - outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid, - outputValues->magneticFieldVectorDerivative, - &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? + // outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, + // sensorValues->mgm1, sensorValues->mgm1Valid, + // sensorValues->mgm2, sensorValues->mgm2Valid, + // sensorValues->mgm3, sensorValues->mgm3Valid, + // sensorValues->mgm4, sensorValues->mgm4Valid, now, + // &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, + // outputValues->gdLongitude, sensorValues->gps0altitude, + // sensorValues->gps0Valid, + // outputValues->magFieldEst, &outputValues->magFieldEstValid, + // outputValues->magFieldModel, &outputValues->magFieldModelValid, + // outputValues->magneticFieldVectorDerivative, + // &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? - processSus(sensorValues->sus0, sensorValues->sus0Valid, sensorValues->sus1, - sensorValues->sus1Valid, sensorValues->sus2, sensorValues->sus2Valid, sensorValues->sus3, - sensorValues->sus3Valid, sensorValues->sus4, sensorValues->sus4Valid, sensorValues->sus5, - sensorValues->sus5Valid, sensorValues->sus6, sensorValues->sus6Valid, sensorValues->sus7, - sensorValues->sus7Valid, sensorValues->sus8, sensorValues->sus8Valid, sensorValues->sus9, - sensorValues->sus9Valid, sensorValues->sus10, sensorValues->sus10Valid, sensorValues->sus11, - sensorValues->sus11Valid, now, &acsParameters->susHandlingParameters, - &acsParameters->sunModelParameters, outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // - VALID outputs ? -*/ - processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, - sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now, - &acsParameters->rmuHandlingParameters, outputValues->satRateEst, - &outputValues->satRateEstValid); + processSus(susData, now, &acsParameters->susHandlingParameters, + &acsParameters->sunModelParameters, outputValues->sunDirEst, + &outputValues->sunDirEstValid, outputValues->sunDirModel, + &outputValues->sunDirModelValid, outputValues->sunVectorDerivative, + &outputValues->sunVectorDerivativeValid); + // VALID outputs ? + + // processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, + // sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now, + // &acsParameters->rmuHandlingParameters, outputValues->satRateEst, + // &outputValues->satRateEstValid); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index baa9c5b6..bde4f34e 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -9,11 +9,11 @@ #include //uint8_t #include /*purpose, timeval ?*/ -#include "SusConverter.h" #include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" #include "OutputValues.h" #include "SensorValues.h" +#include "SusConverter.h" #include "config/classIds.h" /*Planned: @@ -35,7 +35,8 @@ class SensorProcessing { SensorProcessing(AcsParameters *acsParameters_); virtual ~SensorProcessing(); - void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + void process(acsctrl::SusDataRaw *susData, timeval now, ACS::SensorValues *sensorValues, + ACS::OutputValues *outputValues, const AcsParameters *acsParameters); // Will call protected functions private: protected: @@ -49,7 +50,7 @@ class SensorProcessing { bool *magFieldModelValid, double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid); // Output - void processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, + void processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, From d9f2dfa72505defad9950314bfa0bf349ad2e232 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 15:58:51 +0200 Subject: [PATCH 044/244] fixed warnings --- 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 a6a91a81..7d9cd8df 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -73,7 +73,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth; // TEST SECTION ! - double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + //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); @@ -309,7 +309,7 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double * rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2]; } - else if ((sensorValues->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && (sensorValues->validRw3)) { + else if ((sensorValues->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && !(sensorValues->validRw3)) { rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0]; rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1]; From f0fec11ad7fd289f2368054601b42c9a0ee05deb Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 16:00:31 +0200 Subject: [PATCH 045/244] set channel output valid flag to true --- dummies/SusDummy.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 0c2feaac..d209368b 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -42,12 +42,12 @@ void SusDummy::performControlOperation() { value = sin(iteration / 80. * M_PI + 10) * 10 - 10; susSet.read(); - susSet.temperatureCelcius = value; - if ((iteration % 100) < 20) { - susSet.setValidity(false, true); - } else { - susSet.setValidity(true, true); - } +// susSet.temperatureCelcius = value; +// if ((iteration % 100) < 20) { +// susSet.setValidity(false, true); +// } else { +// susSet.setValidity(true, true); +// } susSet.channels[0] = 3913; susSet.channels[1] = 3912; susSet.channels[2] = 3799; @@ -60,7 +60,7 @@ ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPo LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, - new PoolEntry({0, 0, 0, 0, 0, 0})); + new PoolEntry({0, 0, 0, 0, 0, 0},true)); return returnvalue::OK; } From 82e9c6d092bca2e7c9edd6202f3551b559bba412 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 16:02:17 +0200 Subject: [PATCH 046/244] comments --- mission/controller/AcsController.cpp | 3 +++ mission/controller/acs/AcsParameters.h | 3 +-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 91d38eff..785a6906 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -65,8 +65,11 @@ void AcsController::performControlOperation() { } } + + // DEBUG : REMOVE AFTER COMPLETION mode = MODE_ON; submode = SUBMODE_DETUMBLE; + // DEBUG END } void AcsController::performSafe() {} diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index c342dabc..85ceaec0 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -892,10 +892,9 @@ public: float omega_0 = 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 - // ToDo: check correct assignment of e and e1. Both were assigned to e before 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 float p2 = 72. / 3600. * M_PI / 180.; //some parameter } sunModelParameters; From 076e67dd0bade52bfa0c7d8e2a8c5653b8fe8bad Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 16:02:57 +0200 Subject: [PATCH 047/244] switched sensor input from AcsController to SensorValues --- mission/controller/acs/SensorProcessing.cpp | 179 ++++++++++++-------- mission/controller/acs/SensorProcessing.h | 22 +-- mission/controller/acs/SensorValues.cpp | 57 +++++-- mission/controller/acs/SensorValues.h | 142 +++++++--------- mission/controller/acs/SusConverter.cpp | 39 ++--- mission/controller/acs/SusConverter.h | 8 +- 6 files changed, 241 insertions(+), 206 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c5db92ad..749ec373 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -19,8 +19,6 @@ #include "util/MathOperations.h" using namespace Math; -// Thought: Maybe separate file for transforming of sensor values -// into geometry frame and body frame SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} { validMagField = false; @@ -144,29 +142,56 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const return true; } -void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement, - const AcsParameters::SusHandlingParameters *susParameters, - const AcsParameters::SunModelParameters *sunModelParameters, - double *sunDirEst, bool *sunDirEstValid, - double *sunVectorInertial, bool *sunVectorInertialValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid) { - susData->sus0.setValid(susConverter.checkSunSensorData(susData->sus0)); - susData->sus1.setValid(susConverter.checkSunSensorData(susData->sus1)); - susData->sus2.setValid(susConverter.checkSunSensorData(susData->sus2)); - susData->sus3.setValid(susConverter.checkSunSensorData(susData->sus3)); - susData->sus4.setValid(susConverter.checkSunSensorData(susData->sus4)); - susData->sus5.setValid(susConverter.checkSunSensorData(susData->sus5)); - susData->sus6.setValid(susConverter.checkSunSensorData(susData->sus6)); - susData->sus7.setValid(susConverter.checkSunSensorData(susData->sus7)); - susData->sus8.setValid(susConverter.checkSunSensorData(susData->sus8)); - susData->sus9.setValid(susConverter.checkSunSensorData(susData->sus9)); - susData->sus10.setValid(susConverter.checkSunSensorData(susData->sus10)); - susData->sus11.setValid(susConverter.checkSunSensorData(susData->sus11)); +void SensorProcessing::processSus( + const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid, + const uint16_t *sus2Value, bool sus2valid, const uint16_t *sus3Value, bool sus3valid, + const uint16_t *sus4Value, bool sus4valid, const uint16_t *sus5Value, bool sus5valid, + const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid, + const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid, + const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid, + timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, + bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, + double *sunVectorDerivative, bool *sunVectorDerivativeValid) { + if (sus0valid) { + sus0valid = susConverter.checkSunSensorData(sus0Value); + } + if (sus1valid) { + sus1valid = susConverter.checkSunSensorData(sus1Value); + } + if (sus2valid) { + sus2valid = susConverter.checkSunSensorData(sus2Value); + } + if (sus3valid) { + sus3valid = susConverter.checkSunSensorData(sus3Value); + } + if (sus4valid) { + sus4valid = susConverter.checkSunSensorData(sus4Value); + } + if (sus5valid) { + sus5valid = susConverter.checkSunSensorData(sus5Value); + } + if (sus6valid) { + sus6valid = susConverter.checkSunSensorData(sus6Value); + } + if (sus7valid) { + sus7valid = susConverter.checkSunSensorData(sus7Value); + } + if (sus8valid) { + sus8valid = susConverter.checkSunSensorData(sus8Value); + } + if (sus9valid) { + sus9valid = susConverter.checkSunSensorData(sus9Value); + } + if (sus10valid) { + sus10valid = susConverter.checkSunSensorData(sus10Value); + } + if (sus11valid) { + sus11valid = susConverter.checkSunSensorData(sus11Value); + } - if (!susData->sus0.isValid() && !susData->sus1.isValid() && !susData->sus2.isValid() && - !susData->sus3.isValid() && !susData->sus4.isValid() && !susData->sus5.isValid() && - !susData->sus6.isValid() && !susData->sus7.isValid() && !susData->sus8.isValid() && - !susData->sus9.isValid() && !susData->sus10.isValid() && !susData->sus11.isValid()) { + if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && + !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { *sunDirEstValid = false; return; } else { @@ -177,97 +202,94 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSu sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - if (susData->sus0.isValid()) { + if (sus0valid) { MatrixOperations::multiply( susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus0, susParameters->sus0coeffAlpha, + susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, susParameters->sus0coeffBeta), sus0VecBody, 3, 3, 1); } - if (susData->sus1.isValid()) { + if (sus1valid) { MatrixOperations::multiply( susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus1, susParameters->sus1coeffAlpha, + susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, susParameters->sus1coeffBeta), sus1VecBody, 3, 3, 1); } - if (susData->sus2.isValid()) { + if (sus2valid) { MatrixOperations::multiply( susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus2, susParameters->sus2coeffAlpha, + susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, susParameters->sus2coeffBeta), sus2VecBody, 3, 3, 1); } - if (susData->sus3.isValid()) { + if (sus3valid) { MatrixOperations::multiply( susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus3, susParameters->sus3coeffAlpha, + susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, susParameters->sus3coeffBeta), sus3VecBody, 3, 3, 1); } - if (susData->sus4.isValid()) { + if (sus4valid) { MatrixOperations::multiply( susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus4, susParameters->sus4coeffAlpha, + susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, susParameters->sus4coeffBeta), sus4VecBody, 3, 3, 1); } - if (susData->sus5.isValid()) { + if (sus5valid) { MatrixOperations::multiply( susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus5, susParameters->sus5coeffAlpha, + susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, susParameters->sus5coeffBeta), sus5VecBody, 3, 3, 1); } - if (susData->sus6.isValid()) { + if (sus6valid) { MatrixOperations::multiply( susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus6, susParameters->sus6coeffAlpha, + susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, susParameters->sus6coeffBeta), sus6VecBody, 3, 3, 1); } - if (susData->sus7.isValid()) { + if (sus7valid) { MatrixOperations::multiply( susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus7, susParameters->sus7coeffAlpha, + susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, susParameters->sus7coeffBeta), sus7VecBody, 3, 3, 1); } - if (susData->sus8.isValid()) { + if (sus8valid) { MatrixOperations::multiply( susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus8, susParameters->sus8coeffAlpha, + susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, susParameters->sus8coeffBeta), sus8VecBody, 3, 3, 1); } - if (susData->sus9.isValid()) { + if (sus9valid) { MatrixOperations::multiply( susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus9, susParameters->sus9coeffAlpha, + susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, susParameters->sus9coeffBeta), sus9VecBody, 3, 3, 1); } - if (susData->sus10.isValid()) { + if (sus10valid) { MatrixOperations::multiply( susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus10, susParameters->sus10coeffAlpha, + susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, susParameters->sus10coeffBeta), sus10VecBody, 3, 3, 1); } - if (susData->sus11.isValid()) { + if (sus11valid) { MatrixOperations::multiply( susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus11, susParameters->sus11coeffAlpha, + susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, susParameters->sus11coeffBeta), sus11VecBody, 3, 3, 1); } /* ------ Mean Value: susDirEst ------ */ - bool validIds[12] = { - susData->sus0.isValid(), susData->sus1.isValid(), susData->sus2.isValid(), - susData->sus3.isValid(), susData->sus4.isValid(), susData->sus5.isValid(), - susData->sus6.isValid(), susData->sus7.isValid(), susData->sus8.isValid(), - susData->sus9.isValid(), susData->sus10.isValid(), susData->sus11.isValid()}; + bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, @@ -279,17 +301,14 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSu sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; double susMeanValue[3] = {0, 0, 0}; - float validSusCounter = 0; for (uint8_t i = 0; i < 12; i++) { if (validIds[i]) { susMeanValue[0] += susVecBody[0][i]; susMeanValue[1] += susVecBody[1][i]; susMeanValue[2] += susVecBody[2][i]; - validSusCounter += 1; } } - double divisor = 1 / validSusCounter; - VectorOperations::mulScalar(susMeanValue, divisor, sunDirEst, 3); + VectorOperations::normalize(susMeanValue, sunDirEst, 3); *sunDirEstValid = true; } @@ -410,26 +429,40 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { - // sensorValues->update(); + sensorValues->update(); // processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, // &outputValues->gcLatitude, &outputValues->gdLongitude); - // outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, - // sensorValues->mgm1, sensorValues->mgm1Valid, - // sensorValues->mgm2, sensorValues->mgm2Valid, - // sensorValues->mgm3, sensorValues->mgm3Valid, - // sensorValues->mgm4, sensorValues->mgm4Valid, now, - // &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, - // outputValues->gdLongitude, sensorValues->gps0altitude, - // sensorValues->gps0Valid, - // outputValues->magFieldEst, &outputValues->magFieldEstValid, - // outputValues->magFieldModel, &outputValues->magFieldModelValid, - // outputValues->magneticFieldVectorDerivative, - // &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? + outputValues->mgmUpdated = processMgm( + sensorValues->mgm0Lis3Set.fieldStrengths.value, + sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), + sensorValues->mgm1Rm3100Set.fieldStrengths.value, + sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(), + sensorValues->mgm2Lis3Set.fieldStrengths.value, + sensorValues->mgm2Lis3Set.fieldStrengths.isValid(), + sensorValues->mgm3Rm3100Set.fieldStrengths.value, + sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, + sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters, + outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gps0altitude, + sensorValues->gps0Valid, outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid, + outputValues->magneticFieldVectorDerivative, + &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? - processSus(susData, now, &acsParameters->susHandlingParameters, - &acsParameters->sunModelParameters, outputValues->sunDirEst, - &outputValues->sunDirEstValid, outputValues->sunDirModel, + processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), + sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), + sensorValues->susSets[2].channels.value, sensorValues->susSets[2].channels.isValid(), + sensorValues->susSets[3].channels.value, sensorValues->susSets[3].channels.isValid(), + sensorValues->susSets[4].channels.value, sensorValues->susSets[4].channels.isValid(), + sensorValues->susSets[5].channels.value, sensorValues->susSets[5].channels.isValid(), + sensorValues->susSets[6].channels.value, sensorValues->susSets[6].channels.isValid(), + sensorValues->susSets[7].channels.value, sensorValues->susSets[7].channels.isValid(), + sensorValues->susSets[8].channels.value, sensorValues->susSets[8].channels.isValid(), + sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(), + sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(), + sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(), + now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, + outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // VALID outputs ? diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index bde4f34e..df2b2a37 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -16,18 +16,6 @@ #include "SusConverter.h" #include "config/classIds.h" -/*Planned: - * - Fusion of Sensor Measurements - - * sunDirEst (mean value) - * magField (mean value) - * rmuSatRate (rmus, mean value) - * - Models to get inertia values - - * sunModelDir (input: time) - * magModelField (input: position,time) - * - Low Pass Filter maybe - - * magField - * SunDirEst*/ - class SensorProcessing { public: void reset(); @@ -50,7 +38,15 @@ class SensorProcessing { bool *magFieldModelValid, double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid); // Output - void processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement, + void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, + bool sus1valid, const uint16_t *sus2Value, bool sus2valid, + const uint16_t *sus3Value, bool sus3valid, const uint16_t *sus4Value, + bool sus4valid, const uint16_t *sus5Value, bool sus5valid, + const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, + bool sus7valid, const uint16_t *sus8Value, bool sus8valid, + const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value, + bool sus10valid, const uint16_t *sus11Value, bool sus11valid, + timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 11720aeb..9ca2b90b 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -5,30 +5,57 @@ * Author: rooob */ #include "SensorValues.h" -#include -#include + +#include #include +#include +#include + +#include namespace ACS { -SensorValues::SensorValues() { +SensorValues::SensorValues() {} + +SensorValues::~SensorValues() {} + +ReturnValue_t SensorValues::updateMgm() { + ReturnValue_t result; + PoolReadGuard pgMgm0(&mgm0Lis3Set), pgMgm1(&mgm1Rm3100Set), pgMgm2(&mgm2Lis3Set), + pgMgm3(&mgm3Rm3100Set), pgImtq(&imtqMgmSet); + + result = (pgMgm0.getReadResult() || pgMgm1.getReadResult() || pgMgm2.getReadResult() || + pgMgm3.getReadResult() || pgImtq.getReadResult()); + return result; } -SensorValues::~SensorValues() { +ReturnValue_t SensorValues::updateSus() { + ReturnValue_t result; + PoolReadGuard pgSus0(&susSets[0]), pgSus1(&susSets[1]), pgSus2(&susSets[2]), pgSus3(&susSets[3]), + pgSus4(&susSets[4]), pgSus5(&susSets[5]), pgSus6(&susSets[6]), pgSus7(&susSets[7]), + pgSus8(&susSets[8]), pgSus9(&susSets[9]), pgSus10(&susSets[10]), pgSus11(&susSets[11]); + + result = (pgSus0.getReadResult() || pgSus1.getReadResult() || pgSus2.getReadResult() || + pgSus3.getReadResult() || pgSus4.getReadResult() || pgSus5.getReadResult() || + pgSus6.getReadResult() || pgSus7.getReadResult() || pgSus8.getReadResult() || + pgSus9.getReadResult() || pgSus10.getReadResult() || pgSus11.getReadResult()); + return result; } ReturnValue_t SensorValues::update() { -// lp_var_t quaternion(objects::STAR_TRACKER, PoolIds::CALI_QW, nullptr, pool_rwm_t::VAR_READ); -// ReturnValue_t result = quaternion.read(); + updateSus(); + updateMgm(); -// if ( result != RETURN_OK) { -// return RETURN_FAILED; -// } -// quatJB[3] = static_cast(quaternion.value); -// quatJBValid = quaternion.isValid(); + // lp_var_t quaternion(objects::STAR_TRACKER, PoolIds::CALI_QW, nullptr, + // pool_rwm_t::VAR_READ); + // ReturnValue_t result = quaternion.read(); + // + // if (result != RETURN_OK) { + // return RETURN_FAILED; + // } + // quatJB[3] = static_cast(quaternion.value); + // quatJBValid = quaternion.isValid(); - return returnvalue::OK; + return returnvalue::OK; } -} - - +} // namespace ACS diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 001b141b..daf4913c 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,99 +1,85 @@ -/* Created on: 08.03.2022 - * Author: Robin - */ - #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ -#include + +#include +#include "mission/devices/devicedefinitions/SusDefinitions.h" +#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" +#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h" namespace ACS { class SensorValues{ -public: - SensorValues(); - virtual ~SensorValues(); + public: + SensorValues(); + virtual ~SensorValues(); - ReturnValue_t update(); + ReturnValue_t update(); + ReturnValue_t updateMgm(); + ReturnValue_t updateSus(); - float mgm0[3]; - float mgm1[3]; - float mgm2[3]; - float mgm3[3]; - float mgm4[3]; + MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = + MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); + RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = + RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); + MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = + MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); + RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = + RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); + IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); - bool mgm0Valid; - bool mgm1Valid; - bool mgm2Valid; - bool mgm3Valid; - bool mgm4Valid; + std::array susSets{ + SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), + SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), + SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), + SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), + SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF), + SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB), + SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF), + SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB), + SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB), + SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF), + SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), + SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), + }; - float sus0[3]; - float sus1[3]; - float sus2[3]; - float sus3[3]; - float sus4[3]; - float sus5[3]; - float sus6[3]; - float sus7[3]; - float sus8[3]; - float sus9[3]; - float sus10[3]; - float sus11[3]; + double rmu0[3]; + double rmu1[3]; + double rmu2[3]; - bool sus0Valid; - bool sus1Valid; - bool sus2Valid; - bool sus3Valid; - bool sus4Valid; - bool sus5Valid; - bool sus6Valid; - bool sus7Valid; - bool sus8Valid; - bool sus9Valid; - bool sus10Valid; - bool sus11Valid; + bool rmu0Valid; + bool rmu1Valid; + bool rmu2Valid; + double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? + bool quatJBValid; + int strIntTime[2]; - double rmu0[3]; - double rmu1[3]; - double rmu2[3]; + double gps0latitude; // Reference is WGS84, so this one will probably be geodetic + double gps0longitude; // Should be geocentric for IGRF + double gps0altitude; + double gps0Velocity[3]; // speed over ground = ?? + double gps0Time; // utc - bool rmu0Valid; - bool rmu1Valid; - bool rmu2Valid; + // valid ids for gps values ! + int gps0TimeYear; + int gps0TimeMonth; + int gps0TimeHour; // should be double + bool gps0Valid; - double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? - bool quatJBValid; - int strIntTime[2]; - - double gps0latitude; //Reference is WGS84, so this one will probably be geodetic - double gps0longitude; //Should be geocentric for IGRF - double gps0altitude; - double gps0Velocity[3]; // speed over ground = ?? - double gps0Time; //utc - -// valid ids for gps values ! - int gps0TimeYear; - int gps0TimeMonth; - int gps0TimeHour; // should be double - bool gps0Valid; - - - bool mgt0valid; - -// Reaction wheel measurements - double speedRw0; // RPM [1/min] - double speedRw1; // RPM [1/min] - double speedRw2; // RPM [1/min] - double speedRw3; // RPM [1/min] - bool validRw0; - bool validRw1; - bool validRw2; - bool validRw3; + bool mgt0valid; + // Reaction wheel measurements + double speedRw0; // RPM [1/min] + double speedRw1; // RPM [1/min] + double speedRw2; // RPM [1/min] + double speedRw3; // RPM [1/min] + bool validRw0; + bool validRw1; + bool validRw2; + bool validRw3; }; } /* namespace ACS */ #endif /*ENSORVALUES_H_*/ - diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index ea52f5a2..26804f12 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -14,30 +14,26 @@ #include -bool SusConverter::checkSunSensorData(lp_vec_t susChannel) { - if (susChannel.value[0] <= susChannelValueCheckLow || - susChannel.value[0] > susChannelValueCheckHigh || - susChannel.value[0] > susChannel.value[GNDREF]) { +bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { + if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh || + susChannel[0] > susChannel[GNDREF]) { return false; } - if (susChannel.value[1] <= susChannelValueCheckLow || - susChannel.value[1] > susChannelValueCheckHigh || - susChannel.value[1] > susChannel.value[GNDREF]) { + if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh || + susChannel[1] > susChannel[GNDREF]) { return false; }; - if (susChannel.value[2] <= susChannelValueCheckLow || - susChannel.value[2] > susChannelValueCheckHigh || - susChannel.value[2] > susChannel.value[GNDREF]) { + if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh || + susChannel[2] > susChannel[GNDREF]) { return false; }; - if (susChannel.value[3] <= susChannelValueCheckLow || - susChannel.value[3] > susChannelValueCheckHigh || - susChannel.value[3] > susChannel.value[GNDREF]) { + if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh || + susChannel[3] > susChannel[GNDREF]) { return false; }; - susChannelValueSum = 4 * susChannel.value[GNDREF] - (susChannel.value[0] + susChannel.value[1] + - susChannel.value[2] + susChannel.value[3]); + susChannelValueSum = + 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); if ((susChannelValueSum < susChannelValueSumHigh) && (susChannelValueSum > susChannelValueSumLow)) { return false; @@ -45,17 +41,17 @@ bool SusConverter::checkSunSensorData(lp_vec_t susChannel) { return true; } -void SusConverter::calcAngle(lp_vec_t susChannel) { +void SusConverter::calcAngle(const uint16_t susChannel[6]) { float xout, yout; float s = 0.03; // s=[mm] gap between diodes uint8_t d = 5; // d=[mm] edge length of the quadratic aperture uint8_t h = 1; // h=[mm] distance between diodes and aperture int ch0, ch1, ch2, ch3; // Substract measurement values from GNDREF zero current threshold - ch0 = susChannel.value[GNDREF] - susChannel.value[0]; - ch1 = susChannel.value[GNDREF] - susChannel.value[1]; - ch2 = susChannel.value[GNDREF] - susChannel.value[2]; - ch3 = susChannel.value[GNDREF] - susChannel.value[3]; + ch0 = susChannel[GNDREF] - susChannel[0]; + ch1 = susChannel[GNDREF] - susChannel[1]; + ch2 = susChannel[GNDREF] - susChannel[2]; + ch3 = susChannel[GNDREF] - susChannel[3]; // Calculation of x and y xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] @@ -124,11 +120,10 @@ float* SusConverter::calculateSunVector() { return sunVectorBodyFrame; } -float* SusConverter::getSunVectorSensorFrame(lp_vec_t susChannel, +float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10], const float coeffBeta[9][10]) { calcAngle(susChannel); calibration(coeffAlpha, coeffBeta); return calculateSunVector(); } - diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 32f9e954..01143a39 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -17,16 +17,14 @@ class SusConverter { public: SusConverter() {} - bool checkSunSensorData(lp_vec_t susChannel); + bool checkSunSensorData(const uint16_t susChannel[6]); - void calcAngle(lp_vec_t susChannel); + void calcAngle(const uint16_t susChannel[6]); void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]); float* calculateSunVector(); - bool getValidFlag(uint8_t susNumber); - float* getSunVectorSensorFrame(lp_vec_t susChannel, const float coeffAlpha[9][10], + float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10], const float coeffBeta[9][10]); - float* TransferSunVector(); private: float alphaBetaRaw[2]; //[°] From c8d91dce7d31ef107efb44735b11e0c15f3eab2c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 11 Oct 2022 14:56:18 +0200 Subject: [PATCH 048/244] format fix. added GYR orientation matrices --- mission/controller/acs/AcsParameters.h | 1734 +++++++++++------------- 1 file changed, 828 insertions(+), 906 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 85ceaec0..29caa59e 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -6,947 +6,869 @@ #ifndef ACSPARAMETERS_H_ #define ACSPARAMETERS_H_ -#include #include +#include + typedef unsigned char uint8_t; -class AcsParameters /*: public HasParametersIF*/{ -public: +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); + */ + struct OnBoardParams { + double sampleTime = 0.1; // [s] + } onBoardParams; - AcsParameters(); - virtual ~AcsParameters(); -/* - virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, - ParameterWrapper *parameterWrapper, - const ParameterWrapper *newValues, uint16_t startAtIndex); -*/ - struct OnBoardParams { - double sampleTime = 0.1; // [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]; + } inertiaEIVE; - 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]; - } 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 mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; + } mgmHandlingParameters; - 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 mgm4orientationMatrix[3][3] = { - { 0, 0,-1}, - {-1, 0, 0}, - { 0, 1, 0}}; - } mgmHandlingParameters; + struct SusHandlingParameters { + float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}}; // FM07 + 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 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 + float sus10orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}}; // FM09 + float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}}; // FM08 - struct SusHandlingParameters { - float sus0orientationMatrix[3][3] = { - { 0, 0, 1}, - { 1, 0, 0}, - { 0, 1, 0}}; // FM07 - 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 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 - float sus10orientationMatrix[3][3] = { - { 0,-1, 0}, - { 1, 0, 0}, - { 0, 0, 1}}; // FM09 - 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}}; + 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}}; + float sus1coeffAlpha[9][10] = { + {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, + -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, + -0.000178087038629721, 4.09208968678946e-05}, + {2.24803085641869, 1.42938692406645, 0.30104994020693, 0.00756499999397385, + 0.0117765927439368, -0.000743685980641362, 4.69920803836194e-05, 0.000129815636957956, + -9.10792250542345e-06, -2.03870119873411e-05}, + {26.9943033817917, 0.147791175366868, -3.48256070200564, -0.0303332422478656, + 0.0183377266255394, 0.124593616125966, -0.000466003049304431, -0.000272000698791331, + -0.00063621309529853, -0.00158363678978767}, + {-0.221893380318465, 1.29919955307083, 0.21872487901019, 0.0049448219667127, + 0.00291224091529189, 0.00654651987282984, -9.86842469311185e-05, 8.20057454706638e-05, + 6.42331081725944e-05, 7.11656918299053e-05}, + {1.40178843964621, 1.1733111455249, 0.287485528779234, -0.000793970428759834, + 0.000170529273905818, -0.00268807864923086, 9.09553964483881e-05, -0.000271892733575409, + 8.52016306311741e-05, -0.000291797625433646}, + {0.65549617899457, 1.25716478394514, 0.301396415134214, -0.00357289640403958, + -0.000473416364133431, -0.010760332636205, -9.77220176481185e-05, 4.40798040046875e-05, + 2.84958344955681e-05, 0.000128583400693359}, + {6.20958048145025, 1.9528406481596, 1.32915657614139, -0.0326944423378284, + -0.0158258335207969, 0.0328249756354635, 0.00027113042931131, -0.000133980867173428, + -0.000357964552318811, 0.000224235061786191}, + {2.46222812180944, 1.1731834908026, 0.17440330925151, -0.00132279581980401, + -0.00447202005426964, -0.000804321602550913, -1.59526570766446e-05, 2.62946483533391e-05, + 3.28466749016414e-05, -6.63837547601294e-06}, + {42.615758859473, 2.46617281707273, -5.742515881283, -0.131942799763164, 0.20250702826603, + 0.0981562802911027, 0.00189939440077981, -0.0018591621618441, -0.00161121179693977, + -0.00058814458116749}}; + float sus1coeffBeta[9][10] = { + {-12.300032617206, -1.06640894101328, 0.33950802247214, -0.00890867870617722, + -0.04872758086642, -0.0114263851027856, 0.000141061196404012, -0.000675469545483099, + -0.000138249928781575, -0.000138871036200597}, + {10.1631114109768, 0.261654603839785, 1.2376413405181, 0.00888558138614535, + 0.00151674939001532, -0.00534577602313027, 9.19430013005559e-05, 5.39804599087081e-05, + -4.15760162347772e-05, -7.60797902457032e-05}, + {-30.142329062199, 1.26939195100229, 6.14467186367471, 0.0464163689935328, + 0.00379001947505376, -0.165444163648109, 0.000516545385538741, 1.56053219154647e-05, + -5.58651971370719e-05, 0.00173185063955313}, + {12.1454103989862, -0.243589095509132, 2.02543716988677, -0.000857989774598331, + -0.00705278543432513, 0.0250580538307654, 3.50683653081847e-05, -2.63093897408875e-05, + -5.67352645830913e-05, 0.000232270832022029}, + {4.4338108906594, -0.305276965994378, 1.17293558142526, 0.000152618994429577, + 0.00134432642920902, -0.00104036813342885, 0.000334476082056995, 6.74826804343671e-05, + 0.000275311897725414, 7.58157740577916e-05}, + {3.47680700379043, -0.154163381023597, 1.389579838768, 0.000799705880026268, + 0.00401980026462874, -0.00915311817354667, -2.54817301605075e-06, -2.27422984169921e-05, + -2.61224817848938e-05, 6.00381132540332e-05}, + {29.469181543703, -0.722888948550437, 3.3623377135197, 0.00148445490093232, + -0.0474780142430845, 0.0486755575785462, 0.000126295091963757, 0.000526632230895258, + -0.000259305985126003, 0.000412751148048724}, + {2.67029041722834, -0.0837968038501666, 1.37628504937018, 0.00165061312885753, + -0.00953813055064273, 0.0032433005486936, -1.6522452172598e-05, 0.000144574078261271, + -8.47348746872376e-05, -1.92509604512729e-06}, + {-20.959201441285, -2.23605897639125, 5.73044624806043, 0.0354141964763815, + 0.0887545371234514, -0.193862330062381, 0.000216532998121618, -0.00207707610520973, + 0.000552928905346826, 0.00190182163597828}}; + float sus2coeffAlpha[9][10] = { + {6.51602979328333, 0.690575501042577, 1.18185457002269, -0.0153161662266588, + 0.00145972227341484, 0.0351496474730776, -0.000172645571366945, -6.04213053580018e-05, + 9.74494676304114e-05, 0.000334122888261002}, + {0.954398509323963, 1.10996214782069, 0.313314231563221, -0.00367553051112208, + 0.0110290193380194, 0.000240079475656232, -6.93444423181303e-05, 0.000107433381295167, + 1.30750132315838e-05, -2.43580795300515e-05}, + {-55.1159841655056, -1.47449655191106, 3.40106264596874, -0.0621428271456258, + 0.0659788065633613, -0.0791732068323335, -0.000524264070592741, 0.000582093651418709, + -0.000586102213707195, 0.000658133691098817}, + {1.98614148820353, 1.32058724763677, 0.156843003413303, 0.002748082456053, + 0.00202677073171519, 0.00382360695862248, -0.000122364309010211, 5.33354637965168e-05, + 3.93641210098335e-05, 4.06398431916703e-05}, + {3.41223117010734, 1.1597568029329, 0.31881674291653, -0.000382400010917784, + -0.000754945672515052, -0.00079200882313927, 0.000145713118224563, -0.00026910957285589, + 0.000137876961532787, -0.000326798596746712}, + {6.23333031852853, 1.24902998148103, -0.0162317540018123, -0.00338184464699201, + 0.000420329743164687, 0.00202038442335185, -7.10435889754986e-05, -6.04039458988991e-06, + 7.25318569569788e-06, -2.5930447720704e-05}, + {191.759784636909, -10.5228276216193, 8.48306234734519, 0.243240262512846, + -0.344226468125615, 0.126267158197535, -0.00186612281541009, 0.00304415728817747, + -0.00304958575196089, 0.000457236034569107}, + {5.61375025356727, 1.1692295110657, 0.224665256727786, -0.00230481633344849, + -0.00746693012026367, -0.00172583925345173, -7.00823444553058e-06, 7.31362778266959e-05, + 5.81988007269583e-05, 1.3723604109425e-05}, + {98.0250669452855, -2.18500123986039, -6.68238707939385, 0.000754807832106659, + 0.256133336978808, 0.110826583415768, 0.000457663127670018, -0.00197655629847616, + -0.00254305206375073, -0.000466731538082995}}; + float sus2coeffBeta[9][10] = { + {41.1102358678699, 2.3034699186519, 2.74551448799899, 0.061701310929235, 0.0317074142089495, + 0.0308171492962288, 0.00049453042200054, 0.000519222896270701, 2.85420168881716e-05, + 0.000259197384126413}, + {4.46821725251333, 0.0125273331991983, 1.32640678842532, 0.000543566569079156, + 0.00396616601484022, -0.00488408099728387, -3.05734704054868e-06, 7.3424831303621e-05, + -5.49439160235527e-05, -8.30708110469922e-05}, + {64.773396165255, 2.97057686090134, -1.90770757709096, 0.062747116236773, + -0.077990648565002, 0.0613989204238974, 0.00055512113297293, -0.000347045533958329, + 0.00104059576098392, -0.000348638726253297}, + {3.10352939390402, -0.2376108554276, 1.60523925160222, 0.00116454605680723, + -0.0067958260462381, 0.0136561370875238, 2.59929059167486e-05, 3.33825895937897e-05, + -5.55828531601728e-05, 0.000109833374761172}, + {0.156052891975873, -0.320721597024578, 1.15208488414874, 0.00164743688819939, + 0.000534718891498932, 0.000469870758457642, 0.000308432468885207, 0.00011789470679678, + 0.000292373398965513, 0.000183599033441813}, + {2.84967971406268, -0.21374251183113, 1.09938586447269, 2.34894704600407e-05, + 0.00588345375399262, 0.00296966835738407, 8.42707308834155e-06, 2.81870099202641e-06, + -3.56732787246631e-05, -7.04534663356379e-05}, + {-7.59892007483895, 0.358662160515702, 0.805137646978357, 0.00222144376998348, + 0.0464438387809707, 0.00847551828841782, 3.24805702347551e-05, 5.45500807838332e-05, + 0.000941378089367713, 0.000353137737023192}, + {-4.65367165487109, 0.201306010390421, 1.19135575710523, -0.00692801521395975, + 0.00394118754078443, 0.00426360093528599, 6.297683536736e-05, -7.15794236895102e-05, + -7.47076172176468e-05, -1.94516917836346e-05}, + {-59.5882618930651, 3.84530212586425, 3.50497032358686, -0.116100453177197, + -0.0380997421813177, -0.0581898335691109, 0.00111464935006159, 0.000559313074537689, + 0.000168067749764069, 0.000563224178849256}}; + float sus3coeffAlpha[9][10] = { + {-174.687021034355, -7.53454036765748, -9.33798316371397, -0.18212338430986, + -0.242523652239734, -0.202086838965846, -0.00138648793335223, -0.00225430176012882, + -0.00198887215340364, -0.00160678535160774}, + {6.92009692410602, 1.8192037428209, 0.254908171908415, 0.0179273243472017, + 0.00894059238779664, -0.000436952529644, 0.000138070523903458, 9.22759645920339e-05, + -9.4312261303588e-06, -1.76647897892869e-05}, + {-17.9720006944368, 0.230510201259892, 1.10751755772907, -0.00973621304161327, + 0.0554450499198677, -0.00590970792122449, -2.92393772526592e-05, 0.000444329929586969, + -0.000436055839773919, -9.5869891049503e-05}, + {-4.9880829382985, 1.33627775121504, -0.330382157073243, 0.00306744056311184, + 0.00376353074674973, -0.0107453978169225, -0.00010680477021693, 5.17225535432745e-05, + 7.4423443938376e-05, -0.000107927900087035}, + {0.952867982900728, 1.14513280899596, 0.307744203675505, 0.000404669974219378, + -0.000737988606997615, 0.00120218232577844, 0.000191147653645603, -0.000275058867995882, + 0.000137187356620739, -0.000320202731145004}, + {8.076706574364, 1.31338618710295, -0.334634356394277, -0.00209719438033295, + -0.00381753503582303, 0.0100347823323616, -7.00550548221671e-05, -1.97626956996069e-05, + 7.80079707003333e-05, -8.95904360920744e-05}, + {-82.4748312650249, 8.63074484663009, -0.949295700187556, -0.178618807265278, + 0.130143669167547, 0.0284326533865768, 0.00149831261351137, -0.0011583692969717, + 0.0010560778729661, 0.000635404380970666}, + {3.34457857521978, 1.09242517408071, 0.241722402244944, 0.00381629887587041, + -0.00863580122530851, 0.00137050492069702, -8.91046701171713e-05, 8.44169683308007e-05, + -3.54608413548779e-05, 8.54042677832451e-06}, + {78.1540457908649, -1.30266922193303, -5.33605443700115, 0.0184226131926499, + 0.146629920899062, 0.110698519952472, 6.64041537651749e-05, -0.00120174584530713, + -0.00133177694921411, -0.000796422644338886}}; + float sus3coeffBeta[9][10] = { + {-31.5704266802979, -5.10700699133189, 2.84549700473812, -0.122701561048957, + -0.11257100034746, 0.102120576206517, -0.000796645106694696, -0.00192211266325167, + -4.99981232866237e-05, 0.00104036677004523}, + {-0.734294938181273, -0.0694317595592039, 1.34746975389878, -0.00103465544451119, + 0.00389798465946559, -0.00308561832194191, -2.91843250099708e-06, 7.59634622232999e-05, + -6.54571602919161e-05, -0.000104146832644606}, + {24.2649069708536, 3.08145095664586, 1.88975821636026, 0.0767528234206466, + -0.0526971951753399, -0.0477053831942802, 0.000613806533422364, -0.000631628059238499, + 0.00026217621127941, 0.000555307997961608}, + {0.62884078560034, -0.152668817824194, 1.70304497205574, 0.000894387499536142, + -0.00306495168098874, 0.0180087418010658, 1.74990847586174e-05, 3.1263263531046e-05, + -7.1643235604579e-06, 0.000147876621100347}, + {-3.05400297018165, -0.316256447664344, 1.14841722699638, 0.000671621084688467, + -0.000906765726598906, 0.000687041032077189, 0.000323419818039841, 0.000128019308781935, + 0.000286018723737538, 0.000192248693306256}, + {-4.39855066935163, -0.322858945262125, 1.44405016355615, -4.93181749911261e-05, + 0.0127396834052722, -0.00523149676786941, 2.56561922352657e-05, 7.61202764874326e-06, + -0.00014623717850039, 8.12219846932013e-06}, + {110.820397525173, -10.9497307382094, 2.48939759290446, 0.296585618718034, + -0.142611297893517, -0.0141810186612052, -0.00275127095595919, 0.00160686698368569, + -0.000872029428758877, -0.000410522437887563}, + {-7.15740446281205, 0.104233532313688, 1.13155893729292, -0.00350418544400852, + 0.00532058598508803, 0.00459314980222008, 3.09155436939349e-05, -7.60935741692174e-05, + -5.87922606348196e-05, 2.56146268588382e-05}, + {44.8818060495112, -7.94729992210875, 3.59286389225051, 0.217944601088562, + 0.108087933176612, -0.116711715153385, -0.00194260120960441, -0.0015752762498594, + -0.000331129410732722, 0.00125896996438418}}; + float sus4coeffAlpha[9][10] = { + {-12.4581187126738, 0.398038572289047, -0.438887880988151, -0.00965382887938283, + -0.0309322349328842, -0.00359106522420111, -7.79546112299913e-06, -0.000432733997178497, + -9.79031907635314e-05, -1.49299384451257e-05}, + {8.41054378583447, 1.87462327360707, 0.266809999719952, 0.0216455385250676, + 0.00879426079919981, -0.00142295319820553, 0.000194819780653264, 8.57549705064449e-05, + -3.56478452552367e-05, -1.65680920554434e-05}, + {16.4141780945815, 2.57697842088604, 0.373972171754278, 0.0498264199400303, + 0.0183175817756131, -0.008545409848878, 0.000422696533006382, -0.000268245978898508, + -0.000663188021815416, -7.51144017137827e-05}, + {0.796692054977593, 1.26773229735266, 0.247715261673662, 0.00358183885438128, + 0.00216435175662881, 0.00713732829335305, -0.000110129715615857, 3.56051594182427e-05, + 5.03074365340535e-05, 8.40279146176271e-05}, + {2.37491588500165, 1.05997969088519, 0.309540461340971, -0.000405047711742513, + 0.000462224730316111, -0.00201887171945793, 0.000260159805167265, -0.000282867209803598, + 0.000201613303652666, -0.000277796442847579}, + {6.36749007598708, 1.31659760017973, -0.122724934153231, -0.00328808937096891, + -0.00577347207798776, 0.00403172074457999, -7.45676459772001e-05, 1.79838644222274e-05, + 0.000104552066440564, -2.78115121929346e-05}, + {-47.9667098848496, 3.97703197139796, -1.96403894754299, -0.0577989657406978, + 0.0634225576208007, -0.0346023445055141, 0.00045886475369098, -0.000326132951996844, + 0.000716490441845967, -0.000136132038635483}, + {6.21505474256094, 0.871830486201601, 0.286906473833627, 0.007875292606045, + -0.00974634725746389, 0.00128416935792136, -0.000111796743751489, 0.000102016719989187, + -3.3503088289589e-05, -1.03874407813931e-05}, + {102.09801265482, -4.12715152309748, -5.04594403360339, 0.075499959116996, + 0.216574192561683, 0.0750031215784663, -0.000147358932612646, -0.0023710703422108, + -0.00143310719642393, -0.000431914403446768}}; + float sus4coeffBeta[9][10] = { + {-21.5077132684032, -1.60004839699939, -0.0298995033958561, -0.0315563250430659, + -0.0424403625879891, -0.0245426225510417, -0.000209861203016225, -0.000422150973104431, + -0.00030514398458781, -0.000211986731019738}, + {9.07644247897601, 0.207457289788099, 1.26735366597312, 0.00768477352180427, + 0.00429230749575816, -0.00514802326062087, 7.56149591998578e-05, 8.42794730840662e-05, + -3.62215715492783e-05, -5.24384190165239e-05}, + {-33.5225408043693, -3.11167857248829, 1.91760591695775, -0.0963752386435729, + 0.00026620241534153, -0.0256680391021823, -0.00102188712837393, 2.63753563968978e-05, + 0.000113172463974702, 0.000271939918507175}, + {19.1379025029401, -0.225979661987912, 2.72337120022998, -0.00136982412154458, + -0.00447301210555274, 0.046496718064139, 2.09123846958985e-05, -4.30383094864847e-05, + -1.22808643520768e-05, 0.000440555709696048}, + {2.957867714783, -0.316069593806939, 1.06379930645214, 0.00103244713047271, + 0.00148059212230411, 0.000557885068990542, 0.000288633931072557, 0.000172775380291659, + 0.000269738457990237, 0.000254577019084984}, + {2.04155199929521, -0.318303488378033, 1.37820715117028, 0.00114788656817743, + 0.0130051117909245, -0.00743109928493789, 1.22403390396844e-05, -3.19245785131217e-05, + -0.000156735218010879, 3.81458400945988e-05}, + {27.314954181241, -1.43916155634084, 2.48967706992348, 0.0278695408478388, + -0.0341141456915131, 0.0281959785297513, -0.000252996164135396, 0.000163365679366542, + -0.000380129463154642, 0.000159350154429114}, + {-0.274693278266294, 0.0199711721436635, 1.26676843352524, -0.0006713759238817, + -0.00389715205101059, 0.00294298337610857, -9.58643121413979e-06, 6.30700938550725e-05, + -6.07188867796123e-05, 7.72199861279611e-06}, + {-74.1601853968901, 2.55641628908672, 6.38533530714782, -0.0582345132980647, + -0.0653804553172819, -0.138850555683872, 0.000489364157827405, 0.000469559629292745, + 0.000698140692952438, 0.00123017528239406}}; + float sus5coeffAlpha[9][10] = { + {-12.1398741236355, 1.99425442858125, -1.9303044815802, 0.0418421279520049, + -0.0309683799946315, -0.0562201123081437, 0.000522607299552916, -0.000375386573815007, + -0.000183899715035788, -0.000600349486293698}, + {4.51862054729553, 1.72396080253297, 0.274562680698765, 0.0162681383591035, + 0.0108410181586673, -0.000272215427359511, 0.000124164068046579, 0.000125586897851351, + -1.24082224214974e-05, -1.63339067540159e-05}, + {63.0100748193658, 7.78014670478172, 0.327263471268564, 0.181264302704374, + -0.0652454854214506, -0.03906716801285, 0.00166924078925478, -0.000749939315526625, + 0.000320696101132374, 0.000499934751180042}, + {-2.14377722994325, 1.33617641673436, 0.0973465660282871, 0.00389526886867845, + 0.00526064997381395, 0.00244964888333519, -8.59416490903541e-05, 4.58871931007681e-05, + 8.6123353128647e-05, 2.85447259858337e-05}, + {0.164792977301912, 1.17541977248641, 0.348838798760518, -0.000180865118417534, + 0.000331789515553421, -0.000734333865631793, 9.76677859410759e-05, -0.000324347075049525, + 8.66683396011167e-05, -0.000385839566009832}, + {-0.228934187493575, 1.30552820143752, 0.306779576899158, -0.00508763741184706, + -0.00318524263093038, -0.00878095392529144, -6.59040013073836e-05, 8.69122529321691e-05, + 5.73853071731283e-05, 8.56628414466758e-05}, + {22.6047744510684, -0.591739857860868, 0.566728856847393, 0.0498124268150265, + -0.0214126910277926, 0.00538091942017912, -0.000391517685229849, 0.000554321668236216, + 0.000191004410219065, 0.000102775124022018}, + {4.54704081104052, 0.844841244606025, 0.181355971462193, 0.0109743851006749, + -0.00363467884122547, 0.00108873046814694, -0.000153236888951059, 3.14623342713789e-06, + -2.78503202185463e-05, 3.99983788680736e-06}, + {-30.878359404848, 5.20536009886854, -0.674455093700773, -0.10801865891189, + -0.0514805639475938, 0.0503660452068572, 0.00072776817295273, 0.00120288537038655, + -0.000301602375634166, -0.000477098479809266}}; + float sus5coeffBeta[9][10] = { + {16.8155737032787, 0.65475660868259, 1.95532810363711, 0.000295624718662669, + 0.0426379914736747, 0.00192544771588337, -4.94534888281508e-05, 8.32299142575155e-05, + 0.000645497238623369, -0.000234155227840799}, + {9.48268090632318, 0.528942263930744, 1.34030963800712, 0.0173605129814363, + 0.00581086655972212, -0.00365006277801141, 0.000180048140973223, 0.000102002650672644, + -4.10833110241736e-05, -8.7810396165556e-05}, + {-47.8325489165383, -4.78262055949503, 1.66912859871505, -0.143518014673292, + 0.0288441527062856, -0.00322823115861497, -0.00148509910480755, 0.000284265179004289, + -0.000175299737313045, -7.04175618676909e-05}, + {3.70510151312723, -0.272200626024415, 1.5527519845099, 0.000589727630962265, + -0.00889682554869096, 0.0109857452472628, 3.05876215574877e-05, 2.09194236165814e-05, + -8.33769024439277e-05, 6.90991113575066e-05}, + {0.820199776906695, -0.355683467192776, 1.17142130858009, -0.000160174871610729, + 4.09723480153701e-05, 0.000209103751629257, 0.000390331989170637, 6.45642836249667e-05, + 0.000318092703362044, 0.000107158633760141}, + {5.52084497768914, -0.227775345312466, 0.845897282556327, 0.00157426476122436, + 0.00657189797805861, 0.0103797665963117, 2.51479848048895e-05, -4.78371400399983e-05, + -5.20221896473413e-05, -0.000143840492906166}, + {-33.4875689683454, 0.937557276329106, -1.02741065470967, -0.0140023273976314, + 0.0401908729477037, -0.0512457211360142, 7.05537967426573e-05, -0.00027521752411122, + 0.000407657552700476, -0.000458411000693613}, + {0.931346887326171, -0.320804452025793, 1.28866325376154, 0.00912456151698805, + -0.00404367403569981, 0.00477543659981282, -9.43987917474817e-05, 4.66464249533497e-05, + -7.89362487264572e-05, -1.0951496495443e-05}, + {-38.3689359928435, 3.8540516906071, 1.26391725545116, -0.108584643500539, + -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}}; + 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}}; + float sus7coeffAlpha[9][10] = { + {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, + -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, + -0.00119021101127241, -0.000460110780350978}, + {1.60822506792345, 1.12993931449931, 0.300781032865641, -0.00405149856360946, + 0.0116663280665617, -0.000746071920075153, -8.36092173253351e-05, 0.000126762041147563, + -1.57820750462019e-05, -2.13840141586661e-05}, + {-151.403952985468, -5.77049222793992, 9.71132757422642, -0.113259116970462, + 0.284142453949027, -0.198625061659164, -0.000836450164210354, 0.00174062771509636, + -0.00323746390757859, 0.00124721932086258}, + {3.47391964888809, 1.28788318973591, 0.358380140281919, 0.0033863520864927, + 0.00154601909793475, 0.0103457296050314, -9.56426572270873e-05, 5.48838958555808e-05, + 2.97537427220847e-05, 0.000104735911514185}, + {3.32650947866065, 1.16701012685798, 0.293514063672376, -0.00065850791542434, + -8.61746510464303e-05, -0.00212038990772211, 0.00010377123197, -0.000262818127593837, + 0.000103360882478383, -0.000296739688930329}, + {-0.440176043435378, 1.18923278867097, 0.519516382652818, -0.00138846714677511, + 0.00266491699926247, -0.014254675949624, -4.20279929822439e-05, -5.49260281515447e-05, + -1.00328708454487e-05, 0.000138142092498215}, + {9.54962966738358, 1.83809145920811, 1.82162819067959, -0.0116786627338505, + -0.00496037444422313, 0.0590883547819332, 7.48465315787857e-05, 0.000221693951602584, + 7.96466345174136e-06, 0.000638822537725177}, + {7.04862901290925, 0.876813777672465, 0.16368093989381, 0.00928717461441627, + -0.00276538956293246, 0.00117995419940653, -0.000141511492474493, -6.09796031786385e-06, + -2.62114930414747e-05, -2.88713611443788e-06}, + {135.349147631811, -7.21933296299596, -6.02379024934871, 0.19557354282067, + 0.207680233512614, 0.12880101618361, -0.00169832076532024, -0.00192216719797732, + -0.00188763612041332, -0.00103101801961442}}; + float sus7coeffBeta[9][10] = { + {-12.7115487367622, -1.08890790360556, 0.0579616268854079, -0.0212303293514951, + -0.0395948453851818, -0.0275564242614342, -0.000228652851842222, -0.000148106159109458, + -0.000555136649469199, -0.000198260004582737}, + {-0.988147625946871, -0.759018567468546, 1.20998292002818, -0.0241231836977845, + -0.000572110443300516, -0.00294835038249426, -0.00026533039022186, 6.82250069765274e-06, + 7.21038415209318e-06, -6.54881435118179e-05}, + {98.0979345921564, 4.27381413621355, -4.39956005193548, 0.0709109587666745, + -0.172774236139236, 0.107243391488741, 0.000421832640471043, -0.00140450884710288, + 0.00158019019392239, -0.00078512547169536}, + {4.10892685652543, -0.229301778557857, 1.33380992987117, -0.000250095848720304, + -0.00555205065514645, 0.00355052914398176, 1.62727119770752e-05, -1.26026527654764e-05, + -3.25505031810898e-05, 5.79970895921158e-06}, + {3.09432502337258, -0.300556003790433, 1.17085811008124, 0.00128679594824324, + 0.00148229981422985, 9.15267474159147e-05, 0.000300497843413856, 6.31378865575566e-05, + 0.000258447032558814, 9.79142983264352e-05}, + {8.92336134924575, -0.197306981784312, 0.659908505354084, 0.00175572239373996, + 0.006801023678097, 0.0189775987436792, 9.2187857727721e-06, -4.8706332690626e-05, + -6.887009887486e-05, -0.000266455617735054}, + {-52.0734887320227, 2.64822385560272, -1.72387600304694, -0.0383944891609251, + 0.110873671161269, -0.0475247245070445, 0.000194652401328063, -0.000697307928990137, + 0.00124021816001, -0.000194213899980878}, + {2.08203985879155, -0.127503525368396, 1.17628056094647, 0.00283288065938444, + 0.00394668214608305, 0.00314868636161131, -2.99504350569853e-05, -7.11070816314279e-05, + -6.30148122529749e-05, 2.28114298989664e-05}, + {191.321181158032, -12.2449557187473, -7.21933741885107, 0.267954293388644, + 0.331529493933124, 0.149867703984027, -0.00222279201444128, -0.00284724570619954, + -0.00298774060233964, -0.000988903783752156}}; + float sus8coeffAlpha[9][10] = { + {5.46354311880959, 1.15370126035432, 0.568432485840475, -0.00105094692478431, + -0.000472899673842554, 0.015581320536192, 2.26460844314248e-05, -0.000254397947062058, + 0.000198938007250408, 0.000102026690279006}, + {8.8976133108173, 1.89502416095352, 0.268670471819199, 0.0217013413241972, + 0.00973925295182384, -0.00116357269193765, 0.000185865842232419, 0.000103311614912702, + -2.46539447920969e-05, -2.06292928734686e-05}, + {-45.4550803910752, 1.27220123406993, 5.21483855848504, 0.0315791081623634, + 0.0725172355124129, -0.13947591535243, 0.000412577580637848, 0.000434545096994917, + -0.000840043932292312, 0.00126857487044307}, + {1.81302768546433, 1.20563501267535, 0.344815267182167, 0.00546879453241056, + -0.00115382996865884, 0.010597876132341, -7.75885604486581e-05, 8.99568815949154e-05, + -2.98129544974679e-06, 0.000108913239345604}, + {2.19111439539173, 1.06951675598148, 0.283707798607213, 0.00016478588207518, + 0.000196086067268121, -0.00214980231173703, 0.000237820475654357, -0.000256402967908595, + 0.000165966620658577, -0.000268394081675921}, + {15.0858674915897, 1.27922724811168, -1.0803137812576, -0.00184009775302466, + -0.00458792284209219, 0.0359393555418547, -6.05121024079603e-05, -1.2288384024143e-05, + 8.55484605384438e-05, -0.000379241348638065}, + {-14.9594190080906, 1.79473182195746, -1.00830704063572, 0.000890685410857856, + 0.0408932029176081, -0.0165460857151619, -0.000170544299916973, -0.000370901607010145, + 0.000324089709129097, -9.33010240878062e-05}, + {0.867614491733251, 1.38248194737027, 0.233408537422123, -0.00772942878114575, + -0.00783126068079782, -0.000413713955432221, 4.5775750146291e-05, 6.97323029940275e-05, + 1.70664456940787e-05, 6.75517901233086e-06}, + {2.34474364146174, -0.777275400251477, 2.09531381577911, 0.0170780716714389, + 0.102855060371092, -0.1203441505925, 0.000187004964420911, -0.00141720441050986, + -0.000336251285258365, 0.00145175125888695}}; + float sus8coeffBeta[9][10] = { + {28.3033101237397, 1.77504446792811, 1.70758838986317, 0.0307800697044683, + 0.0598759344275936, -0.014461432284373, 0.000128415617799076, 0.000664419128546701, + 0.000312923304130995, -0.000269026446641855}, + {7.73040563051023, 0.0267291479555493, 1.16189582308493, 0.000611047892976521, + -0.00213680506915073, -0.00517435586596902, -3.60304406049766e-06, -1.74452976404459e-05, + -3.95396925228538e-05, -7.01948519410633e-05}, + {-48.0766126130725, -3.77981206700298, 3.03482861087335, -0.0678496412519532, + 0.115260678424016, -0.0109681510065038, -0.000438011443691466, 0.00097230136258486, + -0.000930875177732769, -0.000203144239955507}, + {12.1881935626341, -0.234345089308583, 2.01134619426134, 0.000181529284001169, + -0.00642848065105061, 0.0243985799415726, 2.0224042581776e-05, 5.22503286757285e-06, + -4.75196303016323e-05, 0.000221160482364556}, + {3.49559433498742, -0.294995112674766, 1.07892379698257, 0.000861664794052587, + 0.00138978933062055, 0.000436385106465176, 0.000288095124755908, 0.000147259769247883, + 0.000256686898599516, 0.000198982412957039}, + {9.36663996178607, -0.171266136751803, 0.799869891484541, -0.000896305696610864, + 0.00477919972789653, 0.0077876110326094, 9.16475263625076e-06, 3.02461250100473e-05, + -3.63917701783264e-05, -0.000101376940843402}, + {9.93372683055145, 1.02056557854246, 3.01635426591734, -0.0477113881140277, + -0.0280426434406976, 0.0438813017696874, 0.000470431190169998, -7.55753674679743e-05, + -0.000516444906323815, 0.000253844418223843}, + {4.12868774589741, -0.305711790187688, 1.15739216407191, 0.00942395115281056, + 0.00264758462357433, 0.00227985850688479, -0.000107537164019682, -4.91396736189963e-05, + -5.3803493814502e-05, 6.80587059728718e-06}, + {64.9193383444005, -1.57724255547465, -3.82166532626293, 0.0104712238987591, + 0.0898786950946473, 0.128910456296131, -8.27123227422217e-05, -0.000143979624107479, + -0.00146684876653306, -0.00102226799570239}}; + float sus9coeffAlpha[9][10] = { + {65.8975109449121, 2.19115342242175, 6.11069527811832, -0.0219884864133703, + 0.119985456538482, 0.142746712551924, -0.000465882328687976, 0.000606525132125852, + 0.00141667074621881, 0.00109715845894006}, + {5.70337356029945, 1.86705636976809, 0.235584190291708, 0.0194937327615426, + 0.00973291465247784, -0.00155675297510773, 0.000147099297988423, 0.000115708967219349, + -4.1462310493722e-05, -9.80097031103588e-06}, + {138.221145997284, 6.07665575619595, -9.08085914250542, 0.0839801072927519, + -0.143071750033303, 0.237868300719915, 0.000626693630444932, -0.000579788170871402, + 0.00181740650944343, -0.00207086879728281}, + {-7.78295582666151, 1.37506685179192, -0.507596181420042, 0.00350118305456038, + 0.00380814310115541, -0.0174012437563343, -0.000124801268056815, 2.96314830184492e-05, + 6.3416992450033e-05, -0.000190177262510221}, + {0.13102597129751, 1.24228303845143, 0.328808873447393, 2.6858679536165e-05, + 0.000231428138164498, -0.000584089095259736, 5.5322167970451e-05, -0.000322205709821716, + 7.71348293209208e-05, -0.000393885990364776}, + {4.64571633968935, 1.2668223691397, -0.158952088650432, -0.0038344859267428, + 0.0030051503726095, 0.00455578826025588, -9.42520993914957e-05, 5.81633314412289e-05, + -4.43545804544095e-05, -4.83524454851519e-05}, + {99.2385930314563, -3.65569343617926, 5.54203926675588, 0.0975630395981933, + -0.15701634159692, 0.107834711298836, -0.000885326636237814, 0.000960753844480462, + -0.00179894024848343, 0.000583066757644971}, + {2.82671549736619, 1.11214198870501, 0.214735318432744, 0.00284415167563662, + -0.00743289575690122, 0.000382705440762292, -7.43232442872501e-05, 6.96994098083348e-05, + -4.15108111710131e-06, 1.33520085213482e-05}, + {36.9013743125415, -0.522392401546163, -1.52452843963663, 0.0261375433218879, + 0.060573568610239, 0.0182582125221054, -0.000244373383911157, -0.000271385147292484, + -0.000723799969427732, 6.76324880239196e-05}}; + float sus9coeffBeta[9][10] = { + {128.70886435409, 7.27355509732751, 7.18142203531244, 0.1536100459329, 0.199455846541636, + 0.101824964939793, 0.00116666116789421, 0.00181595584079788, 0.00159271319494017, + 0.000556768406475719}, + {-7.07933839681024, -0.979062424441878, 1.21792546815617, -0.0295740143783226, + -0.00442780611714201, -0.00329612819203176, -0.000291373125216143, -7.47259350176359e-05, + -4.87265282482212e-05, -7.87490350444332e-05}, + {41.1357193180502, 2.75138456414254, -0.0160889117718198, 0.0274001112562423, + -0.145644717742057, -0.0316076203283094, -0.000136443337244472, -0.00153945199081365, + 0.000938960439977633, 0.000599987111822885}, + {2.7980384746608, -0.234741037383589, 1.5342193016705, -0.000993791566721689, + -0.00787533639513478, 0.00927468655141365, 2.63308697896639e-05, -3.42816267184975e-05, + -8.48879419798771e-05, 3.84043821333798e-05}, + {0.427687530667804, -0.346076633694936, 1.22968527483851, -4.95098138311122e-05, + 0.000298245372198029, 0.000332756250024796, 0.00040375986210644, 5.20675972504572e-05, + 0.000327042170278218, 5.93011568264671e-05}, + {4.50337810133314, -0.279364254817202, 0.945812187846199, 0.000116182663432306, + 0.0115646046622083, 0.00908289960302886, 1.90394667311541e-05, -4.4360223646434e-06, + -0.000131398914898614, -0.000145568992865512}, + {-36.3377213654193, 2.21047221783626, 0.0609982245149821, -0.0670546774988572, + 0.016827777144747, -0.0277834084058314, 0.000778301409125556, 0.000135846745194401, + 0.00043261858797068, -0.00021172728254561}, + {-0.737678205841529, -0.217352122193475, 1.23494846329297, 0.00748173441779792, + 0.0019595873704705, 0.00567253723266176, -8.34768773292938e-05, -3.50608394184873e-05, + -0.000107500091550635, -5.1379722947632e-07}, + {-36.6150844777671, 3.24952006904945, 1.7222457840185, -0.0846362445435584, + -0.0625549615377418, 0.019178365782485, 0.000664877496455304, 0.000942971403881222, + 0.000190754698755098, -0.000372226659190439}}; + float sus10coeffAlpha[9][10] = { + {14.4562393748324, 0.669162330324919, 2.13895255446541, -0.0161997097021299, + 0.00185995785065838, 0.0621351118528379, -0.000278999272493087, 0.000238469666491965, + -0.000279407497782961, 0.000726904943739837}, + {-4.45678285887022, 0.92869611919737, 0.186752102727282, -0.00706160758952316, + 0.00532680276723634, -0.00119102617674229, -0.000105283880098953, 3.90673052334419e-05, + -3.13338277344246e-05, 5.32977236959767e-06}, + {30.4255268053197, 3.00991076401191, -1.4855621363519, 0.033934286288413, + -0.0553588742704929, 0.0299275582316466, 0.000167915322354466, -0.00050925078118232, + 0.000463662961330962, -0.000232919143454163}, + {2.45076465343337, 1.30206564388838, 0.635121046212765, 0.00517109639797675, + 0.00360579544364496, 0.0198490668911362, -9.31556816982662e-05, 6.7313653707875e-05, + 6.4669137025142e-05, 0.000209727581169138}, + {-0.784841314851562, 1.10058314980836, 0.314063830836532, 0.000583003703415889, + 0.000312635453606579, -0.000183738114552387, 0.000214096205760617, -0.000286744686021244, + 0.000159157597180407, -0.00032235099420715}, + {7.19568036510586, 1.33307479701657, -0.465585141952456, -0.0031910726544199, + -0.00546273504371797, 0.0145494754402526, -7.9863949693769e-05, 4.83681329120104e-05, + 8.85844309936609e-05, -0.000143217870916994}, + {-12.8344546267449, 1.36023633150143, -0.728527724854506, 0.019982118403416, + 0.0385056413989437, -0.00468598272326268, -0.000303957957649245, -6.37783846968216e-05, + 0.000514049116643205, 0.000112015427600697}, + {-2.58279031298065, 1.42167821629586, 0.208769467511292, -0.00640190372145885, + -0.0056405289717473, 0.000509611313918708, 2.23310562107823e-05, 3.23685469522147e-05, + -7.55982776243849e-06, 2.78417756661088e-06}, + {-29.7178996143914, 2.636972251183, 1.97316329325243, -0.03983524158327, + -0.0193152048730234, -0.0600902798379509, 0.00031786916010672, 0.000162178988605602, + 0.000224550786416246, 0.000614337977361927}}; + float sus10coeffBeta[9][10] = { + {12.4771349792459, 1.44317849705414, 0.975637226331561, 0.0430284146301043, + 0.0220810531548995, -0.0220926906772, 0.000310052324529521, 0.000658151808869523, + -0.000288026365111098, -0.000214619731807045}, + {-0.113203260140131, -0.272424061092191, 1.27704377191184, -0.00791746619331075, + 0.00278646694862191, -0.00398881099259934, -8.09569694307212e-05, 5.99617384829016e-05, + -5.4550919751855e-05, -8.6314530565085e-05}, + {-48.585664295448, -2.04899787231903, 4.48757129623549, -0.0226180460431321, + 0.090326735447661, -0.0722998813632622, -6.77623771415477e-05, 0.000562585419036509, + -0.000956171370931993, 0.000491554402311223}, + {-1.20986884955482, -0.215604107185474, 1.22123198786617, 0.000256508527822089, + -0.00625056735692847, 0.00262961582224303, 2.27433984698861e-05, 1.60471509861372e-05, + -4.85061736834262e-05, -1.8387092782907e-06}, + {-0.250205907903409, -0.315819331560782, 1.09018364376391, -0.000521787614293089, + -0.000500747760913489, 2.48184307342838e-05, 0.000313799238640988, 0.000136669146368744, + 0.000278914324565192, 0.000218512838469476}, + {-1.38512578184076, -0.240456589364121, 1.34170304231345, 0.00017499230372669, + 0.0070862275911073, -0.00460640844814105, 1.27594111036696e-05, -4.73855624902052e-06, + -5.41141037812903e-05, 8.50767021818388e-06}, + {58.9917559342216, -2.28705697628345, 5.35995190407842, 0.0214721399750612, + -0.112195722921667, 0.0890150265857542, -0.000100675657768708, 0.000493488022135339, + -0.00137672908303878, 0.000518683157694955}, + {3.18905073365834, -0.633376549706314, 1.17190259811174, 0.0188817945597344, + 0.00107470708915782, 0.00400880471375267, -0.000197312295539118, -2.46543035998379e-05, + -6.07871064300252e-05, 1.91822310311955e-05}, + {-21.6881499304099, -0.563186103920008, 3.70747028664292, 0.021112883967427, + -0.00650020689049325, -0.0778533644688476, -0.000131921888670268, -0.000402754836445439, + 0.000551249824375055, 0.00062236627391337}}; + float sus11coeffAlpha[9][10] = { + {-5.23569698615548, -1.45500092391928, 2.7643243644756, -0.0762912296128707, + -0.0201645929971608, 0.0997226845779083, -0.000741669441569556, -0.000485368004931528, + 0.000166230212359982, 0.00103455037278067}, + {-7.7405077383712, 0.892040861541276, 0.39014957203484, -0.00952030929935314, + 0.0185577462685363, 0.000500600568760257, -0.000151227821554572, 0.000245334737283439, + 1.89380065823205e-05, 1.83481122973969e-07}, + {-27.462143709831, -1.68192474287299, 0.689411302961069, -0.0146021086710062, + 0.227153492753605, 0.0815806579791421, 2.92919049429149e-05, 0.00153760357651792, + -0.00247865821192621, -0.00166333309739387}, + {-6.74664748624368, 1.43279156053015, 0.0212787292914553, 0.00764792230365732, + 0.00796410301290615, 0.0014384998868733, -8.95239151813685e-05, 9.55245417090909e-05, + 0.000127251739461239, 3.26943341606363e-05}, + {-2.20391533717674, 1.32902400478083, 0.38633027011889, 0.00104660852197061, + 0.00105228824412283, -0.00242067551428214, -6.98346290136652e-05, -0.000369075232184835, + -1.59510520000704e-05, -0.000448565104826966}, + {-5.29476778147188, 1.4549556336236, 0.561334186252557, -0.00260896342687109, + -0.00855934179001141, -0.0182515354646452, -8.79385828606048e-05, 5.98357681659175e-05, + 0.000146570207542509, 0.000201596912834554}, + {-45.7906613832612, 3.77630104475902, -1.67494598155515, -0.0545433897761635, + 0.047897938410221, -0.0355687158405231, 0.000374738707508583, -0.000448453494537518, + 0.000377784972619365, -0.000276573228333836}, + {-9.11681182090372, 2.06933872940742, 0.26131496122122, -0.0259534033367855, + -0.00777266937872862, -0.00262135395843891, 0.000223790782655445, 6.40488537928934e-05, + 7.75581514100296e-05, -9.25934285039627e-06}, + {183.243883340634, -8.02281039502717, -10.0756951652703, 0.168750521462303, + 0.314006821405967, 0.200264755034566, -0.0011895153717447, -0.00253812476819896, + -0.00291324393641628, -0.00140062522117514}}; + float sus11coeffBeta[9][10] = { + {34.4499366074013, -0.438583698052091, 4.72111001451028, -0.041810050989433, + 0.0562461093661426, 0.0856849892524893, -0.000477813051406167, -3.16404257494464e-05, + 0.00102633196865105, 0.000552974013759876}, + {7.6366298088699, 0.150314752958302, 1.31364679484924, 0.00557696667395871, + 0.00163731860604376, -0.00454759608980269, 5.83979683674572e-05, 4.45944881220665e-05, + -4.27874746147066e-05, -8.77418673597557e-05}, + {130.156215011893, 1.85759000444524, -10.986892391833, -0.00686275191260681, + -0.188837138116058, 0.346177462085361, -0.000183276946352264, -0.000702183496893294, + 0.00293145272693857, -0.00318194442670715}, + {-1.67854820161036, -0.358899332859806, 0.956690839640595, -4.93862910503803e-05, + -0.0136134783014874, -0.00848731301504507, 3.75950499927045e-05, 1.35374694383289e-06, + -0.000156596507890443, -0.000123254220377897}, + {3.67569209537767, -0.387260959713287, 1.31343215605952, -0.00206444615206506, + 0.00145334813110285, -0.00151259497696238, 0.000449492568365603, 6.95883968949488e-07, + 0.000368585523744765, -6.3420715525635e-05}, + {14.3296323024886, -0.182979476956897, 0.306817119309235, -0.00022212115978293, + 0.00463485302909649, 0.0277574953550035, 1.1422454625565e-05, 1.06053257479502e-05, + -2.05720000720608e-05, -0.000338584671430337}, + {-18.7534921817754, 1.14272710923224, 0.460498062012866, -0.00995826989278202, + 0.0658502318647112, 0.00616942819937029, -7.70857153768402e-05, -0.000641755741925561, + 0.00047849204592989, 0.000158509018296766}, + {1.26543621388607, -0.176674379740481, 1.38814920935488, 0.00545485262295305, + -0.00499775616702264, 0.0038057039142173, -6.59604252054511e-05, 6.40211116049053e-05, + -6.74778593434431e-05, -2.81973589469059e-05}, + {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136, + 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, + -0.000889232196185857, -0.00168429567131815}}; - 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}}; - 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}}; - float sus1coeffAlpha[9][10] = { - {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084, - -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176, - -0.000178087038629721, 4.09208968678946e-05}, - {2.24803085641869, 1.42938692406645, 0.30104994020693, 0.00756499999397385, 0.0117765927439368, - -0.000743685980641362, 4.69920803836194e-05, 0.000129815636957956, -9.10792250542345e-06, - -2.03870119873411e-05}, - {26.9943033817917, 0.147791175366868, -3.48256070200564, -0.0303332422478656, - 0.0183377266255394, 0.124593616125966, -0.000466003049304431, -0.000272000698791331, - -0.00063621309529853, -0.00158363678978767}, - {-0.221893380318465, 1.29919955307083, 0.21872487901019, 0.0049448219667127, - 0.00291224091529189, 0.00654651987282984, -9.86842469311185e-05, 8.20057454706638e-05, - 6.42331081725944e-05, 7.11656918299053e-05}, - {1.40178843964621, 1.1733111455249, 0.287485528779234, -0.000793970428759834, - 0.000170529273905818, -0.00268807864923086, 9.09553964483881e-05, -0.000271892733575409, - 8.52016306311741e-05, -0.000291797625433646}, - {0.65549617899457, 1.25716478394514, 0.301396415134214, -0.00357289640403958, - -0.000473416364133431, -0.010760332636205, -9.77220176481185e-05, 4.40798040046875e-05, - 2.84958344955681e-05, 0.000128583400693359}, - {6.20958048145025, 1.9528406481596, 1.32915657614139, -0.0326944423378284, -0.0158258335207969, - 0.0328249756354635, 0.00027113042931131, -0.000133980867173428, -0.000357964552318811, - 0.000224235061786191}, - {2.46222812180944, 1.1731834908026, 0.17440330925151, -0.00132279581980401, - -0.00447202005426964, -0.000804321602550913, -1.59526570766446e-05, 2.62946483533391e-05, - 3.28466749016414e-05, -6.63837547601294e-06}, - {42.615758859473, 2.46617281707273, -5.742515881283, -0.131942799763164, 0.20250702826603, - 0.0981562802911027, 0.00189939440077981, -0.0018591621618441, -0.00161121179693977, - -0.00058814458116749}}; - float sus1coeffBeta[9][10] = { - {-12.300032617206, -1.06640894101328, 0.33950802247214, -0.00890867870617722, -0.04872758086642, - -0.0114263851027856, 0.000141061196404012, -0.000675469545483099, -0.000138249928781575, - -0.000138871036200597}, - {10.1631114109768, 0.261654603839785, 1.2376413405181, 0.00888558138614535, 0.00151674939001532, - -0.00534577602313027, 9.19430013005559e-05, 5.39804599087081e-05, -4.15760162347772e-05, - -7.60797902457032e-05}, - {-30.142329062199, 1.26939195100229, 6.14467186367471, 0.0464163689935328, 0.00379001947505376, - -0.165444163648109, 0.000516545385538741, 1.56053219154647e-05, -5.58651971370719e-05, - 0.00173185063955313}, - {12.1454103989862, -0.243589095509132, 2.02543716988677, -0.000857989774598331, - -0.00705278543432513, 0.0250580538307654, 3.50683653081847e-05, -2.63093897408875e-05, - -5.67352645830913e-05, 0.000232270832022029}, - {4.4338108906594, -0.305276965994378, 1.17293558142526, 0.000152618994429577, - 0.00134432642920902, -0.00104036813342885, 0.000334476082056995, 6.74826804343671e-05, - 0.000275311897725414, 7.58157740577916e-05}, - {3.47680700379043, -0.154163381023597, 1.389579838768, 0.000799705880026268, - 0.00401980026462874, -0.00915311817354667, -2.54817301605075e-06, -2.27422984169921e-05, - -2.61224817848938e-05, 6.00381132540332e-05}, - {29.469181543703, -0.722888948550437, 3.3623377135197, 0.00148445490093232, -0.0474780142430845, - 0.0486755575785462, 0.000126295091963757, 0.000526632230895258, -0.000259305985126003, - 0.000412751148048724}, - {2.67029041722834, -0.0837968038501666, 1.37628504937018, 0.00165061312885753, - -0.00953813055064273, 0.0032433005486936, -1.6522452172598e-05, 0.000144574078261271, - -8.47348746872376e-05, -1.92509604512729e-06}, - {-20.959201441285, -2.23605897639125, 5.73044624806043, 0.0354141964763815, 0.0887545371234514, - -0.193862330062381, 0.000216532998121618, -0.00207707610520973, 0.000552928905346826, - 0.00190182163597828}}; - float sus2coeffAlpha[9][10] = { - {6.51602979328333, 0.690575501042577, 1.18185457002269, -0.0153161662266588, - 0.00145972227341484, 0.0351496474730776, -0.000172645571366945, -6.04213053580018e-05, - 9.74494676304114e-05, 0.000334122888261002}, - {0.954398509323963, 1.10996214782069, 0.313314231563221, -0.00367553051112208, - 0.0110290193380194, 0.000240079475656232, -6.93444423181303e-05, 0.000107433381295167, - 1.30750132315838e-05, -2.43580795300515e-05}, - {-55.1159841655056, -1.47449655191106, 3.40106264596874, -0.0621428271456258, - 0.0659788065633613, -0.0791732068323335, -0.000524264070592741, 0.000582093651418709, - -0.000586102213707195, 0.000658133691098817}, - {1.98614148820353, 1.32058724763677, 0.156843003413303, 0.002748082456053, 0.00202677073171519, - 0.00382360695862248, -0.000122364309010211, 5.33354637965168e-05, 3.93641210098335e-05, - 4.06398431916703e-05}, - {3.41223117010734, 1.1597568029329, 0.31881674291653, -0.000382400010917784, - -0.000754945672515052, -0.00079200882313927, 0.000145713118224563, -0.00026910957285589, - 0.000137876961532787, -0.000326798596746712}, - {6.23333031852853, 1.24902998148103, -0.0162317540018123, -0.00338184464699201, - 0.000420329743164687, 0.00202038442335185, -7.10435889754986e-05, -6.04039458988991e-06, - 7.25318569569788e-06, -2.5930447720704e-05}, - {191.759784636909, -10.5228276216193, 8.48306234734519, 0.243240262512846, -0.344226468125615, - 0.126267158197535, -0.00186612281541009, 0.00304415728817747, -0.00304958575196089, - 0.000457236034569107}, - {5.61375025356727, 1.1692295110657, 0.224665256727786, -0.00230481633344849, - -0.00746693012026367, -0.00172583925345173, -7.00823444553058e-06, 7.31362778266959e-05, - 5.81988007269583e-05, 1.3723604109425e-05}, - {98.0250669452855, -2.18500123986039, -6.68238707939385, 0.000754807832106659, - 0.256133336978808, 0.110826583415768, 0.000457663127670018, -0.00197655629847616, - -0.00254305206375073, -0.000466731538082995}}; - float sus2coeffBeta[9][10] = { - {41.1102358678699, 2.3034699186519, 2.74551448799899, 0.061701310929235, 0.0317074142089495, - 0.0308171492962288, 0.00049453042200054, 0.000519222896270701, 2.85420168881716e-05, - 0.000259197384126413}, - {4.46821725251333, 0.0125273331991983, 1.32640678842532, 0.000543566569079156, - 0.00396616601484022, -0.00488408099728387, -3.05734704054868e-06, 7.3424831303621e-05, - -5.49439160235527e-05, -8.30708110469922e-05}, - {64.773396165255, 2.97057686090134, -1.90770757709096, 0.062747116236773, -0.077990648565002, - 0.0613989204238974, 0.00055512113297293, -0.000347045533958329, 0.00104059576098392, - -0.000348638726253297}, - {3.10352939390402, -0.2376108554276, 1.60523925160222, 0.00116454605680723, -0.0067958260462381, - 0.0136561370875238, 2.59929059167486e-05, 3.33825895937897e-05, -5.55828531601728e-05, - 0.000109833374761172}, - {0.156052891975873, -0.320721597024578, 1.15208488414874, 0.00164743688819939, - 0.000534718891498932, 0.000469870758457642, 0.000308432468885207, 0.00011789470679678, - 0.000292373398965513, 0.000183599033441813}, - {2.84967971406268, -0.21374251183113, 1.09938586447269, 2.34894704600407e-05, - 0.00588345375399262, 0.00296966835738407, 8.42707308834155e-06, 2.81870099202641e-06, - -3.56732787246631e-05, -7.04534663356379e-05}, - {-7.59892007483895, 0.358662160515702, 0.805137646978357, 0.00222144376998348, - 0.0464438387809707, 0.00847551828841782, 3.24805702347551e-05, 5.45500807838332e-05, - 0.000941378089367713, 0.000353137737023192}, - {-4.65367165487109, 0.201306010390421, 1.19135575710523, -0.00692801521395975, - 0.00394118754078443, 0.00426360093528599, 6.297683536736e-05, -7.15794236895102e-05, - -7.47076172176468e-05, -1.94516917836346e-05}, - {-59.5882618930651, 3.84530212586425, 3.50497032358686, -0.116100453177197, -0.0380997421813177, - -0.0581898335691109, 0.00111464935006159, 0.000559313074537689, 0.000168067749764069, - 0.000563224178849256}}; - float sus3coeffAlpha[9][10] = { - {-174.687021034355, -7.53454036765748, -9.33798316371397, -0.18212338430986, -0.242523652239734, - -0.202086838965846, -0.00138648793335223, -0.00225430176012882, -0.00198887215340364, - -0.00160678535160774}, - {6.92009692410602, 1.8192037428209, 0.254908171908415, 0.0179273243472017, 0.00894059238779664, - -0.000436952529644, 0.000138070523903458, 9.22759645920339e-05, -9.4312261303588e-06, - -1.76647897892869e-05}, - {-17.9720006944368, 0.230510201259892, 1.10751755772907, -0.00973621304161327, - 0.0554450499198677, -0.00590970792122449, -2.92393772526592e-05, 0.000444329929586969, - -0.000436055839773919, -9.5869891049503e-05}, - {-4.9880829382985, 1.33627775121504, -0.330382157073243, 0.00306744056311184, - 0.00376353074674973, -0.0107453978169225, -0.00010680477021693, 5.17225535432745e-05, - 7.4423443938376e-05, -0.000107927900087035}, - {0.952867982900728, 1.14513280899596, 0.307744203675505, 0.000404669974219378, - -0.000737988606997615, 0.00120218232577844, 0.000191147653645603, -0.000275058867995882, - 0.000137187356620739, -0.000320202731145004}, - {8.076706574364, 1.31338618710295, -0.334634356394277, -0.00209719438033295, - -0.00381753503582303, 0.0100347823323616, -7.00550548221671e-05, -1.97626956996069e-05, - 7.80079707003333e-05, -8.95904360920744e-05}, - {-82.4748312650249, 8.63074484663009, -0.949295700187556, -0.178618807265278, 0.130143669167547, - 0.0284326533865768, 0.00149831261351137, -0.0011583692969717, 0.0010560778729661, - 0.000635404380970666}, - {3.34457857521978, 1.09242517408071, 0.241722402244944, 0.00381629887587041, - -0.00863580122530851, 0.00137050492069702, -8.91046701171713e-05, 8.44169683308007e-05, - -3.54608413548779e-05, 8.54042677832451e-06}, - {78.1540457908649, -1.30266922193303, -5.33605443700115, 0.0184226131926499, 0.146629920899062, - 0.110698519952472, 6.64041537651749e-05, -0.00120174584530713, -0.00133177694921411, - -0.000796422644338886}}; - float sus3coeffBeta[9][10] = { - {-31.5704266802979, -5.10700699133189, 2.84549700473812, -0.122701561048957, -0.11257100034746, - 0.102120576206517, -0.000796645106694696, -0.00192211266325167, -4.99981232866237e-05, - 0.00104036677004523}, - {-0.734294938181273, -0.0694317595592039, 1.34746975389878, -0.00103465544451119, - 0.00389798465946559, -0.00308561832194191, -2.91843250099708e-06, 7.59634622232999e-05, - -6.54571602919161e-05, -0.000104146832644606}, - {24.2649069708536, 3.08145095664586, 1.88975821636026, 0.0767528234206466, -0.0526971951753399, - -0.0477053831942802, 0.000613806533422364, -0.000631628059238499, 0.00026217621127941, - 0.000555307997961608}, - {0.62884078560034, -0.152668817824194, 1.70304497205574, 0.000894387499536142, - -0.00306495168098874, 0.0180087418010658, 1.74990847586174e-05, 3.1263263531046e-05, - -7.1643235604579e-06, 0.000147876621100347}, - {-3.05400297018165, -0.316256447664344, 1.14841722699638, 0.000671621084688467, - -0.000906765726598906, 0.000687041032077189, 0.000323419818039841, 0.000128019308781935, - 0.000286018723737538, 0.000192248693306256}, - {-4.39855066935163, -0.322858945262125, 1.44405016355615, -4.93181749911261e-05, - 0.0127396834052722, -0.00523149676786941, 2.56561922352657e-05, 7.61202764874326e-06, - -0.00014623717850039, 8.12219846932013e-06}, - {110.820397525173, -10.9497307382094, 2.48939759290446, 0.296585618718034, -0.142611297893517, - -0.0141810186612052, -0.00275127095595919, 0.00160686698368569, -0.000872029428758877, - -0.000410522437887563}, - {-7.15740446281205, 0.104233532313688, 1.13155893729292, -0.00350418544400852, - 0.00532058598508803, 0.00459314980222008, 3.09155436939349e-05, -7.60935741692174e-05, - -5.87922606348196e-05, 2.56146268588382e-05}, - {44.8818060495112, -7.94729992210875, 3.59286389225051, 0.217944601088562, 0.108087933176612, - -0.116711715153385, -0.00194260120960441, -0.0015752762498594, -0.000331129410732722, - 0.00125896996438418}}; - float sus4coeffAlpha[9][10] = { - {-12.4581187126738, 0.398038572289047, -0.438887880988151, -0.00965382887938283, - -0.0309322349328842, -0.00359106522420111, -7.79546112299913e-06, -0.000432733997178497, - -9.79031907635314e-05, -1.49299384451257e-05}, - {8.41054378583447, 1.87462327360707, 0.266809999719952, 0.0216455385250676, 0.00879426079919981, - -0.00142295319820553, 0.000194819780653264, 8.57549705064449e-05, -3.56478452552367e-05, - -1.65680920554434e-05}, - {16.4141780945815, 2.57697842088604, 0.373972171754278, 0.0498264199400303, 0.0183175817756131, - -0.008545409848878, 0.000422696533006382, -0.000268245978898508, -0.000663188021815416, - -7.51144017137827e-05}, - {0.796692054977593, 1.26773229735266, 0.247715261673662, 0.00358183885438128, - 0.00216435175662881, 0.00713732829335305, -0.000110129715615857, 3.56051594182427e-05, - 5.03074365340535e-05, 8.40279146176271e-05}, - {2.37491588500165, 1.05997969088519, 0.309540461340971, -0.000405047711742513, - 0.000462224730316111, -0.00201887171945793, 0.000260159805167265, -0.000282867209803598, - 0.000201613303652666, -0.000277796442847579}, - {6.36749007598708, 1.31659760017973, -0.122724934153231, -0.00328808937096891, - -0.00577347207798776, 0.00403172074457999, -7.45676459772001e-05, 1.79838644222274e-05, - 0.000104552066440564, -2.78115121929346e-05}, - {-47.9667098848496, 3.97703197139796, -1.96403894754299, -0.0577989657406978, - 0.0634225576208007, -0.0346023445055141, 0.00045886475369098, -0.000326132951996844, - 0.000716490441845967, -0.000136132038635483}, - {6.21505474256094, 0.871830486201601, 0.286906473833627, 0.007875292606045, - -0.00974634725746389, 0.00128416935792136, -0.000111796743751489, 0.000102016719989187, - -3.3503088289589e-05, -1.03874407813931e-05}, - {102.09801265482, -4.12715152309748, -5.04594403360339, 0.075499959116996, 0.216574192561683, - 0.0750031215784663, -0.000147358932612646, -0.0023710703422108, -0.00143310719642393, - -0.000431914403446768}}; - float sus4coeffBeta[9][10] = { - {-21.5077132684032, -1.60004839699939, -0.0298995033958561, -0.0315563250430659, - -0.0424403625879891, -0.0245426225510417, -0.000209861203016225, -0.000422150973104431, - -0.00030514398458781, -0.000211986731019738}, - {9.07644247897601, 0.207457289788099, 1.26735366597312, 0.00768477352180427, - 0.00429230749575816, -0.00514802326062087, 7.56149591998578e-05, 8.42794730840662e-05, - -3.62215715492783e-05, -5.24384190165239e-05}, - {-33.5225408043693, -3.11167857248829, 1.91760591695775, -0.0963752386435729, - 0.00026620241534153, -0.0256680391021823, -0.00102188712837393, 2.63753563968978e-05, - 0.000113172463974702, 0.000271939918507175}, - {19.1379025029401, -0.225979661987912, 2.72337120022998, -0.00136982412154458, - -0.00447301210555274, 0.046496718064139, 2.09123846958985e-05, -4.30383094864847e-05, - -1.22808643520768e-05, 0.000440555709696048}, - {2.957867714783, -0.316069593806939, 1.06379930645214, 0.00103244713047271, 0.00148059212230411, - 0.000557885068990542, 0.000288633931072557, 0.000172775380291659, 0.000269738457990237, - 0.000254577019084984}, - {2.04155199929521, -0.318303488378033, 1.37820715117028, 0.00114788656817743, - 0.0130051117909245, -0.00743109928493789, 1.22403390396844e-05, -3.19245785131217e-05, - -0.000156735218010879, 3.81458400945988e-05}, - {27.314954181241, -1.43916155634084, 2.48967706992348, 0.0278695408478388, -0.0341141456915131, - 0.0281959785297513, -0.000252996164135396, 0.000163365679366542, -0.000380129463154642, - 0.000159350154429114}, - {-0.274693278266294, 0.0199711721436635, 1.26676843352524, -0.0006713759238817, - -0.00389715205101059, 0.00294298337610857, -9.58643121413979e-06, 6.30700938550725e-05, - -6.07188867796123e-05, 7.72199861279611e-06}, - {-74.1601853968901, 2.55641628908672, 6.38533530714782, -0.0582345132980647, - -0.0653804553172819, -0.138850555683872, 0.000489364157827405, 0.000469559629292745, - 0.000698140692952438, 0.00123017528239406}}; - float sus5coeffAlpha[9][10] = { - {-12.1398741236355, 1.99425442858125, -1.9303044815802, 0.0418421279520049, -0.0309683799946315, - -0.0562201123081437, 0.000522607299552916, -0.000375386573815007, -0.000183899715035788, - -0.000600349486293698}, - {4.51862054729553, 1.72396080253297, 0.274562680698765, 0.0162681383591035, 0.0108410181586673, - -0.000272215427359511, 0.000124164068046579, 0.000125586897851351, -1.24082224214974e-05, - -1.63339067540159e-05}, - {63.0100748193658, 7.78014670478172, 0.327263471268564, 0.181264302704374, -0.0652454854214506, - -0.03906716801285, 0.00166924078925478, -0.000749939315526625, 0.000320696101132374, - 0.000499934751180042}, - {-2.14377722994325, 1.33617641673436, 0.0973465660282871, 0.00389526886867845, - 0.00526064997381395, 0.00244964888333519, -8.59416490903541e-05, 4.58871931007681e-05, - 8.6123353128647e-05, 2.85447259858337e-05}, - {0.164792977301912, 1.17541977248641, 0.348838798760518, -0.000180865118417534, - 0.000331789515553421, -0.000734333865631793, 9.76677859410759e-05, -0.000324347075049525, - 8.66683396011167e-05, -0.000385839566009832}, - {-0.228934187493575, 1.30552820143752, 0.306779576899158, -0.00508763741184706, - -0.00318524263093038, -0.00878095392529144, -6.59040013073836e-05, 8.69122529321691e-05, - 5.73853071731283e-05, 8.56628414466758e-05}, - {22.6047744510684, -0.591739857860868, 0.566728856847393, 0.0498124268150265, - -0.0214126910277926, 0.00538091942017912, -0.000391517685229849, 0.000554321668236216, - 0.000191004410219065, 0.000102775124022018}, - {4.54704081104052, 0.844841244606025, 0.181355971462193, 0.0109743851006749, - -0.00363467884122547, 0.00108873046814694, -0.000153236888951059, 3.14623342713789e-06, - -2.78503202185463e-05, 3.99983788680736e-06}, - {-30.878359404848, 5.20536009886854, -0.674455093700773, -0.10801865891189, -0.0514805639475938, - 0.0503660452068572, 0.00072776817295273, 0.00120288537038655, -0.000301602375634166, - -0.000477098479809266}}; - float sus5coeffBeta[9][10] = { - {16.8155737032787, 0.65475660868259, 1.95532810363711, 0.000295624718662669, 0.0426379914736747, - 0.00192544771588337, -4.94534888281508e-05, 8.32299142575155e-05, 0.000645497238623369, - -0.000234155227840799}, - {9.48268090632318, 0.528942263930744, 1.34030963800712, 0.0173605129814363, 0.00581086655972212, - -0.00365006277801141, 0.000180048140973223, 0.000102002650672644, -4.10833110241736e-05, - -8.7810396165556e-05}, - {-47.8325489165383, -4.78262055949503, 1.66912859871505, -0.143518014673292, 0.0288441527062856, - -0.00322823115861497, -0.00148509910480755, 0.000284265179004289, -0.000175299737313045, - -7.04175618676909e-05}, - {3.70510151312723, -0.272200626024415, 1.5527519845099, 0.000589727630962265, - -0.00889682554869096, 0.0109857452472628, 3.05876215574877e-05, 2.09194236165814e-05, - -8.33769024439277e-05, 6.90991113575066e-05}, - {0.820199776906695, -0.355683467192776, 1.17142130858009, -0.000160174871610729, - 4.09723480153701e-05, 0.000209103751629257, 0.000390331989170637, 6.45642836249667e-05, - 0.000318092703362044, 0.000107158633760141}, - {5.52084497768914, -0.227775345312466, 0.845897282556327, 0.00157426476122436, - 0.00657189797805861, 0.0103797665963117, 2.51479848048895e-05, -4.78371400399983e-05, - -5.20221896473413e-05, -0.000143840492906166}, - {-33.4875689683454, 0.937557276329106, -1.02741065470967, -0.0140023273976314, - 0.0401908729477037, -0.0512457211360142, 7.05537967426573e-05, -0.00027521752411122, - 0.000407657552700476, -0.000458411000693613}, - {0.931346887326171, -0.320804452025793, 1.28866325376154, 0.00912456151698805, - -0.00404367403569981, 0.00477543659981282, -9.43987917474817e-05, 4.66464249533497e-05, - -7.89362487264572e-05, -1.0951496495443e-05}, - {-38.3689359928435, 3.8540516906071, 1.26391725545116, -0.108584643500539, -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}}; - 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}}; - float sus7coeffAlpha[9][10] = { - {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523, - -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283, - -0.00119021101127241, -0.000460110780350978}, - {1.60822506792345, 1.12993931449931, 0.300781032865641, -0.00405149856360946, - 0.0116663280665617, -0.000746071920075153, -8.36092173253351e-05, 0.000126762041147563, - -1.57820750462019e-05, -2.13840141586661e-05}, - {-151.403952985468, -5.77049222793992, 9.71132757422642, -0.113259116970462, 0.284142453949027, - -0.198625061659164, -0.000836450164210354, 0.00174062771509636, -0.00323746390757859, - 0.00124721932086258}, - {3.47391964888809, 1.28788318973591, 0.358380140281919, 0.0033863520864927, 0.00154601909793475, - 0.0103457296050314, -9.56426572270873e-05, 5.48838958555808e-05, 2.97537427220847e-05, - 0.000104735911514185}, - {3.32650947866065, 1.16701012685798, 0.293514063672376, -0.00065850791542434, - -8.61746510464303e-05, -0.00212038990772211, 0.00010377123197, -0.000262818127593837, - 0.000103360882478383, -0.000296739688930329}, - {-0.440176043435378, 1.18923278867097, 0.519516382652818, -0.00138846714677511, - 0.00266491699926247, -0.014254675949624, -4.20279929822439e-05, -5.49260281515447e-05, - -1.00328708454487e-05, 0.000138142092498215}, - {9.54962966738358, 1.83809145920811, 1.82162819067959, -0.0116786627338505, - -0.00496037444422313, 0.0590883547819332, 7.48465315787857e-05, 0.000221693951602584, - 7.96466345174136e-06, 0.000638822537725177}, - {7.04862901290925, 0.876813777672465, 0.16368093989381, 0.00928717461441627, - -0.00276538956293246, 0.00117995419940653, -0.000141511492474493, -6.09796031786385e-06, - -2.62114930414747e-05, -2.88713611443788e-06}, - {135.349147631811, -7.21933296299596, -6.02379024934871, 0.19557354282067, 0.207680233512614, - 0.12880101618361, -0.00169832076532024, -0.00192216719797732, -0.00188763612041332, - -0.00103101801961442}}; - float sus7coeffBeta[9][10] = { - {-12.7115487367622, -1.08890790360556, 0.0579616268854079, -0.0212303293514951, - -0.0395948453851818, -0.0275564242614342, -0.000228652851842222, -0.000148106159109458, - -0.000555136649469199, -0.000198260004582737}, - {-0.988147625946871, -0.759018567468546, 1.20998292002818, -0.0241231836977845, - -0.000572110443300516, -0.00294835038249426, -0.00026533039022186, 6.82250069765274e-06, - 7.21038415209318e-06, -6.54881435118179e-05}, - {98.0979345921564, 4.27381413621355, -4.39956005193548, 0.0709109587666745, -0.172774236139236, - 0.107243391488741, 0.000421832640471043, -0.00140450884710288, 0.00158019019392239, - -0.00078512547169536}, - {4.10892685652543, -0.229301778557857, 1.33380992987117, -0.000250095848720304, - -0.00555205065514645, 0.00355052914398176, 1.62727119770752e-05, -1.26026527654764e-05, - -3.25505031810898e-05, 5.79970895921158e-06}, - {3.09432502337258, -0.300556003790433, 1.17085811008124, 0.00128679594824324, - 0.00148229981422985, 9.15267474159147e-05, 0.000300497843413856, 6.31378865575566e-05, - 0.000258447032558814, 9.79142983264352e-05}, - {8.92336134924575, -0.197306981784312, 0.659908505354084, 0.00175572239373996, - 0.006801023678097, 0.0189775987436792, 9.2187857727721e-06, -4.8706332690626e-05, - -6.887009887486e-05, -0.000266455617735054}, - {-52.0734887320227, 2.64822385560272, -1.72387600304694, -0.0383944891609251, 0.110873671161269, - -0.0475247245070445, 0.000194652401328063, -0.000697307928990137, 0.00124021816001, - -0.000194213899980878}, - {2.08203985879155, -0.127503525368396, 1.17628056094647, 0.00283288065938444, - 0.00394668214608305, 0.00314868636161131, -2.99504350569853e-05, -7.11070816314279e-05, - -6.30148122529749e-05, 2.28114298989664e-05}, - {191.321181158032, -12.2449557187473, -7.21933741885107, 0.267954293388644, 0.331529493933124, - 0.149867703984027, -0.00222279201444128, -0.00284724570619954, -0.00298774060233964, - -0.000988903783752156}}; - float sus8coeffAlpha[9][10] = { - {5.46354311880959, 1.15370126035432, 0.568432485840475, -0.00105094692478431, - -0.000472899673842554, 0.015581320536192, 2.26460844314248e-05, -0.000254397947062058, - 0.000198938007250408, 0.000102026690279006}, - {8.8976133108173, 1.89502416095352, 0.268670471819199, 0.0217013413241972, 0.00973925295182384, - -0.00116357269193765, 0.000185865842232419, 0.000103311614912702, -2.46539447920969e-05, - -2.06292928734686e-05}, - {-45.4550803910752, 1.27220123406993, 5.21483855848504, 0.0315791081623634, 0.0725172355124129, - -0.13947591535243, 0.000412577580637848, 0.000434545096994917, -0.000840043932292312, - 0.00126857487044307}, - {1.81302768546433, 1.20563501267535, 0.344815267182167, 0.00546879453241056, - -0.00115382996865884, 0.010597876132341, -7.75885604486581e-05, 8.99568815949154e-05, - -2.98129544974679e-06, 0.000108913239345604}, - {2.19111439539173, 1.06951675598148, 0.283707798607213, 0.00016478588207518, - 0.000196086067268121, -0.00214980231173703, 0.000237820475654357, -0.000256402967908595, - 0.000165966620658577, -0.000268394081675921}, - {15.0858674915897, 1.27922724811168, -1.0803137812576, -0.00184009775302466, - -0.00458792284209219, 0.0359393555418547, -6.05121024079603e-05, -1.2288384024143e-05, - 8.55484605384438e-05, -0.000379241348638065}, - {-14.9594190080906, 1.79473182195746, -1.00830704063572, 0.000890685410857856, - 0.0408932029176081, -0.0165460857151619, -0.000170544299916973, -0.000370901607010145, - 0.000324089709129097, -9.33010240878062e-05}, - {0.867614491733251, 1.38248194737027, 0.233408537422123, -0.00772942878114575, - -0.00783126068079782, -0.000413713955432221, 4.5775750146291e-05, 6.97323029940275e-05, - 1.70664456940787e-05, 6.75517901233086e-06}, - {2.34474364146174, -0.777275400251477, 2.09531381577911, 0.0170780716714389, 0.102855060371092, - -0.1203441505925, 0.000187004964420911, -0.00141720441050986, -0.000336251285258365, - 0.00145175125888695}}; - float sus8coeffBeta[9][10] = { - {28.3033101237397, 1.77504446792811, 1.70758838986317, 0.0307800697044683, 0.0598759344275936, - -0.014461432284373, 0.000128415617799076, 0.000664419128546701, 0.000312923304130995, - -0.000269026446641855}, - {7.73040563051023, 0.0267291479555493, 1.16189582308493, 0.000611047892976521, - -0.00213680506915073, -0.00517435586596902, -3.60304406049766e-06, -1.74452976404459e-05, - -3.95396925228538e-05, -7.01948519410633e-05}, - {-48.0766126130725, -3.77981206700298, 3.03482861087335, -0.0678496412519532, 0.115260678424016, - -0.0109681510065038, -0.000438011443691466, 0.00097230136258486, -0.000930875177732769, - -0.000203144239955507}, - {12.1881935626341, -0.234345089308583, 2.01134619426134, 0.000181529284001169, - -0.00642848065105061, 0.0243985799415726, 2.0224042581776e-05, 5.22503286757285e-06, - -4.75196303016323e-05, 0.000221160482364556}, - {3.49559433498742, -0.294995112674766, 1.07892379698257, 0.000861664794052587, - 0.00138978933062055, 0.000436385106465176, 0.000288095124755908, 0.000147259769247883, - 0.000256686898599516, 0.000198982412957039}, - {9.36663996178607, -0.171266136751803, 0.799869891484541, -0.000896305696610864, - 0.00477919972789653, 0.0077876110326094, 9.16475263625076e-06, 3.02461250100473e-05, - -3.63917701783264e-05, -0.000101376940843402}, - {9.93372683055145, 1.02056557854246, 3.01635426591734, -0.0477113881140277, -0.0280426434406976, - 0.0438813017696874, 0.000470431190169998, -7.55753674679743e-05, -0.000516444906323815, - 0.000253844418223843}, - {4.12868774589741, -0.305711790187688, 1.15739216407191, 0.00942395115281056, - 0.00264758462357433, 0.00227985850688479, -0.000107537164019682, -4.91396736189963e-05, - -5.3803493814502e-05, 6.80587059728718e-06}, - {64.9193383444005, -1.57724255547465, -3.82166532626293, 0.0104712238987591, 0.0898786950946473, - 0.128910456296131, -8.27123227422217e-05, -0.000143979624107479, -0.00146684876653306, - -0.00102226799570239}}; - float sus9coeffAlpha[9][10] = { - {65.8975109449121, 2.19115342242175, 6.11069527811832, -0.0219884864133703, 0.119985456538482, - 0.142746712551924, -0.000465882328687976, 0.000606525132125852, 0.00141667074621881, - 0.00109715845894006}, - {5.70337356029945, 1.86705636976809, 0.235584190291708, 0.0194937327615426, 0.00973291465247784, - -0.00155675297510773, 0.000147099297988423, 0.000115708967219349, -4.1462310493722e-05, - -9.80097031103588e-06}, - {138.221145997284, 6.07665575619595, -9.08085914250542, 0.0839801072927519, -0.143071750033303, - 0.237868300719915, 0.000626693630444932, -0.000579788170871402, 0.00181740650944343, - -0.00207086879728281}, - {-7.78295582666151, 1.37506685179192, -0.507596181420042, 0.00350118305456038, - 0.00380814310115541, -0.0174012437563343, -0.000124801268056815, 2.96314830184492e-05, - 6.3416992450033e-05, -0.000190177262510221}, - {0.13102597129751, 1.24228303845143, 0.328808873447393, 2.6858679536165e-05, - 0.000231428138164498, -0.000584089095259736, 5.5322167970451e-05, -0.000322205709821716, - 7.71348293209208e-05, -0.000393885990364776}, - {4.64571633968935, 1.2668223691397, -0.158952088650432, -0.0038344859267428, 0.0030051503726095, - 0.00455578826025588, -9.42520993914957e-05, 5.81633314412289e-05, -4.43545804544095e-05, - -4.83524454851519e-05}, - {99.2385930314563, -3.65569343617926, 5.54203926675588, 0.0975630395981933, -0.15701634159692, - 0.107834711298836, -0.000885326636237814, 0.000960753844480462, -0.00179894024848343, - 0.000583066757644971}, - {2.82671549736619, 1.11214198870501, 0.214735318432744, 0.00284415167563662, - -0.00743289575690122, 0.000382705440762292, -7.43232442872501e-05, 6.96994098083348e-05, - -4.15108111710131e-06, 1.33520085213482e-05}, - {36.9013743125415, -0.522392401546163, -1.52452843963663, 0.0261375433218879, 0.060573568610239, - 0.0182582125221054, -0.000244373383911157, -0.000271385147292484, -0.000723799969427732, - 6.76324880239196e-05}}; - float sus9coeffBeta[9][10] = { - {128.70886435409, 7.27355509732751, 7.18142203531244, 0.1536100459329, 0.199455846541636, - 0.101824964939793, 0.00116666116789421, 0.00181595584079788, 0.00159271319494017, - 0.000556768406475719}, - {-7.07933839681024, -0.979062424441878, 1.21792546815617, -0.0295740143783226, - -0.00442780611714201, -0.00329612819203176, -0.000291373125216143, -7.47259350176359e-05, - -4.87265282482212e-05, -7.87490350444332e-05}, - {41.1357193180502, 2.75138456414254, -0.0160889117718198, 0.0274001112562423, - -0.145644717742057, -0.0316076203283094, -0.000136443337244472, -0.00153945199081365, - 0.000938960439977633, 0.000599987111822885}, - {2.7980384746608, -0.234741037383589, 1.5342193016705, -0.000993791566721689, - -0.00787533639513478, 0.00927468655141365, 2.63308697896639e-05, -3.42816267184975e-05, - -8.48879419798771e-05, 3.84043821333798e-05}, - {0.427687530667804, -0.346076633694936, 1.22968527483851, -4.95098138311122e-05, - 0.000298245372198029, 0.000332756250024796, 0.00040375986210644, 5.20675972504572e-05, - 0.000327042170278218, 5.93011568264671e-05}, - {4.50337810133314, -0.279364254817202, 0.945812187846199, 0.000116182663432306, - 0.0115646046622083, 0.00908289960302886, 1.90394667311541e-05, -4.4360223646434e-06, - -0.000131398914898614, -0.000145568992865512}, - {-36.3377213654193, 2.21047221783626, 0.0609982245149821, -0.0670546774988572, - 0.016827777144747, -0.0277834084058314, 0.000778301409125556, 0.000135846745194401, - 0.00043261858797068, -0.00021172728254561}, - {-0.737678205841529, -0.217352122193475, 1.23494846329297, 0.00748173441779792, - 0.0019595873704705, 0.00567253723266176, -8.34768773292938e-05, -3.50608394184873e-05, - -0.000107500091550635, -5.1379722947632e-07}, - {-36.6150844777671, 3.24952006904945, 1.7222457840185, -0.0846362445435584, -0.0625549615377418, - 0.019178365782485, 0.000664877496455304, 0.000942971403881222, 0.000190754698755098, - -0.000372226659190439}}; - float sus10coeffAlpha[9][10] = { - {14.4562393748324, 0.669162330324919, 2.13895255446541, -0.0161997097021299, - 0.00185995785065838, 0.0621351118528379, -0.000278999272493087, 0.000238469666491965, - -0.000279407497782961, 0.000726904943739837}, - {-4.45678285887022, 0.92869611919737, 0.186752102727282, -0.00706160758952316, - 0.00532680276723634, -0.00119102617674229, -0.000105283880098953, 3.90673052334419e-05, - -3.13338277344246e-05, 5.32977236959767e-06}, - {30.4255268053197, 3.00991076401191, -1.4855621363519, 0.033934286288413, -0.0553588742704929, - 0.0299275582316466, 0.000167915322354466, -0.00050925078118232, 0.000463662961330962, - -0.000232919143454163}, - {2.45076465343337, 1.30206564388838, 0.635121046212765, 0.00517109639797675, - 0.00360579544364496, 0.0198490668911362, -9.31556816982662e-05, 6.7313653707875e-05, - 6.4669137025142e-05, 0.000209727581169138}, - {-0.784841314851562, 1.10058314980836, 0.314063830836532, 0.000583003703415889, - 0.000312635453606579, -0.000183738114552387, 0.000214096205760617, -0.000286744686021244, - 0.000159157597180407, -0.00032235099420715}, - {7.19568036510586, 1.33307479701657, -0.465585141952456, -0.0031910726544199, - -0.00546273504371797, 0.0145494754402526, -7.9863949693769e-05, 4.83681329120104e-05, - 8.85844309936609e-05, -0.000143217870916994}, - {-12.8344546267449, 1.36023633150143, -0.728527724854506, 0.019982118403416, 0.0385056413989437, - -0.00468598272326268, -0.000303957957649245, -6.37783846968216e-05, 0.000514049116643205, - 0.000112015427600697}, - {-2.58279031298065, 1.42167821629586, 0.208769467511292, -0.00640190372145885, - -0.0056405289717473, 0.000509611313918708, 2.23310562107823e-05, 3.23685469522147e-05, - -7.55982776243849e-06, 2.78417756661088e-06}, - {-29.7178996143914, 2.636972251183, 1.97316329325243, -0.03983524158327, -0.0193152048730234, - -0.0600902798379509, 0.00031786916010672, 0.000162178988605602, 0.000224550786416246, - 0.000614337977361927}}; - float sus10coeffBeta[9][10] = { - {12.4771349792459, 1.44317849705414, 0.975637226331561, 0.0430284146301043, 0.0220810531548995, - -0.0220926906772, 0.000310052324529521, 0.000658151808869523, -0.000288026365111098, - -0.000214619731807045}, - {-0.113203260140131, -0.272424061092191, 1.27704377191184, -0.00791746619331075, - 0.00278646694862191, -0.00398881099259934, -8.09569694307212e-05, 5.99617384829016e-05, - -5.4550919751855e-05, -8.6314530565085e-05}, - {-48.585664295448, -2.04899787231903, 4.48757129623549, -0.0226180460431321, 0.090326735447661, - -0.0722998813632622, -6.77623771415477e-05, 0.000562585419036509, -0.000956171370931993, - 0.000491554402311223}, - {-1.20986884955482, -0.215604107185474, 1.22123198786617, 0.000256508527822089, - -0.00625056735692847, 0.00262961582224303, 2.27433984698861e-05, 1.60471509861372e-05, - -4.85061736834262e-05, -1.8387092782907e-06}, - {-0.250205907903409, -0.315819331560782, 1.09018364376391, -0.000521787614293089, - -0.000500747760913489, 2.48184307342838e-05, 0.000313799238640988, 0.000136669146368744, - 0.000278914324565192, 0.000218512838469476}, - {-1.38512578184076, -0.240456589364121, 1.34170304231345, 0.00017499230372669, - 0.0070862275911073, -0.00460640844814105, 1.27594111036696e-05, -4.73855624902052e-06, - -5.41141037812903e-05, 8.50767021818388e-06}, - {58.9917559342216, -2.28705697628345, 5.35995190407842, 0.0214721399750612, -0.112195722921667, - 0.0890150265857542, -0.000100675657768708, 0.000493488022135339, -0.00137672908303878, - 0.000518683157694955}, - {3.18905073365834, -0.633376549706314, 1.17190259811174, 0.0188817945597344, - 0.00107470708915782, 0.00400880471375267, -0.000197312295539118, -2.46543035998379e-05, - -6.07871064300252e-05, 1.91822310311955e-05}, - {-21.6881499304099, -0.563186103920008, 3.70747028664292, 0.021112883967427, - -0.00650020689049325, -0.0778533644688476, -0.000131921888670268, -0.000402754836445439, - 0.000551249824375055, 0.00062236627391337}}; - float sus11coeffAlpha[9][10] = { - {-5.23569698615548, -1.45500092391928, 2.7643243644756, -0.0762912296128707, - -0.0201645929971608, 0.0997226845779083, -0.000741669441569556, -0.000485368004931528, - 0.000166230212359982, 0.00103455037278067}, - {-7.7405077383712, 0.892040861541276, 0.39014957203484, -0.00952030929935314, - 0.0185577462685363, 0.000500600568760257, -0.000151227821554572, 0.000245334737283439, - 1.89380065823205e-05, 1.83481122973969e-07}, - {-27.462143709831, -1.68192474287299, 0.689411302961069, -0.0146021086710062, 0.227153492753605, - 0.0815806579791421, 2.92919049429149e-05, 0.00153760357651792, -0.00247865821192621, - -0.00166333309739387}, - {-6.74664748624368, 1.43279156053015, 0.0212787292914553, 0.00764792230365732, - 0.00796410301290615, 0.0014384998868733, -8.95239151813685e-05, 9.55245417090909e-05, - 0.000127251739461239, 3.26943341606363e-05}, - {-2.20391533717674, 1.32902400478083, 0.38633027011889, 0.00104660852197061, - 0.00105228824412283, -0.00242067551428214, -6.98346290136652e-05, -0.000369075232184835, - -1.59510520000704e-05, -0.000448565104826966}, - {-5.29476778147188, 1.4549556336236, 0.561334186252557, -0.00260896342687109, - -0.00855934179001141, -0.0182515354646452, -8.79385828606048e-05, 5.98357681659175e-05, - 0.000146570207542509, 0.000201596912834554}, - {-45.7906613832612, 3.77630104475902, -1.67494598155515, -0.0545433897761635, 0.047897938410221, - -0.0355687158405231, 0.000374738707508583, -0.000448453494537518, 0.000377784972619365, - -0.000276573228333836}, - {-9.11681182090372, 2.06933872940742, 0.26131496122122, -0.0259534033367855, - -0.00777266937872862, -0.00262135395843891, 0.000223790782655445, 6.40488537928934e-05, - 7.75581514100296e-05, -9.25934285039627e-06}, - {183.243883340634, -8.02281039502717, -10.0756951652703, 0.168750521462303, 0.314006821405967, - 0.200264755034566, -0.0011895153717447, -0.00253812476819896, -0.00291324393641628, - -0.00140062522117514}}; - float sus11coeffBeta[9][10] = { - {34.4499366074013, -0.438583698052091, 4.72111001451028, -0.041810050989433, 0.0562461093661426, - 0.0856849892524893, -0.000477813051406167, -3.16404257494464e-05, 0.00102633196865105, - 0.000552974013759876}, - {7.6366298088699, 0.150314752958302, 1.31364679484924, 0.00557696667395871, 0.00163731860604376, - -0.00454759608980269, 5.83979683674572e-05, 4.45944881220665e-05, -4.27874746147066e-05, - -8.77418673597557e-05}, - {130.156215011893, 1.85759000444524, -10.986892391833, -0.00686275191260681, -0.188837138116058, - 0.346177462085361, -0.000183276946352264, -0.000702183496893294, 0.00293145272693857, - -0.00318194442670715}, - {-1.67854820161036, -0.358899332859806, 0.956690839640595, -4.93862910503803e-05, - -0.0136134783014874, -0.00848731301504507, 3.75950499927045e-05, 1.35374694383289e-06, - -0.000156596507890443, -0.000123254220377897}, - {3.67569209537767, -0.387260959713287, 1.31343215605952, -0.00206444615206506, - 0.00145334813110285, -0.00151259497696238, 0.000449492568365603, 6.95883968949488e-07, - 0.000368585523744765, -6.3420715525635e-05}, - {14.3296323024886, -0.182979476956897, 0.306817119309235, -0.00022212115978293, - 0.00463485302909649, 0.0277574953550035, 1.1422454625565e-05, 1.06053257479502e-05, - -2.05720000720608e-05, -0.000338584671430337}, - {-18.7534921817754, 1.14272710923224, 0.460498062012866, -0.00995826989278202, - 0.0658502318647112, 0.00616942819937029, -7.70857153768402e-05, -0.000641755741925561, - 0.00047849204592989, 0.000158509018296766}, - {1.26543621388607, -0.176674379740481, 1.38814920935488, 0.00545485262295305, - -0.00499775616702264, 0.0038057039142173, -6.59604252054511e-05, 6.40211116049053e-05, - -6.74778593434431e-05, -2.81973589469059e-05}, - {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; - float filterAlpha; - float sunThresh; - } susHandlingParameters; + struct GyrHandlingParameters { + double gyr0orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}; + 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}}; + } gyrHandlingParameters; - struct RmuHandlingParameters { - double rmu0orientationMatrix[3][3]; - double rmu1orientationMatrix[3][3]; - double rmu2orientationMatrix[3][3]; - } rmuHandlingParameters; + 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] + } rwHandlingParameters; - 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] - } rwHandlingParameters; + struct RwMatrices { + 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}; + } rwMatrices; - struct RwMatrices { - 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}; - } rwMatrices; + struct SafeModeControllerParameters { + double k_rate_mekf = 0.00059437; + double k_align_mekf = 0.000056875; - 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; - double k_align_no_mekf; - double sunMagAngleMin; + double sunTargetDir[3] = {1, 0, 0}; // Body frame + double satRateRef[3]; // Body frame - double sunTargetDir[3] = { 1, 0, 0}; //Body frame - double satRateRef[3]; //Body frame + } safeModeControllerParameters; - } safeModeControllerParameters; + struct DetumbleCtrlParameters { + double gainD = pow(10.0, -3.3); - struct DetumbleCtrlParameters { + } detumbleCtrlParameters; - double gainD = pow(10.0,-3.3); + // ToDo: mutiple structs for different pointing mode controllers? + struct PointingModeControllerParameters { + double updtFlag; + double A_rw[3][12]; - } detumbleCtrlParameters; + double refDirection[3] = {1, 0, 0}; + double refRotRate[3] = {0, 0, 0}; + double quatRef[4] = {0, 0, 0, 1}; + bool avoidBlindStr = true; + double blindAvoidStart = 1.5; + double blindAvoidStop = 2.5; + double blindRotRate = 1 * M_PI / 180; - // ToDo: mutiple structs for different pointing mode controllers? - struct PointingModeControllerParameters { - double updtFlag; - double A_rw[3][12]; + 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; - double refDirection[3] = { 1, 0, 0}; - double refRotRate[3] = { 0, 0, 0}; - double quatRef[4] = { 0, 0, 0, 1}; - bool avoidBlindStr = true; - double blindAvoidStart = 1.5; - double blindAvoidStop = 2.5; - double blindRotRate = 1 * M_PI /180; + double desatMomentumRef[3] = {0, 0, 0}; + double deSatGainFactor = 1000; + bool desatOn = true; - 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; + double omegaEarth = 0.000072921158553; - double desatMomentumRef[3] = {0,0,0}; - double deSatGainFactor = 1000; - bool desatOn = true; + } inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters; - double omegaEarth = 0.000072921158553; + 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 + } strParameters; + struct GpsParameters { + } gpsParameters; - } inertialModeControllerParameters, nadirModeControllerParameters, - targetModeControllerParameters; + 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 + 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 + 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.; - 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 - } strParameters; + float p1 = 6892. / 3600. * M_PI / 180.; // some parameter + float p2 = 72. / 3600. * M_PI / 180.; // some parameter + } sunModelParameters; - struct GpsParameters { - } gpsParameters; + struct KalmanFilterParameters { + uint8_t activateKalmanFilter; + uint8_t requestResetFlag; + double maxToleratedTimeBetweenKalmanFilterExecutionSteps; + double processNoiseOmega[3]; + double processNoiseQuaternion[4]; - 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 - double earthRadiusEquat = 6378137; // [m] - double earthRadiusPolar = 6356752.314; // [m] - } groundStationParameters; // Stuttgart + double sensorNoiseSTR = 0.1 * M_PI / 180; + double sensorNoiseSS = 8 * M_PI / 180; + double sensorNoiseMAG = 4 * M_PI / 180; + double sensorNoiseRMU[3]; - 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 - 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.; + double sensorNoiseArwRmu; // Angular Random Walk + double sensorNoiseBsRMU; // Bias Stability + } kalmanFilterParameters; - float p1 = 6892. / 3600. * M_PI / 180.; //some parameter - float p2 = 72. / 3600. * M_PI / 180.; //some parameter - } sunModelParameters; + struct MagnetorquesParameter { + double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}}; + double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; + double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; + double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; + double DipolMax = 0.2; // [Am^2] - struct KalmanFilterParameters { - uint8_t activateKalmanFilter; - uint8_t requestResetFlag; - double maxToleratedTimeBetweenKalmanFilterExecutionSteps; - double processNoiseOmega[3]; - double processNoiseQuaternion[4]; + } magnetorquesParameter; - double sensorNoiseSTR = 0.1 * M_PI / 180; - double sensorNoiseSS = 8 * M_PI / 180; - double sensorNoiseMAG = 4 * M_PI / 180; - double sensorNoiseRMU[3]; - - double sensorNoiseArwRmu; //Angular Random Walk - double sensorNoiseBsRMU; // Bias Stability - } kalmanFilterParameters; - - struct MagnetorquesParameter { - - double mtq0orientationMatrix[3][3] = { - { 1, 0, 0}, - { 0, 0, 1}, - { 0,-1, 0}}; - double mtq1orientationMatrix[3][3] = { - { 1, 0, 0}, - { 0, 1, 0}, - { 0, 0, 1}}; - double mtq2orientationMatrix[3][3] = { - { 0, 0, 1}, - { 0, 1, 0}, - {-1, 0, 0}}; - double alignmentMatrixMtq[3][3] = { - { 0, 0,-1}, - {-1, 0, 0}, - { 0, 1, 0}}; - double inverseAlignment[3][3] = { - { 0,-1, 0}, - { 0, 0, 1}, - {-1, 0, 0}}; - double DipolMax = 0.2; // [Am^2] - - } magnetorquesParameter; - - struct DetumbleParameter { - - uint8_t detumblecounter; - double omegaDetumbleStart; - double omegaDetumbleEnd; - } detumbleParameter; + struct DetumbleParameter { + uint8_t detumblecounter; + double omegaDetumbleStart; + double omegaDetumbleEnd; + } detumbleParameter; }; #endif /* ACSPARAMETERS_H_ */ From 0f06219fd3a190fd35b11174d951ce91e16427b2 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 11 Oct 2022 14:57:58 +0200 Subject: [PATCH 049/244] changed initial Mgm values to arbitrary other than 0 values --- dummies/ImtqDummy.cpp | 2 +- dummies/MgmLIS3MDLDummy.cpp | 3 ++- dummies/MgmRm3100Dummy.cpp | 3 ++- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 8765e978..331a6201 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -41,7 +41,7 @@ ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataP localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0,0.0,0.0})); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry({0.0,0.0,0.0})); + localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry({0.12, 0.76, -0.45}, true)); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); return returnvalue::OK; } diff --git a/dummies/MgmLIS3MDLDummy.cpp b/dummies/MgmLIS3MDLDummy.cpp index b89d74b8..31228260 100644 --- a/dummies/MgmLIS3MDLDummy.cpp +++ b/dummies/MgmLIS3MDLDummy.cpp @@ -41,6 +41,7 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); - localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, new PoolEntry({0.0, 0.0, 0.0})); + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, + new PoolEntry({1.02, 0.56, -0.78}, true)); return returnvalue::OK; } diff --git a/dummies/MgmRm3100Dummy.cpp b/dummies/MgmRm3100Dummy.cpp index cb27209c..1bd4f513 100644 --- a/dummies/MgmRm3100Dummy.cpp +++ b/dummies/MgmRm3100Dummy.cpp @@ -36,6 +36,7 @@ uint32_t MgmRm3100Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { ReturnValue_t MgmRm3100Dummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, new PoolEntry({0.0, 0.0, 0.0})); + localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, + new PoolEntry({0.87, -0.95, 0.11}, true)); return returnvalue::OK; } From b2a715ef6ad060eee9d5a911e7c3db4dba9186f7 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 11 Oct 2022 15:00:37 +0200 Subject: [PATCH 050/244] added GYR dummies to acsTask. Added complete GYR dataSets to the dummies --- bsp_q7s/core/InitMission.cpp | 4 ++++ dummies/GyroAdisDummy.cpp | 7 +++++++ dummies/GyroL3GD20Dummy.cpp | 7 ++++--- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index 5bac228b..b502c5a4 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -157,6 +157,10 @@ void initmission::initTasks() { acsTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); acsTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); acsTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); + acsTask->addComponent(objects::GYRO_0_ADIS_HANDLER); + acsTask->addComponent(objects::GYRO_1_L3G_HANDLER); + acsTask->addComponent(objects::GYRO_2_ADIS_HANDLER); + acsTask->addComponent(objects::GYRO_3_L3G_HANDLER); #endif PeriodicTaskIF* sysTask = factory->createPeriodicTask( diff --git a/dummies/GyroAdisDummy.cpp b/dummies/GyroAdisDummy.cpp index 42af6412..050c838d 100644 --- a/dummies/GyroAdisDummy.cpp +++ b/dummies/GyroAdisDummy.cpp @@ -40,6 +40,13 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({-0.5}, true)); + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.2}, true)); + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({-1.2}, true)); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); + return returnvalue::OK; } diff --git a/dummies/GyroL3GD20Dummy.cpp b/dummies/GyroL3GD20Dummy.cpp index 935e32f5..a800cd62 100644 --- a/dummies/GyroL3GD20Dummy.cpp +++ b/dummies/GyroL3GD20Dummy.cpp @@ -38,11 +38,12 @@ void GyroL3GD20Dummy::fillCommandAndReplyMap() {} uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; } + ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({1.2}, true)); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({-0.1}, true)); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.7}, true)); localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); return returnvalue::OK; } From 46dd2b92e50da6ab7524617f6f4805704d444c94 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 11 Oct 2022 15:01:32 +0200 Subject: [PATCH 051/244] renamed RMU related stuff to GYR. added GYR processing --- mission/controller/acs/SensorProcessing.cpp | 96 +++++++++++++-------- mission/controller/acs/SensorProcessing.h | 14 ++- mission/controller/acs/SensorValues.cpp | 31 ++++--- mission/controller/acs/SensorValues.h | 20 ++--- 4 files changed, 100 insertions(+), 61 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 749ec373..3c6a3564 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -353,62 +353,79 @@ void SensorProcessing::processSus( *sunVectorInertialValid = true; } -void SensorProcessing::processRmu(const double rmu0Value[], bool rmu0valid, - const double rmu1Value[], bool rmu1valid, - const double rmu2Value[], bool rmu2valid, - timeval timeOfrmuMeasurement, - const AcsParameters::RmuHandlingParameters *rmuParameters, - double *satRatEst, bool *satRateEstValid) { - if (!rmu0valid && !rmu1valid && !rmu2valid) { +void SensorProcessing::processGyr( + const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid, + const double gyr0axZvalue, bool gyr0axZvalid, const double gyr1axXvalue, bool gyr1axXvalid, + const double gyr1axYvalue, bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid, + const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid, + const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid, + const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, + timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters, + double *satRatEst, bool *satRateEstValid) { + if (!gyr0axXvalid && !gyr0axYvalid && !gyr0axZvalid && !gyr1axXvalid && !gyr1axYvalid && + !gyr1axZvalid && !gyr2axXvalid && !gyr2axYvalid && !gyr2axZvalid && !gyr3axXvalid && + !gyr3axYvalid && !gyr3axZvalid) { *satRateEstValid = false; return; } // Transforming Values to the Body Frame (actually it is the geometry frame atm) - double rmu0ValueBody[3] = {0, 0, 0}, rmu1ValueBody[3] = {0, 0, 0}, rmu2ValueBody[3] = {0, 0, 0}; + double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0}, + gyr3ValueBody[3] = {0, 0, 0}; - bool validUnit[3] = {false, false, false}; + bool validUnit[4] = {false, false, false, false}; uint8_t validCount = 0; - if (rmu0valid) { - MatrixOperations::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value, - rmu0ValueBody, 3, 3, 1); + if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) { + const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, + gyr0ValueBody, 3, 3, 1); validCount += 1; validUnit[0] = true; } - if (rmu1valid) { - MatrixOperations::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value, - rmu1ValueBody, 3, 3, 1); + if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) { + const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, + gyr1ValueBody, 3, 3, 1); validCount += 1; validUnit[1] = true; } - if (rmu2valid) { - MatrixOperations::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value, - rmu2ValueBody, 3, 3, 1); + if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) { + const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, + gyr2ValueBody, 3, 3, 1); validCount += 1; validUnit[2] = true; } + if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) { + const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, + gyr3ValueBody, 3, 3, 1); + validCount += 1; + validUnit[3] = true; + } /* -------- SatRateEst: Middle Value ------- */ - double rmuValues[3][3] = {{rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]}, - {rmu0ValueBody[1], rmu1ValueBody[1], rmu2ValueBody[1]}, - {rmu0ValueBody[2], rmu1ValueBody[2], rmu2ValueBody[2]}}; - double rmuValidValues[3][validCount]; + double gyrValues[3][4] = { + {gyr0ValueBody[0], gyr1ValueBody[0], gyr2ValueBody[0], gyr3ValueBody[0]}, + {gyr0ValueBody[1], gyr1ValueBody[1], gyr2ValueBody[1], gyr3ValueBody[1]}, + {gyr0ValueBody[2], gyr1ValueBody[2], gyr2ValueBody[2], gyr3ValueBody[2]}}; + double gyrValidValues[3][validCount]; uint8_t j = 0; for (uint8_t i = 0; i < validCount; i++) { if (validUnit[i]) { - rmuValidValues[0][j] = rmuValues[0][i]; - rmuValidValues[1][j] = rmuValues[1][i]; - rmuValidValues[2][j] = rmuValues[2][i]; + gyrValidValues[0][j] = gyrValues[0][i]; + gyrValidValues[1][j] = gyrValues[1][i]; + gyrValidValues[2][j] = gyrValues[2][i]; j += 1; } } // Selection Sort - double rmuValidValuesSort[3][validCount]; - MathOperations::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount); + double gyrValidValuesSort[3][validCount]; + MathOperations::selectionSort(*gyrValidValues, *gyrValidValuesSort, 3, validCount); uint8_t n = ceil(validCount / 2); - satRatEst[0] = rmuValidValuesSort[0][n]; - satRatEst[1] = rmuValidValuesSort[1][n]; - satRatEst[2] = rmuValidValuesSort[2][n]; + satRatEst[0] = gyrValidValuesSort[0][n]; + satRatEst[1] = gyrValidValuesSort[1][n]; + satRatEst[2] = gyrValidValuesSort[2][n]; *satRateEstValid = true; } @@ -467,8 +484,19 @@ void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now, &outputValues->sunVectorDerivativeValid); // VALID outputs ? - // processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, - // sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now, - // &acsParameters->rmuHandlingParameters, outputValues->satRateEst, - // &outputValues->satRateEstValid); + processGyr( + sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), + sensorValues->gyr0AdisSet.angVelocY.value, sensorValues->gyr0AdisSet.angVelocY.isValid(), + sensorValues->gyr0AdisSet.angVelocZ.value, sensorValues->gyr0AdisSet.angVelocZ.isValid(), + sensorValues->gyr1L3gSet.angVelocX.value, sensorValues->gyr1L3gSet.angVelocX.isValid(), + sensorValues->gyr1L3gSet.angVelocY.value, sensorValues->gyr1L3gSet.angVelocY.isValid(), + sensorValues->gyr1L3gSet.angVelocZ.value, sensorValues->gyr1L3gSet.angVelocZ.isValid(), + sensorValues->gyr2AdisSet.angVelocX.value, sensorValues->gyr2AdisSet.angVelocX.isValid(), + sensorValues->gyr2AdisSet.angVelocY.value, sensorValues->gyr2AdisSet.angVelocY.isValid(), + sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(), + sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(), + sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(), + sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, + &acsParameters->gyrHandlingParameters, outputValues->satRateEst, + &outputValues->satRateEstValid); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index df2b2a37..609c6e6a 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -52,10 +52,16 @@ class SensorProcessing { bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, double *sunVectorDerivative, bool *sunVectorDerivativeValid); - void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu - const double rmu1Value[], bool rmu1valid, const double rmu2Value[], - bool rmu2valid, timeval timeOfrmuMeasurement, - const AcsParameters::RmuHandlingParameters *rmuParameters, double *satRatEst, + void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, + bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid, + const double gyr1axXvalue, bool gyr1axXvalid, const double gyr1axYvalue, + bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid, + const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, + bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid, + const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, + bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, + timeval timeOfGyrMeasurement, + const AcsParameters::GyrHandlingParameters *gyrParameters, double *satRatEst, bool *satRateEstValid); void processStr(); diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 9ca2b90b..3a47937a 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -42,20 +42,25 @@ ReturnValue_t SensorValues::updateSus() { return result; } +ReturnValue_t SensorValues::updateGyr() { + ReturnValue_t result; + PoolReadGuard pgGyr0(&gyr0AdisSet), pgGyr1(&gyr1L3gSet), pgGyr2(&gyr2AdisSet), + pgGyr3(&gyr3L3gSet); + + result = (pgGyr0.getReadResult() || pgGyr1.getReadResult() || pgGyr2.getReadResult() || + pgGyr3.getReadResult()); + return result; +} + ReturnValue_t SensorValues::update() { - updateSus(); - updateMgm(); - - // lp_var_t quaternion(objects::STAR_TRACKER, PoolIds::CALI_QW, nullptr, - // pool_rwm_t::VAR_READ); - // ReturnValue_t result = quaternion.read(); - // - // if (result != RETURN_OK) { - // return RETURN_FAILED; - // } - // quatJB[3] = static_cast(quaternion.value); - // quatJBValid = quaternion.isValid(); - + ReturnValue_t mgmUpdate = updateMgm(); + ReturnValue_t susUpdate = updateSus(); + ReturnValue_t gyrUpdate = updateGyr(); + if ((mgmUpdate && susUpdate && gyrUpdate) == returnvalue::FAILED) { + return returnvalue::FAILED; + }; return returnvalue::OK; } + } // namespace ACS +// namespace ACS diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index daf4913c..5182215d 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,16 +1,18 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ - #include -#include "mission/devices/devicedefinitions/SusDefinitions.h" + #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h" #include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h" +#include "mission/devices/devicedefinitions/SusDefinitions.h" namespace ACS { -class SensorValues{ +class SensorValues { public: SensorValues(); virtual ~SensorValues(); @@ -18,6 +20,7 @@ class SensorValues{ ReturnValue_t update(); ReturnValue_t updateMgm(); ReturnValue_t updateSus(); + ReturnValue_t updateGyr(); MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); @@ -44,13 +47,10 @@ class SensorValues{ SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), }; - double rmu0[3]; - double rmu1[3]; - double rmu2[3]; - - bool rmu0Valid; - bool rmu1Valid; - bool rmu2Valid; + AdisGyroPrimaryDataset gyr0AdisSet = AdisGyroPrimaryDataset(objects::GYRO_0_ADIS_HANDLER); + GyroPrimaryDataset gyr1L3gSet = GyroPrimaryDataset(objects::GYRO_1_L3G_HANDLER); + AdisGyroPrimaryDataset gyr2AdisSet = AdisGyroPrimaryDataset(objects::GYRO_2_ADIS_HANDLER); + GyroPrimaryDataset gyr3L3gSet = GyroPrimaryDataset(objects::GYRO_3_L3G_HANDLER); double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? bool quatJBValid; From 8536c79445eb4152609ab2805155cffbddff89f9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 11 Oct 2022 15:01:44 +0200 Subject: [PATCH 052/244] clean up --- mission/controller/AcsController.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 785a6906..e662948b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -78,10 +78,7 @@ void AcsController::performDetumble() { ACS::SensorValues sensorValues; ACS::OutputValues outputValues; - // sensorValues.read(); - // outputValues.read(); - - timeval now; // = {0,0}; + timeval now; Clock::getClock_timeval(&now); sensorProcessing.process(&susData, now, &sensorValues, &outputValues, &acsParameters); From d929b27c088ffee72cc7ff710bc6437977f6f852 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 10:26:33 +0200 Subject: [PATCH 053/244] emplaced STR quaternion related dataPool. added STR dummy to acsTask --- bsp_q7s/core/InitMission.cpp | 1 + dummies/StarTrackerDummy.cpp | 24 ++++++++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index b502c5a4..9935f9c1 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -161,6 +161,7 @@ void initmission::initTasks() { acsTask->addComponent(objects::GYRO_1_L3G_HANDLER); acsTask->addComponent(objects::GYRO_2_ADIS_HANDLER); acsTask->addComponent(objects::GYRO_3_L3G_HANDLER); + acsTask->addComponent(objects::STAR_TRACKER); #endif PeriodicTaskIF* sysTask = factory->createPeriodicTask( diff --git a/dummies/StarTrackerDummy.cpp b/dummies/StarTrackerDummy.cpp index 8e2dd507..71f0030c 100644 --- a/dummies/StarTrackerDummy.cpp +++ b/dummies/StarTrackerDummy.cpp @@ -41,5 +41,29 @@ uint32_t StarTrackerDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) ReturnValue_t StarTrackerDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(startracker::MCU_TEMPERATURE, new PoolEntry({0})); + + localDataPoolMap.emplace(startracker::TICKS_SOLUTION_SET, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::TIME_SOLUTION_SET, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::CALI_QW, new PoolEntry({1.0}, true)); + localDataPoolMap.emplace(startracker::CALI_QX, new PoolEntry({0.0}, true)); + localDataPoolMap.emplace(startracker::CALI_QY, new PoolEntry({0.0}, true)); + localDataPoolMap.emplace(startracker::CALI_QZ, new PoolEntry({0.0}, true)); + localDataPoolMap.emplace(startracker::TRACK_CONFIDENCE, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::TRACK_QW, new PoolEntry({1.0})); + localDataPoolMap.emplace(startracker::TRACK_QX, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::TRACK_QY, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::TRACK_QZ, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::TRACK_REMOVED, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::STARS_CENTROIDED, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::STARS_MATCHED_DATABASE, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::LISA_QW, new PoolEntry({1.0})); + localDataPoolMap.emplace(startracker::LISA_QX, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::LISA_QY, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::LISA_QZ, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::LISA_PERC_CLOSE, new PoolEntry({0.0})); + localDataPoolMap.emplace(startracker::LISA_NR_CLOSE, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::TRUST_WORTHY, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::STABLE_COUNT, new PoolEntry({0})); + localDataPoolMap.emplace(startracker::SOLUTION_STRATEGY, new PoolEntry({0})); return returnvalue::OK; } From 430646afd05261fec3e666b817c1f376163f125c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 10:27:01 +0200 Subject: [PATCH 054/244] added STR to SensorValues --- mission/controller/acs/SensorValues.cpp | 11 ++++++++++- mission/controller/acs/SensorValues.h | 10 +++++++--- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 3a47937a..b38a35b1 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -52,11 +52,20 @@ ReturnValue_t SensorValues::updateGyr() { return result; } +ReturnValue_t SensorValues::updateStr() { + ReturnValue_t result; + PoolReadGuard pgStr(&strSet); + + result = pgStr.getReadResult(); + return result; +} + ReturnValue_t SensorValues::update() { ReturnValue_t mgmUpdate = updateMgm(); ReturnValue_t susUpdate = updateSus(); ReturnValue_t gyrUpdate = updateGyr(); - if ((mgmUpdate && susUpdate && gyrUpdate) == returnvalue::FAILED) { + ReturnValue_t strUpdate = updateStr(); + if ((mgmUpdate && susUpdate && gyrUpdate && strUpdate) == returnvalue::FAILED) { return returnvalue::FAILED; }; return returnvalue::OK; diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 5182215d..4e3a215f 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -5,6 +5,7 @@ #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" #include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h" #include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h" @@ -21,6 +22,7 @@ class SensorValues { ReturnValue_t updateMgm(); ReturnValue_t updateSus(); ReturnValue_t updateGyr(); + ReturnValue_t updateStr(); MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); @@ -52,9 +54,11 @@ class SensorValues { AdisGyroPrimaryDataset gyr2AdisSet = AdisGyroPrimaryDataset(objects::GYRO_2_ADIS_HANDLER); GyroPrimaryDataset gyr3L3gSet = GyroPrimaryDataset(objects::GYRO_3_L3G_HANDLER); - double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? - bool quatJBValid; - int strIntTime[2]; + startracker::SolutionSet strSet = startracker::SolutionSet(objects::STAR_TRACKER); + + // double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? + // bool quatJBValid; + // int strIntTime[2]; double gps0latitude; // Reference is WGS84, so this one will probably be geodetic double gps0longitude; // Should be geocentric for IGRF From 7be6bbc948d5173f6f47f5853c489ae07a93c373 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 10:27:28 +0200 Subject: [PATCH 055/244] removed deprecated SusData input --- mission/controller/acs/SensorProcessing.cpp | 4 ++-- mission/controller/acs/SensorProcessing.h | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 3c6a3564..bf9894e2 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -443,8 +443,8 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo } } -void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now, - ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, +void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, + ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { sensorValues->update(); // processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 609c6e6a..50b78238 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -23,8 +23,7 @@ class SensorProcessing { SensorProcessing(AcsParameters *acsParameters_); virtual ~SensorProcessing(); - void process(acsctrl::SusDataRaw *susData, timeval now, ACS::SensorValues *sensorValues, - ACS::OutputValues *outputValues, + void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, const AcsParameters *acsParameters); // Will call protected functions private: protected: From 4aecd079707b9f76a899d9e9ba46c45e4e821636 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 10:28:44 +0200 Subject: [PATCH 056/244] added PtgCtrl code. matched inputs to actual SensorValues inputs --- mission/controller/AcsController.cpp | 40 ++++++++++++++-- mission/controller/AcsController.h | 5 ++ mission/controller/acs/Navigation.cpp | 69 ++++++++++++++------------- 3 files changed, 75 insertions(+), 39 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index e662948b..84a7682f 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -1,14 +1,15 @@ #include "AcsController.h" #include -//#include AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId, objects::NO_OBJECT), sensorProcessing(&acsParameters), navigation(&acsParameters), actuatorCmd(&acsParameters), + guidance(&acsParameters), detumble(&acsParameters), + ptgCtrl(&acsParameters), detumbleCounter{0}, mgmData(this), susData(this) {} @@ -42,7 +43,7 @@ void AcsController::performControlOperation() { break; case SUBMODE_PTG_GS: - // performPointingCtrl(); + performPointingCtrl(); break; } } @@ -65,7 +66,6 @@ void AcsController::performControlOperation() { } } - // DEBUG : REMOVE AFTER COMPLETION mode = MODE_ON; submode = SUBMODE_DETUMBLE; @@ -81,7 +81,7 @@ void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); - sensorProcessing.process(&susData, now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); double magMomMtq[3] = {0, 0, 0}; @@ -108,7 +108,37 @@ void AcsController::performDetumble() { } } -void AcsController::performPointingCtrl() {} +void AcsController::performPointingCtrl() { + 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.targetQuatPtg(&sensorValues, &outputValues, now, 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.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); + double rwTrqNs[4] = {0, 0, 0, 0}; + ptgCtrl.ptgNullspace(&(sensorValues.speedRw0), &(sensorValues.speedRw1), &(sensorValues.speedRw2), + &(sensorValues.speedRw3), rwTrqNs); + double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input + actuatorCmd.cmdSpeedToRws(&(sensorValues.speedRw0), &(sensorValues.speedRw1), + &(sensorValues.speedRw2), &(sensorValues.speedRw3), 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.speedRw0), + &(sensorValues.speedRw1), &(sensorValues.speedRw2), + &(sensorValues.speedRw3), mgtDpDes); + actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); +} ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index fa4ba88e..c23adfc1 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -6,9 +6,11 @@ #include #include "acs/ActuatorCmd.h" +#include "acs/Guidance.h" #include "acs/Navigation.h" #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" +#include "acs/control/PtgCtrl.h" #include "controllerdefinitions/AcsCtrlDefinitions.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" @@ -37,8 +39,11 @@ class AcsController : public ExtendedControllerBase { SensorProcessing sensorProcessing; Navigation navigation; ActuatorCmd actuatorCmd; + Guidance guidance; Detumble detumble; + PtgCtrl ptgCtrl; + uint8_t detumbleCounter; enum class InternalState { STARTUP, INITIAL_DELAY, READY }; diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index a2e3aa5d..deb50aa1 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -6,45 +6,46 @@ */ #include "Navigation.h" -#include "util/MathOperations.h" -#include "util/CholeskyDecomposition.h" -#include -#include + #include #include +#include +#include +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" -Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){ - acsParameters = *acsParameters_; +Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) { + acsParameters = *acsParameters_; } -Navigation::~Navigation(){ +Navigation::~Navigation() {} +void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + ReturnValue_t *mekfValid) { + double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, + sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; + bool quatJBValid = + (sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() && + sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid()); + + if (kalmanInit) { + *mekfValid = multiplicativeKalmanFilter.mekfEst( + quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid, + outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst, + &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ, + outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? + } + else { + multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->sunDirEst, &outputValues->sunDirEstValid, + outputValues->sunDirModel, &outputValues->sunDirModelValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid); + kalmanInit = true; + *mekfValid = 0; + + // Maybe we need feedback from kalmanfilter to identify if there was a problem with the init + //of kalman filter where does this class know from that kalman filter was not initialized ? + } } - -void Navigation::useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid){ - - if (kalmanInit) { - *mekfValid = multiplicativeKalmanFilter.mekfEst(sensorValues->quatJB, &sensorValues->quatJBValid, - outputValues->satRateEst, &outputValues->satRateEstValid, - outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid, - outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? - - } - else { - multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid); - kalmanInit = true; - *mekfValid = 0; - -// Maybe we need feedback from kalmanfilter to identify if there was a problem with the init of kalman filter -// where does this class know from that kalman filter was not initialized ? - } -} - - From 2a9dc518a0693bda9fdf0d863c7c551a47da864c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 15:04:19 +0200 Subject: [PATCH 057/244] added GpsDummy --- bsp_q7s/core/InitMission.cpp | 1 + dummies/CMakeLists.txt | 1 + dummies/GpsDummy.cpp | 56 ++++++++++++++++++++++++++++++++++++ dummies/GpsDummy.h | 33 +++++++++++++++++++++ dummies/helpers.cpp | 6 ++-- 5 files changed, 95 insertions(+), 2 deletions(-) create mode 100644 dummies/GpsDummy.cpp create mode 100644 dummies/GpsDummy.h diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index 9935f9c1..e3e26be9 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -161,6 +161,7 @@ void initmission::initTasks() { acsTask->addComponent(objects::GYRO_1_L3G_HANDLER); acsTask->addComponent(objects::GYRO_2_ADIS_HANDLER); acsTask->addComponent(objects::GYRO_3_L3G_HANDLER); + acsTask->addComponent(objects::GPS_CONTROLLER); acsTask->addComponent(objects::STAR_TRACKER); #endif diff --git a/dummies/CMakeLists.txt b/dummies/CMakeLists.txt index 4cb05289..3c46736f 100644 --- a/dummies/CMakeLists.txt +++ b/dummies/CMakeLists.txt @@ -12,6 +12,7 @@ target_sources( AcuDummy.cpp PduDummy.cpp P60DockDummy.cpp + GpsDummy.cpp GyroAdisDummy.cpp GyroL3GD20Dummy.cpp MgmLIS3MDLDummy.cpp diff --git a/dummies/GpsDummy.cpp b/dummies/GpsDummy.cpp new file mode 100644 index 00000000..d1f526d2 --- /dev/null +++ b/dummies/GpsDummy.cpp @@ -0,0 +1,56 @@ +#include "GpsDummy.h" + +#include + +GpsDummy::GpsDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) + : DeviceHandlerBase(objectId, comif, comCookie) {} + +GpsDummy::~GpsDummy() {} + +void GpsDummy::doStartUp() {} + +void GpsDummy::doShutDown() {} + +ReturnValue_t GpsDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } + +ReturnValue_t GpsDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; +} + +ReturnValue_t GpsDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) { + return returnvalue::OK; +} + +ReturnValue_t GpsDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) { + return returnvalue::OK; +} + +ReturnValue_t GpsDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + return returnvalue::OK; +} + +void GpsDummy::fillCommandAndReplyMap() {} + +uint32_t GpsDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; } + +ReturnValue_t GpsDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry({0.0})); + localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry({0.0})); + localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry({0.0})); + localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry({7684.2})); + localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry({0})); + localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry({0})); + + return returnvalue::OK; +} diff --git a/dummies/GpsDummy.h b/dummies/GpsDummy.h new file mode 100644 index 00000000..b32b7f17 --- /dev/null +++ b/dummies/GpsDummy.h @@ -0,0 +1,33 @@ +#ifndef DUMMIES_GPSDUMMY_H_ +#define DUMMIES_GPSDUMMY_H_ + +#include + +class GpsDummy : public DeviceHandlerBase { + public: + static const DeviceCommandId_t SIMPLE_COMMAND = 1; + static const DeviceCommandId_t PERIODIC_REPLY = 2; + + static const uint8_t SIMPLE_COMMAND_DATA = 1; + static const uint8_t PERIODIC_REPLY_DATA = 2; + + GpsDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + virtual ~GpsDummy(); + + protected: + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; +}; + +#endif /* DUMMIES_GPSDUMMY_H_ */ diff --git a/dummies/helpers.cpp b/dummies/helpers.cpp index 46cf207d..74410f56 100644 --- a/dummies/helpers.cpp +++ b/dummies/helpers.cpp @@ -1,10 +1,9 @@ -#include "helpers.h" - #include #include #include #include #include +#include #include #include #include @@ -18,6 +17,8 @@ #include #include +#include "helpers.h" + using namespace dummy; void dummy::createDummies(DummyCfg cfg) { @@ -51,6 +52,7 @@ void dummy::createDummies(DummyCfg cfg) { new MgmLIS3MDLDummy(objects::MGM_2_LIS3_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new MgmRm3100Dummy(objects::MGM_1_RM3100_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); new MgmRm3100Dummy(objects::MGM_3_RM3100_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); + new GpsDummy(objects::GPS_CONTROLLER, objects::DUMMY_COM_IF, comCookieDummy); } if (cfg.addSusDummies) { From 84fc44fd5fcf9d7794f2c5bf222be52fda130430 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 15:06:24 +0200 Subject: [PATCH 058/244] fixed GPS and STR inputs --- mission/controller/AcsController.cpp | 20 +- mission/controller/acs/ActuatorCmd.cpp | 61 +-- mission/controller/acs/ActuatorCmd.h | 7 +- mission/controller/acs/Guidance.cpp | 558 ++++++++++---------- mission/controller/acs/SensorProcessing.cpp | 4 +- mission/controller/acs/SensorValues.cpp | 17 + mission/controller/acs/SensorValues.h | 44 +- mission/controller/acs/control/PtgCtrl.cpp | 94 ++-- mission/controller/acs/control/PtgCtrl.h | 67 +-- 9 files changed, 434 insertions(+), 438 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 84a7682f..856a0034 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -126,17 +126,19 @@ void AcsController::performPointingCtrl() { double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws); double rwTrqNs[4] = {0, 0, 0, 0}; - ptgCtrl.ptgNullspace(&(sensorValues.speedRw0), &(sensorValues.speedRw1), &(sensorValues.speedRw2), - &(sensorValues.speedRw3), rwTrqNs); + 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.speedRw0), &(sensorValues.speedRw1), - &(sensorValues.speedRw2), &(sensorValues.speedRw3), torquePtgRws, - rwTrqNs, cmdSpeedRws); + 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.speedRw0), - &(sensorValues.speedRw1), &(sensorValues.speedRw2), - &(sensorValues.speedRw3), mgtDpDes); + 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); } diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 261277a0..920989ed 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -23,42 +23,37 @@ ActuatorCmd::~ActuatorCmd(){ } -void ActuatorCmd::cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2, - const double *speedRw3, const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed){ - - using namespace Math; - // 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 (maxValue > maxTrq) { - - double scalingFactor = maxTrq / maxValue; - VectorOperations::mulScalar(torque, scalingFactor, torque, 4); - - } - - // Calculating the commanded speed in RPM for every reaction wheel - double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; - double deltaSpeed[4] = {0, 0, 0, 0}; - double commandTime = acsParameters.onBoardParams.sampleTime, - inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; - double radToRpm = 60 / (2 * PI); // factor for conversion to RPM - // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = commandTime / inertiaWheel * radToRpm; - VectorOperations::mulScalar(torque, factor, deltaSpeed, 4); - VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); +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; + // 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 (maxValue > maxTrq) { + double scalingFactor = maxTrq / maxValue; + VectorOperations::mulScalar(torque, scalingFactor, torque, 4); + } + // Calculating the commanded speed in RPM for every reaction wheel + double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double deltaSpeed[4] = {0, 0, 0, 0}; + double commandTime = acsParameters.onBoardParams.sampleTime, + inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; + double radToRpm = 60 / (2 * PI); // factor for conversion to RPM + // W_RW = Torque_RW / I_RW * delta t [rad/s] + double factor = commandTime / inertiaWheel * radToRpm; + VectorOperations::mulScalar(torque, factor, deltaSpeed, 4); + VectorOperations::add(speedRws, deltaSpeed, rwCmdSpeed, 4); } void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) { diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 820f1f00..1da2c89a 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -28,10 +28,11 @@ public: * rwTrqNS Nullspace torque * rwCmdSpeed output revolutions per minute for every reaction wheel */ - void cmdSpeedToRws(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, - const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed); + 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); - /* + /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * * @param: dipolMoment given dipol moment in spacecraft frame diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 7d9cd8df..f6ebee5f 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -5,343 +5,323 @@ * Author: Robin Marquardt */ - #include "Guidance.h" -#include "string.h" -#include "util/MathOperations.h" -#include "util/CholeskyDecomposition.h" -#include -#include + #include #include +#include +#include -Guidance::Guidance(AcsParameters *acsParameters_) { - acsParameters = *acsParameters_; +#include "string.h" +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" -} +Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } -Guidance::~Guidance() { - -} +Guidance::~Guidance() {} void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { + for (int i = 0; i < 3; i++) { + sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i]; + satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i]; + } - for (int i = 0; i < 3; i++) { - - sunTargetSafe[i] = - acsParameters.safeModeControllerParameters.sunTargetDir[i]; - satRateSafe[i] = - acsParameters.safeModeControllerParameters.satRateRef[i]; - - } - - -// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); - + // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); } -void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]){ -//------------------------------------------------------------------------------------- -// Calculation of target quaternion to groundstation -//------------------------------------------------------------------------------------- -// Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth fixed/centered frame) - double groundStationCart[3] = {0, 0, 0}; +void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + timeval now, double targetQuat[4], double refSatRate[3]) { + //------------------------------------------------------------------------------------- + // Calculation of target quaternion to groundstation + //------------------------------------------------------------------------------------- + // Transform longitude, latitude and altitude of groundstation 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); + 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}; - MathOperations::cartesianFromLatLongAlt(sensorValues->gps0latitude, sensorValues->gps0longitude, - sensorValues->gps0altitude, posSatE); + // 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, + sensorValues->gpsSet.altitude.value, posSatE); -// Target direction in the ECEF frame - double targetDirE[3] = {0, 0, 0}; - VectorOperations::subtract(groundStationCart, posSatE, targetDirE, 3); + // Target direction in the ECEF frame + 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}}; - 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}}; - 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; + // 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}}; + 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); + // 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); + 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 - 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); + // 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); + // 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.refDirection[0]; - refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; - refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; - double noramlizedTargetDirB[3] = {0, 0, 0}; - VectorOperations::normalize(targetDirB, noramlizedTargetDirB, 3); - VectorOperations::normalize(refDir, refDir, 3); - 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); + // rotation quaternion from two vectors + double refDir[3] = {0, 0, 0}; + refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0]; + refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1]; + refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2]; + 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 -//------------------------------------------------------------------------------------- - double velSatE[3] = {0, 0, 0}; - velSatE[0] = sensorValues->gps0Velocity[0]; - velSatE[1] = sensorValues->gps0Velocity[1]; - velSatE[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); - double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); - MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); - VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); + //------------------------------------------------------------------------------------- + // 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]; + 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); + double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3); + MatrixOperations::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); + VectorOperations::add(velSatBPart1, velSatBPart2, velSatB, 3); - double normVelSatB = VectorOperations::norm(velSatB, 3); - double normRefSatRate = normVelSatB / normTargetDirB; + double normVelSatB = VectorOperations::norm(velSatB, 3); + double normRefSatRate = normVelSatB / normTargetDirB; - double satRateDir[3] = {0, 0, 0}; - VectorOperations::cross(velSatB, targetDirB, satRateDir); - VectorOperations::normalize(satRateDir, satRateDir, 3); - VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); + double satRateDir[3] = {0, 0, 0}; + VectorOperations::cross(velSatB, targetDirB, satRateDir); + VectorOperations::normalize(satRateDir, satRateDir, 3); + VectorOperations::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); -//------------------------------------------------------------------------------------- -// Calculation of reference rotation rate in case of star tracker blinding -//------------------------------------------------------------------------------------- - if ( acsParameters.targetModeControllerParameters.avoidBlindStr ) { + //------------------------------------------------------------------------------------- + // 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}; - double sunDirJ[3] = {0, 0, 0}; - double 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, sunDirB, 3, 3, 1); + } - if ( outputValues->sunDirModelValid ) { + else { + sunDirB[0] = outputValues->sunDirEst[0]; + sunDirB[1] = outputValues->sunDirEst[1]; + sunDirB[2] = outputValues->sunDirEst[2]; + } - sunDirJ[0] = outputValues->sunDirModel[0]; - sunDirJ[1] = outputValues->sunDirModel[1]; - sunDirJ[2] = outputValues->sunDirModel[2]; - MatrixOperations::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1); - } + double exclAngle = acsParameters.strParameters.exclusionAngle, + blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, + blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; - else { - sunDirB[0] = outputValues->sunDirEst[0]; - sunDirB[1] = outputValues->sunDirEst[1]; - sunDirB[2] = outputValues->sunDirEst[2]; + double sightAngleSun = + VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); - } + if (!(strBlindAvoidFlag)) { + double critSightAngle = blindStart * exclAngle; - double exclAngle = acsParameters.strParameters.exclusionAngle, - blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, - blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; + if (sightAngleSun < critSightAngle) { + strBlindAvoidFlag = true; + } - double sightAngleSun = VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); + } - if ( !(strBlindAvoidFlag) ) { + else { + if (sightAngleSun < blindEnd * exclAngle) { + double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; + double blindRefRate[3] = {0, 0, 0}; - double critSightAngle = blindStart * exclAngle; + if (sunDirB[1] < 0) { + blindRefRate[0] = normBlindRefRate; + blindRefRate[1] = 0; + blindRefRate[2] = 0; + } else { + blindRefRate[0] = -normBlindRefRate; + blindRefRate[1] = 0; + blindRefRate[2] = 0; + } - if ( sightAngleSun < critSightAngle) { - strBlindAvoidFlag = true; - } + VectorOperations::add(blindRefRate, refSatRate, refSatRate, 3); - } - - else { - if ( sightAngleSun < blindEnd * exclAngle) { - - double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; - double blindRefRate[3] = {0, 0, 0}; - - - if ( sunDirB[1] < 0) { - blindRefRate[0] = normBlindRefRate; - blindRefRate[1] = 0; - blindRefRate[2] = 0; - } - else { - blindRefRate[0] = -normBlindRefRate; - blindRefRate[1] = 0; - blindRefRate[2] = 0; - } - - VectorOperations::add(blindRefRate, refSatRate, refSatRate, 3); - - } - else { - strBlindAvoidFlag = false; - } - } - } + } else { + strBlindAvoidFlag = false; + } + } + } } +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}; + quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0]; + quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1]; + quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2]; + quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; -void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ) { + double satRate[3] = {0, 0, 0}; + satRate[0] = outputValues->satRateMekf[0]; + satRate[1] = outputValues->satRateMekf[1]; + satRate[2] = outputValues->satRateMekf[2]; + VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); + // valid checks ? + double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]}, + {-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]}, + {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]}, + {quatRef[0], -quatRef[1], quatRef[2], quatRef[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 quatErrorComplete[4] = {0, 0, 0, 0}; + MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); - double satRate[3] = {0, 0, 0}; - satRate[0] = outputValues->satRateMekf[0]; - satRate[1] = outputValues->satRateMekf[1]; - satRate[2] = outputValues->satRateMekf[2]; - VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); - // valid checks ? - double quatErrorMtx[4][4] = {{ quatRef[3], quatRef[2], -quatRef[1], -quatRef[0] }, - { -quatRef[2], quatRef[3], quatRef[0], -quatRef[1] }, - { quatRef[1], -quatRef[0], quatRef[3], -quatRef[2] }, - { quatRef[0], -quatRef[1], quatRef[2], quatRef[3] }}; + if (quatErrorComplete[3] < 0) { + quatErrorComplete[3] *= -1; + } - double quatErrorComplete[4] = {0, 0, 0, 0}; - MatrixOperations::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1); - - if (quatErrorComplete[3] < 0) { - quatErrorComplete[3] *= -1; - } - - quatError[0] = quatErrorComplete[0]; - 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 ?? + quatError[0] = quatErrorComplete[0]; + 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 ?? } +void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { + if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() && + sensorValues->rw4Set.isValid()) { + rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; -void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv) { + } - if (sensorValues->validRw0 && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) { + else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() && + sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { + rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2]; + } - rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; + else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) && + sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { + rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2]; + } - } + else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) && + !(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) { + rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2]; + } - else if (!(sensorValues->validRw0) && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) { + else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) && + (sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) { + rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2]; + } - rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2]; - } - - else if ((sensorValues->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) { - - rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2]; - } - - else if ((sensorValues->validRw0) && (sensorValues->validRw1) && !(sensorValues->validRw2) && sensorValues->validRw3) { - - rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2]; - } - - else if ((sensorValues->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && !(sensorValues->validRw3)) { - - rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2]; - } - - else { -// @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. - rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; - rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; - rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; - rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; - rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; - rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; - rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; - rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; - rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; - rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; - rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; - rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; - - } + else { + // @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. + rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0]; + rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1]; + rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2]; + rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0]; + rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1]; + rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2]; + rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0]; + rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1]; + rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2]; + rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0]; + rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1]; + rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2]; + } } diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index bf9894e2..93b9eb47 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -460,8 +460,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->mgm3Rm3100Set.fieldStrengths.value, sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters, - outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gps0altitude, - sensorValues->gps0Valid, outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value, + sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->magneticFieldVectorDerivative, &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index b38a35b1..1492ad6e 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -60,6 +60,23 @@ ReturnValue_t SensorValues::updateStr() { return result; } +ReturnValue_t SensorValues::updateGps() { + ReturnValue_t result; + PoolReadGuard pgGps(&gpsSet); + + result = pgGps.getReadResult(); + return result; +} + +ReturnValue_t SensorValues::updateRw() { + ReturnValue_t result; + PoolReadGuard pgRw1(&rw1Set), pgRw2(&rw2Set), pgRw3(&rw3Set), pgRw4(&rw4Set); + + result = (pgRw1.getReadResult() || pgRw2.getReadResult() || pgRw3.getReadResult() || + pgRw4.getReadResult()); + return result; +} + ReturnValue_t SensorValues::update() { ReturnValue_t mgmUpdate = updateMgm(); ReturnValue_t susUpdate = updateSus(); diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 4e3a215f..08fe5177 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -6,9 +6,11 @@ #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/IMTQHandlerDefinitions.h" +#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" namespace ACS { @@ -22,7 +24,9 @@ class SensorValues { ReturnValue_t updateMgm(); ReturnValue_t updateSus(); ReturnValue_t updateGyr(); + ReturnValue_t updateGps(); ReturnValue_t updateStr(); + ReturnValue_t updateRw(); MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); @@ -56,33 +60,27 @@ class SensorValues { startracker::SolutionSet strSet = startracker::SolutionSet(objects::STAR_TRACKER); - // double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? - // bool quatJBValid; - // int strIntTime[2]; + GpsPrimaryDataset gpsSet = GpsPrimaryDataset(objects::GPS_CONTROLLER); - double gps0latitude; // Reference is WGS84, so this one will probably be geodetic - double gps0longitude; // Should be geocentric for IGRF - double gps0altitude; - double gps0Velocity[3]; // speed over ground = ?? - double gps0Time; // utc + // double gps0latitude; // Reference is WGS84, so this one will probably be geodetic + // double gps0longitude; // Should be geocentric for IGRF + // double gps0altitude; + // double gps0Velocity[3]; // speed over ground = ?? + // double gps0Time; // utc + // + // // valid ids for gps values ! + // int gps0TimeYear; + // int gps0TimeMonth; + // int gps0TimeHour; // should be double + // bool gps0Valid; - // valid ids for gps values ! - int gps0TimeYear; - int gps0TimeMonth; - int gps0TimeHour; // should be double - bool gps0Valid; + // bool mgt0valid; - bool mgt0valid; - // Reaction wheel measurements - double speedRw0; // RPM [1/min] - double speedRw1; // RPM [1/min] - double speedRw2; // RPM [1/min] - double speedRw3; // RPM [1/min] - bool validRw0; - bool validRw1; - bool validRw2; - bool validRw3; + RwDefinitions::StatusSet rw1Set = RwDefinitions::StatusSet(objects::RW1); + RwDefinitions::StatusSet rw2Set = RwDefinitions::StatusSet(objects::RW2); + RwDefinitions::StatusSet rw3Set = RwDefinitions::StatusSet(objects::RW3); + RwDefinitions::StatusSet rw4Set = RwDefinitions::StatusSet(objects::RW4); }; } /* namespace ACS */ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 999df912..6201a32a 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -112,54 +112,54 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do } -void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0, - double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes) { - if ( !(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { - - mgtDpDes[0] = 0; - mgtDpDes[1] = 0; - mgtDpDes[2] = 0; - return; - - } - -// calculating momentum of satellite and momentum of reaction wheels - double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; - double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; - VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); - MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, 1); - double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); - VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); -// calculating momentum error - double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract(momentumTotal, pointingModeControllerParameters->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; - VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); +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)) { + mgtDpDes[0] = 0; + mgtDpDes[1] = 0; + mgtDpDes[2] = 0; + return; + } + // calculating momentum of satellite and momentum of reaction wheels + double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; + VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); + MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, + 1); + double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1); + VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); + // calculating momentum error + double deltaMomentum[3] = {0, 0, 0}; + VectorOperations::subtract( + momentumTotal, pointingModeControllerParameters->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; + VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); } -void PtgCtrl::ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs) { - - double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; - double wheelMomentum[4] = {0, 0, 0, 0}; - double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; - //Conversion to [rad/s] for further calculations - VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); - VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); - double diffRwSpeed[4] = {0, 0, 0, 0}; - VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, wheelMomentum, 4); - double gainNs = pointingModeControllerParameters->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); - MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); - VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); - VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); - - +void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, + const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { + double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double wheelMomentum[4] = {0, 0, 0, 0}; + double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; + // Conversion to [rad/s] for further calculations + VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); + VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); + double diffRwSpeed[4] = {0, 0, 0, 0}; + VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); + VectorOperations::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, + wheelMomentum, 4); + double gainNs = pointingModeControllerParameters->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); + MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); + VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 87a66f2d..be67187d 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -4,8 +4,8 @@ * Created on: 17 Jul 2022 * Author: Robin Marquardt * - * @brief: This class handles the pointing control mechanism. Calculation of an commanded torque - * for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * @brief: This class handles the pointing control mechanism. Calculation of an commanded + * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation * * @note: A description of the used algorithms can be found in * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 @@ -14,46 +14,49 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ -#include "../SensorValues.h" -#include "../OutputValues.h" -#include "../AcsParameters.h" -#include "../config/classIds.h" -#include #include +#include #include -class PtgCtrl{ +#include "../AcsParameters.h" +#include "../OutputValues.h" +#include "../SensorValues.h" +#include "../config/classIds.h" -public: - /* @brief: Constructor - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - PtgCtrl(AcsParameters *acsParameters_); - virtual ~PtgCtrl(); +class PtgCtrl { + public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + PtgCtrl(AcsParameters *acsParameters_); + virtual ~PtgCtrl(); - static const uint8_t INTERFACE_ID = CLASS_ID::PTG; - static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + 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 - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); + /* @brief: Load AcsParameters für this class + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void loadAcsParameters(AcsParameters *acsParameters_); - /* @brief: Calculates the needed torque for the pointing control mechanism - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void ptgGroundstation(const double mode,const double *qError,const double *deltaRate,const double *rwPseudoInv, double *torqueRws); + /* @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, + const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0, - double *speedRw1, double *speedRw2, double *speedRw3, double *mgtDpDes); + void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, + int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, + double *mgtDpDes); - void ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs); + void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, + const int32_t *speedRw3, double *rwTrqNs); -private: - AcsParameters::PointingModeControllerParameters* pointingModeControllerParameters; - AcsParameters::RwHandlingParameters* rwHandlingParameters; - AcsParameters::InertiaEIVE* inertiaEIVE; - AcsParameters::RwMatrices* rwMatrices; + private: + AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; + AcsParameters::RwHandlingParameters *rwHandlingParameters; + AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters::RwMatrices *rwMatrices; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ From 43497b399faca19363a1a9565611c15e11992998 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 15:18:07 +0200 Subject: [PATCH 059/244] enabled processGps --- mission/controller/acs/SensorProcessing.cpp | 4 ++-- mission/controller/acs/SensorValues.cpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 93b9eb47..dc930fc8 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -447,8 +447,8 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { sensorValues->update(); - // processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, - // &outputValues->gcLatitude, &outputValues->gdLongitude); + processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, + sensorValues->gpsSet.isValid(), &outputValues->gcLatitude, &outputValues->gdLongitude); outputValues->mgmUpdated = processMgm( sensorValues->mgm0Lis3Set.fieldStrengths.value, diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 1492ad6e..acc6f3eb 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -82,6 +82,7 @@ ReturnValue_t SensorValues::update() { ReturnValue_t susUpdate = updateSus(); ReturnValue_t gyrUpdate = updateGyr(); ReturnValue_t strUpdate = updateStr(); + if ((mgmUpdate && susUpdate && gyrUpdate && strUpdate) == returnvalue::FAILED) { return returnvalue::FAILED; }; From 8edf7fc0de244a627e7515b69c9e0895245d4349 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 15:22:26 +0200 Subject: [PATCH 060/244] deprecated code removed --- mission/controller/acs/SensorValues.h | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 08fe5177..1a260635 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -62,21 +62,8 @@ class SensorValues { GpsPrimaryDataset gpsSet = GpsPrimaryDataset(objects::GPS_CONTROLLER); - // double gps0latitude; // Reference is WGS84, so this one will probably be geodetic - // double gps0longitude; // Should be geocentric for IGRF - // double gps0altitude; - // double gps0Velocity[3]; // speed over ground = ?? - // double gps0Time; // utc - // - // // valid ids for gps values ! - // int gps0TimeYear; - // int gps0TimeMonth; - // int gps0TimeHour; // should be double - // bool gps0Valid; - // bool mgt0valid; - RwDefinitions::StatusSet rw1Set = RwDefinitions::StatusSet(objects::RW1); RwDefinitions::StatusSet rw2Set = RwDefinitions::StatusSet(objects::RW2); RwDefinitions::StatusSet rw3Set = RwDefinitions::StatusSet(objects::RW3); From cc35db91cdd48e55e1498a4039fada076393f8a2 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Oct 2022 17:14:33 +0200 Subject: [PATCH 061/244] added calibration of raw MGM values --- mission/controller/acs/AcsParameters.h | 19 +++++++++++ mission/controller/acs/SensorProcessing.cpp | 36 +++++++++++++++++---- 2 files changed, 49 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 29caa59e..6c72d9e5 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -36,6 +36,25 @@ class AcsParameters /*: public HasParametersIF*/ { 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 mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; + + float mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508}; + float mgm1hardIronOffset[3] = {10.95500, -8.053403, -33.36383}; + float mgm2hardIronOffset[3] = {15.72181, -26.87090, -62.19010}; + float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0}; + float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0}; + + float mgm0softIronInverse[3][3] = {{1420.727e-3, 9.352825e-3, -127.1979e-3}, + {9.352825e-3, 1031.965e-3, -80.02734e-3}, + {-127.1979e-3, -80.02734e-3, 934.8899e-3}}; + float mgm1softIronInverse[3][3] = {{126.7325e-2, -4.146410e-2, -18.37963e-2}, + {-4.146410e-2, 109.3310e-2, -5.246314e-2}, + {-18.37963e-2, -5.246314e-2, 105.7300e-2}}; + float mgm2softIronInverse[3][3] = {{143.0438e-2, 7.095763e-2, 15.67482e-2}, + {7.095763e-2, 99.65167e-2, -6.958760e-2}, + {15.67482e-2, -6.958760e-2, 94.50124e-2}}; + float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + } mgmHandlingParameters; struct SusHandlingParameters { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index dc930fc8..998927f6 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -42,38 +42,62 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const validMagField = false; return false; } - // Transforming Values to the Body Frame (actually it is the geometry frame atm) + float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0}, + mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0}, + mgm4ValueNoBias[3] = {0, 0, 0}; + float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0}, + mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0}; float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0}, mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0}; bool validUnit[5] = {false, false, false, false, false}; uint8_t validCount = 0; if (mgm0valid) { - MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, + VectorOperations::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias, + mgm0ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib, mgm0ValueBody, 3, 3, 1); validCount += 1; validUnit[0] = true; } if (mgm1valid) { - MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, + VectorOperations::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias, + mgm1ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib, mgm1ValueBody, 3, 3, 1); validCount += 1; validUnit[1] = true; } if (mgm2valid) { - MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, + VectorOperations::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias, + mgm2ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib, mgm2ValueBody, 3, 3, 1); validCount += 1; validUnit[2] = true; } if (mgm3valid) { - MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, + VectorOperations::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias, + mgm3ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib, mgm3ValueBody, 3, 3, 1); validCount += 1; validUnit[3] = true; } if (mgm4valid) { - MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value, + VectorOperations::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias, + 3); + MatrixOperations::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias, + mgm4ValueCalib, 3, 3, 1); + MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib, mgm4ValueBody, 3, 3, 1); validCount += 1; validUnit[4] = true; From f89f7630c179aff7d33ace5406ca49f1009b4eea Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 13 Oct 2022 10:49:39 +0200 Subject: [PATCH 062/244] removed comment --- mission/controller/acs/SensorValues.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index acc6f3eb..152f866c 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -90,4 +90,4 @@ ReturnValue_t SensorValues::update() { } } // namespace ACS -// namespace ACS + From 8f61d14a828fb897f0db06d21861b06978438d9b Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 14 Oct 2022 11:11:23 +0200 Subject: [PATCH 063/244] implemented getParameter for AcsParameters --- mission/controller/acs/AcsParameters.cpp | 536 ++++++++++++++++++++++- 1 file changed, 530 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 0b516c13..ca1057bc 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -3,15 +3,539 @@ #include #include -#include - -AcsParameters::AcsParameters() {} +AcsParameters::AcsParameters(){}; //(uint8_t parameterModuleId) : + // parameterModuleId(parameterModuleId) {} AcsParameters::~AcsParameters() {} -/* -ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, + +/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId, ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, uint16_t startAtIndex) { - return returnvalue::OK; + if (domainId == parameterModuleId) { + switch (parameterId >> 8) { + case 0x0: // direct members + switch (parameterId & 0xFF) { + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x1: // OnBoardParams + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(onBoardParams.sampleTime); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x2: // InertiaEIVE + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(inertiaEIVE.inertiaMatrix); + break; + case 0x1: + parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x3: // MgmHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix); + break; + case 0x5: + parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset); + break; + case 0x6: + parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset); + break; + case 0x7: + parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset); + break; + case 0x8: + parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset); + break; + case 0x9: + parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset); + break; + case 0xA: + parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse); + break; + case 0xB: + parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse); + break; + case 0xC: + parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse); + break; + case 0xD: + parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse); + break; + case 0xE: + parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case 0x4: // SusHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(susHandlingParameters.sus0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(susHandlingParameters.sus1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(susHandlingParameters.sus2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(susHandlingParameters.sus3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(susHandlingParameters.sus4orientationMatrix); + break; + case 0x5: + parameterWrapper->set(susHandlingParameters.sus5orientationMatrix); + break; + case 0x6: + parameterWrapper->set(susHandlingParameters.sus6orientationMatrix); + break; + case 0x7: + parameterWrapper->set(susHandlingParameters.sus7orientationMatrix); + break; + case 0x8: + parameterWrapper->set(susHandlingParameters.sus8orientationMatrix); + break; + case 0x9: + parameterWrapper->set(susHandlingParameters.sus9orientationMatrix); + break; + case 0xA: + parameterWrapper->set(susHandlingParameters.sus10orientationMatrix); + break; + case 0xB: + parameterWrapper->set(susHandlingParameters.sus11orientationMatrix); + break; + case 0xC: + parameterWrapper->set(susHandlingParameters.sus0coeffAlpha); + break; + case 0xD: + parameterWrapper->set(susHandlingParameters.sus0coeffBeta); + break; + case 0xE: + parameterWrapper->set(susHandlingParameters.sus1coeffAlpha); + break; + case 0xF: + parameterWrapper->set(susHandlingParameters.sus1coeffBeta); + break; + case 0x10: + parameterWrapper->set(susHandlingParameters.sus2coeffAlpha); + break; + case 0x11: + parameterWrapper->set(susHandlingParameters.sus2coeffBeta); + break; + case 0x12: + parameterWrapper->set(susHandlingParameters.sus3coeffAlpha); + break; + case 0x13: + parameterWrapper->set(susHandlingParameters.sus3coeffBeta); + break; + case 0x14: + parameterWrapper->set(susHandlingParameters.sus4coeffAlpha); + break; + case 0x15: + parameterWrapper->set(susHandlingParameters.sus4coeffBeta); + break; + case 0x16: + parameterWrapper->set(susHandlingParameters.sus5coeffAlpha); + break; + case 0x17: + parameterWrapper->set(susHandlingParameters.sus5coeffBeta); + break; + case 0x18: + parameterWrapper->set(susHandlingParameters.sus6coeffAlpha); + break; + case 0x19: + parameterWrapper->set(susHandlingParameters.sus6coeffBeta); + break; + case 0x1A: + parameterWrapper->set(susHandlingParameters.sus7coeffAlpha); + break; + case 0x1B: + parameterWrapper->set(susHandlingParameters.sus7coeffBeta); + break; + case 0x1C: + parameterWrapper->set(susHandlingParameters.sus8coeffAlpha); + break; + case 0x1D: + parameterWrapper->set(susHandlingParameters.sus8coeffBeta); + break; + case 0x1E: + parameterWrapper->set(susHandlingParameters.sus9coeffAlpha); + break; + case 0x1F: + parameterWrapper->set(susHandlingParameters.sus9coeffBeta); + break; + case 0x20: + parameterWrapper->set(susHandlingParameters.sus10coeffAlpha); + break; + case 0x21: + parameterWrapper->set(susHandlingParameters.sus10coeffBeta); + break; + case 0x22: + parameterWrapper->set(susHandlingParameters.sus11coeffAlpha); + break; + case 0x23: + parameterWrapper->set(susHandlingParameters.sus11coeffBeta); + break; + case 0x24: + parameterWrapper->set(susHandlingParameters.filterAlpha); + break; + case 0x25: + parameterWrapper->set(susHandlingParameters.sunThresh); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x5): // GyrHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x6): // RwHandlingParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix); + break; + case 0x4: + parameterWrapper->set(rwHandlingParameters.inertiaWheel); + break; + case 0x5: + parameterWrapper->set(rwHandlingParameters.maxTrq); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x7): // RwMatrices + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(rwMatrices.alignmentMatrix); + break; + case 0x1: + parameterWrapper->set(rwMatrices.pseudoInverse); + break; + case 0x2: + parameterWrapper->set(rwMatrices.without0); + break; + case 0x3: + parameterWrapper->set(rwMatrices.without1); + break; + case 0x4: + parameterWrapper->set(rwMatrices.without2); + break; + case 0x5: + parameterWrapper->set(rwMatrices.without3); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x8): // SafeModeControllerParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(safeModeControllerParameters.k_rate_mekf); + break; + case 0x1: + parameterWrapper->set(safeModeControllerParameters.k_align_mekf); + break; + case 0x2: + parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf); + break; + case 0x3: + parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf); + break; + case 0x4: + parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); + break; + case 0x5: + parameterWrapper->set(safeModeControllerParameters.sunTargetDir); + break; + case 0x6: + parameterWrapper->set(safeModeControllerParameters.satRateRef); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x9): // DetumbleCtrlParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(detumbleCtrlParameters.gainD); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xA): // PointingModeControllerParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(targetModeControllerParameters.updtFlag); + break; + case 0x1: + parameterWrapper->set(targetModeControllerParameters.A_rw); + break; + case 0x2: + parameterWrapper->set(targetModeControllerParameters.refDirection); + break; + case 0x3: + parameterWrapper->set(targetModeControllerParameters.refRotRate); + break; + case 0x4: + parameterWrapper->set(targetModeControllerParameters.quatRef); + break; + case 0x5: + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + break; + case 0x6: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + break; + case 0x7: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x8: + parameterWrapper->set(targetModeControllerParameters.blindRotRate); + break; + case 0x9: + parameterWrapper->set(targetModeControllerParameters.zeta); + break; + case 0xA: + parameterWrapper->set(targetModeControllerParameters.zetaLow); + break; + case 0xB: + parameterWrapper->set(targetModeControllerParameters.om); + break; + case 0xC: + parameterWrapper->set(targetModeControllerParameters.omLow); + break; + case 0xD: + parameterWrapper->set(targetModeControllerParameters.omMax); + break; + case 0xE: + parameterWrapper->set(targetModeControllerParameters.qiMin); + break; + case 0xF: + parameterWrapper->set(targetModeControllerParameters.gainNullspace); + break; + case 0x10: + parameterWrapper->set(targetModeControllerParameters.desatMomentumRef); + break; + case 0x11: + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + break; + case 0x12: + parameterWrapper->set(targetModeControllerParameters.desatOn); + break; + case 0x13: + parameterWrapper->set(targetModeControllerParameters.omegaEarth); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xB): // StrParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(strParameters.exclusionAngle); + break; + case 0x1: + parameterWrapper->set(strParameters.boresightAxis); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xC): // GpsParameters + switch (parameterId & 0xFF) { + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xD): // GroundStationParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(groundStationParameters.latitudeGs); + break; + case 0x1: + parameterWrapper->set(groundStationParameters.longitudeGs); + break; + case 0x2: + parameterWrapper->set(groundStationParameters.altitudeGs); + break; + case 0x3: + parameterWrapper->set(groundStationParameters.earthRadiusEquat); + break; + case 0x4: + parameterWrapper->set(groundStationParameters.earthRadiusPolar); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xE): // SunModelParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(sunModelParameters.useSunModel); + break; + case 0x1: + parameterWrapper->set(sunModelParameters.domega); + break; + case 0x2: + parameterWrapper->set(sunModelParameters.omega_0); + break; + case 0x3: + parameterWrapper->set(sunModelParameters.m_0); + break; + case 0x4: + parameterWrapper->set(sunModelParameters.dm); + break; + case 0x5: + parameterWrapper->set(sunModelParameters.e); + break; + case 0x6: + parameterWrapper->set(sunModelParameters.e1); + break; + case 0x7: + parameterWrapper->set(sunModelParameters.p1); + break; + case 0x8: + parameterWrapper->set(sunModelParameters.p2); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0xF): // KalmanFilterParameters + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter); + break; + case 0x1: + parameterWrapper->set(kalmanFilterParameters.requestResetFlag); + break; + case 0x2: + parameterWrapper->set( + kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps); + break; + case 0x3: + parameterWrapper->set(kalmanFilterParameters.processNoiseOmega); + break; + case 0x4: + parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion); + break; + case 0x5: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); + break; + case 0x6: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); + break; + case 0x7: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); + break; + case 0x8: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); + break; + case 0x9: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); + break; + case 0xA: + parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x10): // MagnetorquesParameter + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix); + break; + case 0x1: + parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix); + break; + case 0x2: + parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix); + break; + case 0x3: + parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq); + break; + case 0x4: + parameterWrapper->set(magnetorquesParameter.inverseAlignment); + break; + case 0x5: + parameterWrapper->set(magnetorquesParameter.DipolMax); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + case (0x11): // DetumbleParameter + switch (parameterId & 0xFF) { + case 0x0: + parameterWrapper->set(detumbleParameter.detumblecounter); + break; + case 0x1: + parameterWrapper->set(detumbleParameter.omegaDetumbleStart); + break; + case 0x2: + parameterWrapper->set(detumbleParameter.omegaDetumbleEnd); + break; + default: + return INVALID_IDENTIFIER_ID; + } + break; + default: + return INVALID_IDENTIFIER_ID; + } + return returnvalue::OK; + } else { + return INVALID_DOMAIN_ID; + } }*/ From 0a5bd6ef73bfadf8d2aaed4b5f54c97b713ba12c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 14 Oct 2022 14:57:22 +0200 Subject: [PATCH 064/244] added sensor fusion logic for GYR --- mission/controller/acs/AcsParameters.cpp | 3 + mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/SensorProcessing.cpp | 75 ++++++++++++--------- 3 files changed, 47 insertions(+), 32 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index ca1057bc..cc9959ab 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -226,6 +226,9 @@ AcsParameters::~AcsParameters() {} case 0x3: parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix); break; + case 0x4: + parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 6c72d9e5..97a31dd3 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -753,6 +753,7 @@ 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}}; + float gyrFusionWeight = 0.8; } gyrHandlingParameters; struct RwHandlingParameters { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 998927f6..a88d25ef 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,10 +1,3 @@ -/* - * SensorProcessing.cpp - * - * Created on: 7 Mar 2022 - * Author: Robin Marquardt - */ - #include "SensorProcessing.h" #include @@ -397,59 +390,77 @@ void SensorProcessing::processGyr( gyr3ValueBody[3] = {0, 0, 0}; bool validUnit[4] = {false, false, false, false}; - uint8_t validCount = 0; if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) { const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); - validCount += 1; validUnit[0] = true; } if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) { const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); - validCount += 1; validUnit[1] = true; } if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) { const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); - validCount += 1; validUnit[2] = true; } if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) { const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); - validCount += 1; validUnit[3] = true; } /* -------- SatRateEst: Middle Value ------- */ - double gyrValues[3][4] = { - {gyr0ValueBody[0], gyr1ValueBody[0], gyr2ValueBody[0], gyr3ValueBody[0]}, - {gyr0ValueBody[1], gyr1ValueBody[1], gyr2ValueBody[1], gyr3ValueBody[1]}, - {gyr0ValueBody[2], gyr1ValueBody[2], gyr2ValueBody[2], gyr3ValueBody[2]}}; - double gyrValidValues[3][validCount]; - uint8_t j = 0; - for (uint8_t i = 0; i < validCount; i++) { - if (validUnit[i]) { - gyrValidValues[0][j] = gyrValues[0][i]; - gyrValidValues[1][j] = gyrValues[1][i]; - gyrValidValues[2][j] = gyrValues[2][i]; - j += 1; + // take ADIS measurements, if both avail + // if just one ADIS measurement avail, perform sensor fusion + if (validUnit[0] && validUnit[2]) { + double gyr02ValuesSum[3]; + VectorOperations::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); + VectorOperations::mulScalar(gyr02ValuesSum, .5, satRatEst, 3); + } else if ((validUnit[0] || validUnit[2]) && !(validUnit[1] || validUnit[3])) { + if (validUnit[0]) { + satRatEst = gyr0ValueBody; + } else if (validUnit[2]) { + satRatEst = gyr2ValueBody; } - } - // Selection Sort - double gyrValidValuesSort[3][validCount]; - MathOperations::selectionSort(*gyrValidValues, *gyrValidValuesSort, 3, validCount); + } else if ((validUnit[1]) && (validUnit[3])) { + double gyr13ValuesSum[3]; + double gyr13ValuesMean[3]; + VectorOperations::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); + VectorOperations::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); + if (validUnit[0]) { + satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * + gyr13ValuesMean[0] + + gyrParameters->gyrFusionWeight * gyr0ValueBody[0]; + satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * + gyr13ValuesMean[1] + + gyrParameters->gyrFusionWeight * gyr0ValueBody[1]; + satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * + gyr13ValuesMean[2] + + gyrParameters->gyrFusionWeight * gyr0ValueBody[2]; - uint8_t n = ceil(validCount / 2); - satRatEst[0] = gyrValidValuesSort[0][n]; - satRatEst[1] = gyrValidValuesSort[1][n]; - satRatEst[2] = gyrValidValuesSort[2][n]; + } else if (validUnit[2]) { + satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * + gyr13ValuesMean[0] + + gyrParameters->gyrFusionWeight * gyr2ValueBody[0]; + satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * + gyr13ValuesMean[1] + + gyrParameters->gyrFusionWeight * gyr2ValueBody[1]; + satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * + gyr13ValuesMean[2] + + gyrParameters->gyrFusionWeight * gyr2ValueBody[2]; + } else + satRatEst = gyr13ValuesMean; + } else if (validUnit[1]) { + satRatEst = gyr1ValueBody; + } else if (validUnit[3]) { + satRatEst = gyr3ValueBody; + } *satRateEstValid = true; } From 2d64892a81bb6f727b781624419ef1b453851ca1 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 17 Oct 2022 14:53:49 +0200 Subject: [PATCH 065/244] inserted fusion weight optimum --- mission/controller/acs/AcsParameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 97a31dd3..15d4d84f 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -753,7 +753,7 @@ 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}}; - float gyrFusionWeight = 0.8; + float gyrFusionWeight = 0.93; } gyrHandlingParameters; struct RwHandlingParameters { From 86450f9d66e0652f74b56b44d9f28ba6f32bd730 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 19 Oct 2022 11:00:46 +0200 Subject: [PATCH 066/244] Added DataSets for all OutputValues/Sensors --- .../AcsCtrlDefinitions.h | 163 +++++++++++++++++- 1 file changed, 155 insertions(+), 8 deletions(-) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 1b38e033..c122be43 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -8,16 +8,35 @@ namespace acsctrl { -enum SetIds : uint32_t { MGM_SENSOR_DATA, SUS_SENSOR_DATA }; +enum SetIds : uint32_t { + MGM_SENSOR_DATA, + MGM_PROCESSED_DATA, + SUS_SENSOR_DATA, + SUS_PROCESSED_DATA, + GYR_SENSOR_DATA, + GYR_PROCESSED_DATA, + GPS_SENSOR_DATA, + MEKF_DATA +}; enum PoolIds : lp_id_t { + // MGM Raw MGM_0_LIS3_UT, MGM_1_RM3100_UT, MGM_2_LIS3_UT, MGM_3_RM3100_UT, MGM_IMTQ_CAL_NT, MGM_IMTQ_CAL_ACT_STATUS, - + // MGM Processed + MGM_0_VEC, + MGM_1_VEC, + MGM_2_VEC, + MGM_3_VEC, + MGM_4_VEC, + MGM_VEC_TOT, + MGM_VEC_TOT_DERIVATIVE, + MAG_IGRF_MODEL, + // SUS Raw SUS_0_N_LOC_XFYFZM_PT_XF, SUS_6_R_LOC_XFYBZM_PT_XF, @@ -35,15 +54,54 @@ enum PoolIds : lp_id_t { SUS_5_N_LOC_XFYMZB_PT_ZB, SUS_11_R_LOC_XBYMZB_PT_ZB, + // SUS Processed + SUS_0_VEC, + SUS_1_VEC, + SUS_2_VEC, + SUS_3_VEC, + SUS_4_VEC, + SUS_5_VEC, + SUS_6_VEC, + SUS_7_VEC, + SUS_8_VEC, + SUS_9_VEC, + SUS_10_VEC, + SUS_11_VEC, + SUS_VEC_TOT, + SUS_VEC_TOT_DERIVATIVE, + SUN_IJK_MODEL, + // GYR Raw + GYR_0_ADIS, + GYR_1_L3, + GYR_2_ADIS, + GYR_3_L3, + // GYR Processed + GYR_0_VEC, + GYR_1_VEC, + GYR_2_VEC, + GYR_3_VEC, + GYR_VEC_TOT, + // GPS PROCESSED + GC_LATITUDE, + GD_LONGITUDE, + // MEKF + SAT_ROT_RATE_MEKF, + QUAT_MEKF, }; -static constexpr uint8_t MGM_SET_ENTRIES = 10; -static constexpr uint8_t SUS_SET_ENTRIES = 12; +static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10; +static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 7 * sizeof(float) + 1 * sizeof(double); +static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12; +static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 14 * sizeof(float) + 1 * sizeof(double); +static constexpr uint8_t GYR_SET_RAW_ENTRIES = 2 * sizeof(float) + 2 * sizeof(double); +static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5 * sizeof(double); +static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2 * sizeof(double); +static constexpr uint8_t MEKF_SET_ENTRIES = 14 * sizeof(float) + 1 * sizeof(double); /** * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. */ -class MgmDataRaw : public StaticLocalDataSet { +class MgmDataRaw : public StaticLocalDataSet { public: MgmDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_SENSOR_DATA) {} @@ -60,7 +118,24 @@ class MgmDataRaw : public StaticLocalDataSet { private: }; -class SusDataRaw : public StaticLocalDataSet { +class MgmDataProcessed : public StaticLocalDataSet { + public: + MgmDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_PROCESSED_DATA) {} + + lp_vec_t mgm0vec = lp_vec_t(sid.objectId, MGM_0_VEC, this); + lp_vec_t mgm1vec = lp_vec_t(sid.objectId, MGM_1_VEC, this); + lp_vec_t mgm2vec = lp_vec_t(sid.objectId, MGM_2_VEC, this); + lp_vec_t mgm3vec = lp_vec_t(sid.objectId, MGM_3_VEC, this); + lp_vec_t mgm4vec = lp_vec_t(sid.objectId, MGM_4_VEC, this); + lp_vec_t mgmVecTot = lp_vec_t(sid.objectId, MGM_VEC_TOT, this); + lp_vec_t mgmVecTotDerivative = + lp_vec_t(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this); + lp_vec_t magIgrfModel = lp_vec_t(sid.objectId, MAG_IGRF_MODEL, this); + + private: +}; + +class SusDataRaw : public StaticLocalDataSet { public: SusDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {} @@ -74,8 +149,80 @@ class SusDataRaw : public StaticLocalDataSet { lp_vec_t sus7 = lp_vec_t(sid.objectId, SUS_7_R_LOC_XBYBZM_PT_XB, this); lp_vec_t sus8 = lp_vec_t(sid.objectId, SUS_8_R_LOC_XBYBZB_PT_YB, this); lp_vec_t sus9 = lp_vec_t(sid.objectId, SUS_9_R_LOC_XBYBZB_PT_YF, this); - lp_vec_t sus10 = lp_vec_t(sid.objectId, SUS_10_N_LOC_XMYBZF_PT_ZF, this); - lp_vec_t sus11 = lp_vec_t(sid.objectId, SUS_11_R_LOC_XBYMZB_PT_ZB, this); + lp_vec_t sus10 = + lp_vec_t(sid.objectId, SUS_10_N_LOC_XMYBZF_PT_ZF, this); + lp_vec_t sus11 = + lp_vec_t(sid.objectId, SUS_11_R_LOC_XBYMZB_PT_ZB, this); + + private: +}; + +class SusDataProcessed : public StaticLocalDataSet { + public: + SusDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_PROCESSED_DATA) {} + + lp_vec_t sus0vec = lp_vec_t(sid.objectId, SUS_0_VEC, this); + lp_vec_t sus1vec = lp_vec_t(sid.objectId, SUS_1_VEC, this); + lp_vec_t sus2vec = lp_vec_t(sid.objectId, SUS_2_VEC, this); + lp_vec_t sus3vec = lp_vec_t(sid.objectId, SUS_3_VEC, this); + lp_vec_t sus4vec = lp_vec_t(sid.objectId, SUS_4_VEC, this); + lp_vec_t sus5vec = lp_vec_t(sid.objectId, SUS_5_VEC, this); + 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 susVecTot = lp_vec_t(sid.objectId, SUS_VEC_TOT, this); + lp_vec_t susVecTotDerivative = + lp_vec_t(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this); + lp_vec_t sunIjkModel = lp_vec_t(sid.objectId, SUN_IJK_MODEL, this); + + private: +}; + +class GyrDataRaw : public StaticLocalDataSet { + public: + GyrDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GYR_SENSOR_DATA) {} + + lp_vec_t gyr0Adis = lp_vec_t(sid.objectId, GYR_0_ADIS, this); + lp_vec_t gyr1L3 = lp_vec_t(sid.objectId, GYR_1_L3, this); + lp_vec_t gyr2Adis = lp_vec_t(sid.objectId, GYR_2_ADIS, this); + lp_vec_t gyr3L3 = lp_vec_t(sid.objectId, GYR_3_L3, this); + + private: +}; + +class GyrDataProcessed : public StaticLocalDataSet { + public: + GyrDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GYR_PROCESSED_DATA) {} + + lp_vec_t gyr0vec = lp_vec_t(sid.objectId, GYR_0_VEC, this); + lp_vec_t gyr1vec = lp_vec_t(sid.objectId, GYR_1_VEC, this); + lp_vec_t gyr2vec = lp_vec_t(sid.objectId, GYR_2_VEC, this); + lp_vec_t gyr3vec = lp_vec_t(sid.objectId, GYR_3_VEC, this); + lp_vec_t gyrVecTot = lp_vec_t(sid.objectId, GYR_VEC_TOT, this); + + private: +}; + +class GpsDataProcessed : public StaticLocalDataSet { + public: + GpsDataProcessed(HasLocalDataPoolIF* hkOwner) + : StaticLocalDataSet(hkOwner, GPS_SET_PROCESSED_ENTRIES) {} + + lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); + lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this); + + private: +}; + +class MekfData : public StaticLocalDataSet { + public: + MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {} + + lp_vec_t quatMekf = lp_vec_t(sid.objectId, QUAT_MEKF, this); + lp_vec_t satRotRateMekf = lp_vec_t(sid.objectId, SAT_ROT_RATE_MEKF, this); private: }; From c50f8c716fad875314c94c0c01018b810c5c72fe Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 19 Oct 2022 11:01:27 +0200 Subject: [PATCH 067/244] amended GYR sensor fusion. replaced MGM sensor fusion --- mission/controller/acs/SensorProcessing.cpp | 104 ++++++++++---------- 1 file changed, 50 insertions(+), 54 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index a88d25ef..a6571496 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -42,9 +42,8 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0}; float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0}, mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0}; + float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; - bool validUnit[5] = {false, false, false, false, false}; - uint8_t validCount = 0; if (mgm0valid) { VectorOperations::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias, 3); @@ -52,8 +51,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm0ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib, mgm0ValueBody, 3, 3, 1); - validCount += 1; - validUnit[0] = true; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; + } } if (mgm1valid) { VectorOperations::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias, @@ -62,8 +63,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm1ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib, mgm1ValueBody, 3, 3, 1); - validCount += 1; - validUnit[1] = true; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; + } } if (mgm2valid) { VectorOperations::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias, @@ -72,8 +75,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm2ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib, mgm2ValueBody, 3, 3, 1); - validCount += 1; - validUnit[2] = true; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i]; + } } if (mgm3valid) { VectorOperations::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias, @@ -82,8 +87,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm3ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib, mgm3ValueBody, 3, 3, 1); - validCount += 1; - validUnit[3] = true; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i]; + } } if (mgm4valid) { VectorOperations::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias, @@ -92,33 +99,17 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgm4ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib, mgm4ValueBody, 3, 3, 1); - validCount += 1; - validUnit[4] = true; - } - - /* -------- MagFieldEst: Middle Value ------- */ - float mgmValues[3][5] = { - {mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], mgm3ValueBody[0], mgm4ValueBody[0]}, - {mgm0ValueBody[1], mgm1ValueBody[1], mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1]}, - {mgm0ValueBody[2], mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2]}}; - double mgmValidValues[3][validCount]; - uint8_t j = 0; - for (uint8_t i = 0; i < validCount; i++) { - if (validUnit[i]) { - mgmValidValues[0][j] = mgmValues[0][i]; - mgmValidValues[1][j] = mgmValues[1][i]; - mgmValidValues[2][j] = mgmValues[2][i]; - j += 1; + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i]; + sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; } } - // Selection Sort - double mgmValidValuesSort[3][validCount]; - MathOperations::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount); - - uint8_t n = ceil(validCount / 2); - magFieldEst[0] = mgmValidValuesSort[0][n]; - magFieldEst[1] = mgmValidValuesSort[1][n]; - magFieldEst[2] = mgmValidValuesSort[2][n]; + for (uint8_t i = 0; i < 3; i++) { + magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; + } + sif::debug << "magFeildEst: " << magFieldEst[0] << " , " << magFieldEst[1] << " , " + << magFieldEst[2] << std::endl; + ; validMagField = true; //-----------------------Mag Rate Computation --------------------------------------------------- @@ -434,26 +425,31 @@ void SensorProcessing::processGyr( VectorOperations::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); VectorOperations::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); if (validUnit[0]) { - satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[0] + - gyrParameters->gyrFusionWeight * gyr0ValueBody[0]; - satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[1] + - gyrParameters->gyrFusionWeight * gyr0ValueBody[1]; - satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[2] + - gyrParameters->gyrFusionWeight * gyr0ValueBody[2]; - + satRatEst[0] = + ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) + + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / + ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); + satRatEst[1] = + ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) + + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / + ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); + satRatEst[2] = + ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) + + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / + ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); } else if (validUnit[2]) { - satRatEst[0] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[0] + - gyrParameters->gyrFusionWeight * gyr2ValueBody[0]; - satRatEst[1] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[1] + - gyrParameters->gyrFusionWeight * gyr2ValueBody[1]; - satRatEst[2] = (1 - gyrParameters->gyrFusionWeight) * gyrParameters->gyrFusionWeight * - gyr13ValuesMean[2] + - gyrParameters->gyrFusionWeight * gyr2ValueBody[2]; + satRatEst[0] = + ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) + + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / + ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); + satRatEst[1] = + ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) + + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / + ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); + satRatEst[2] = + ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) + + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / + ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); } else satRatEst = gyr13ValuesMean; } else if (validUnit[1]) { From 5c2266e2143df464ba2ad3fde5d18ffa52903692 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 19 Oct 2022 11:01:53 +0200 Subject: [PATCH 068/244] added variances for sensors for sensor fusion --- mission/controller/acs/AcsParameters.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 15d4d84f..f5b72252 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -54,6 +54,9 @@ class AcsParameters /*: public HasParametersIF*/ { {15.67482e-2, -6.958760e-2, 94.50124e-2}}; float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + float mgm02variance[3] = {1, 1, 1}; + float mgm13variance[3] = {1, 1, 1}; + float mgm4variance[3] = {1, 1, 1}; } mgmHandlingParameters; @@ -753,7 +756,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}}; - float gyrFusionWeight = 0.93; + // 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 + float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; } gyrHandlingParameters; struct RwHandlingParameters { From deb7c4e5008a30b0553290caa4d73660f4d3a5b7 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 20 Oct 2022 11:07:45 +0200 Subject: [PATCH 069/244] added saftCtrl to acsController --- mission/controller/AcsController.cpp | 66 ++++- mission/controller/AcsController.h | 3 +- mission/controller/acs/control/SafeCtrl.cpp | 262 ++++++++++---------- mission/controller/acs/control/SafeCtrl.h | 69 +++--- 4 files changed, 221 insertions(+), 179 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 856a0034..22c871fd 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: @@ -72,7 +73,66 @@ void AcsController::performControlOperation() { // DEBUG END } -void AcsController::performSafe() {} +void AcsController::performSafe() { + // Concept: SAFE MODE WITH MEKF + // -do the sensor processing, maybe is does make more sense do call this class function in + // another place since we have to do it for every mode regardless of safe or not + + ACS::SensorValues sensorValues; + ACS::OutputValues outputValues; + + timeval now; // We need to give the actual time here + sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + ReturnValue_t validMekf; + navigation.useMekf(&sensorValues, &outputValues, &validMekf); // DOES THIS WORK WITH VALID? + // 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 == returnvalue::OK) { + 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); + } + + double dipolCmdUnits[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + + // Detumble check and switch + if (outputValues.satRateMekfValid && VectorOperations::norm(outputValues.satRateMekf, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + + else if (outputValues.satRateEstValid && + VectorOperations::norm(outputValues.satRateEst, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + + } + + else { + detumbleCounter = 0; + } + + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { + submode = SUBMODE_DETUMBLE; + detumbleCounter = 0; + } + + // commanding.magnetorquesDipol(); +} void AcsController::performDetumble() { ACS::SensorValues sensorValues; @@ -293,6 +353,8 @@ void AcsController::copySusData() { PoolReadGuard pg(&susSets[9]); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t)); + sif::debug << susData.sus9.isValid() << std::endl; + sif::debug << susSets[9].channels.isValid() << std::endl; } } { diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c23adfc1..a8ae9daa 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -11,6 +11,7 @@ #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" #include "acs/control/PtgCtrl.h" +#include "acs/control/SafeCtrl.h" #include "controllerdefinitions/AcsCtrlDefinitions.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" @@ -29,7 +30,6 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_PTG_NADIR = 5; protected: - void performSafe(); void performDetumble(); void performPointingCtrl(); @@ -41,6 +41,7 @@ class AcsController : public ExtendedControllerBase { ActuatorCmd actuatorCmd; Guidance guidance; + SafeCtrl safeCtrl; Detumble detumble; PtgCtrl ptgCtrl; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 32029ad7..c892fc05 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -6,182 +6,172 @@ */ #include "SafeCtrl.h" -#include "../util/MathOperations.h" -#include + #include #include #include #include +#include +#include "../util/MathOperations.h" -SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){ - loadAcsParameters(acsParameters_); - MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, 3); +SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { + loadAcsParameters(acsParameters_); + MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, + 3); } -SafeCtrl::~SafeCtrl(){ +SafeCtrl::~SafeCtrl() {} -} - -void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){ - safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); - inertiaEIVE = &(acsParameters_->inertiaEIVE); +void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) { + safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); + inertiaEIVE = &(acsParameters_->inertiaEIVE); } ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid, - double *magFieldModel, bool *magFieldModelValid, - double *sunDirModel, bool *sunDirModelValid, - double *satRateMekf, bool *rateMekfValid, - double *sunDirRef, double *satRatRef, - double *outputMagMomB, bool *outputValid){ + double *magFieldModel, bool *magFieldModelValid, + double *sunDirModel, bool *sunDirModelValid, double *satRateMekf, + bool *rateMekfValid, double *sunDirRef, double *satRatRef, + double *outputMagMomB, bool *outputValid) { + if (!(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || !(*rateMekfValid)) { + *outputValid = false; + return SAFECTRL_MEKF_INPUT_INVALID; + } - if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || - !(*rateMekfValid)) { - *outputValid = false; - return SAFECTRL_MEKF_INPUT_INVALID; - } + double kRate = 0, kAlign = 0; + kRate = safeModeControllerParameters->k_rate_mekf; + kAlign = safeModeControllerParameters->k_align_mekf; - double kRate = 0, kAlign = 0; - kRate = safeModeControllerParameters->k_rate_mekf; - kAlign = safeModeControllerParameters->k_align_mekf; + // Calc sunDirB ,magFieldB with mekf output and model + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::dcmFromQuat(quatBJ, *dcmBJ); + double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0}; + MatrixOperations::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); -// Calc sunDirB ,magFieldB with mekf output and model - double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - MathOperations::dcmFromQuat(quatBJ, *dcmBJ); - double sunDirB[3] = {0,0,0}, magFieldB[3] = {0,0,0}; - MatrixOperations::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); - MatrixOperations::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); + double crossSun[3] = {0, 0, 0}; - double crossSun[3] = {0, 0, 0}; + VectorOperations::cross(sunDirRef, sunDirB, crossSun); + double normCrossSun = VectorOperations::norm(crossSun, 3); - VectorOperations::cross(sunDirRef, sunDirB, crossSun); - double normCrossSun = VectorOperations::norm(crossSun, 3); + // calc angle alpha between sunDirRef and sunDIr + double alpha = 0, dotSun = 0; + dotSun = VectorOperations::dot(sunDirRef, sunDirB); + alpha = acos(dotSun); -// calc angle alpha between sunDirRef and sunDIr - double alpha = 0, dotSun = 0; - dotSun = VectorOperations::dot(sunDirRef, sunDirB); - alpha = acos(dotSun); + // Law Torque calculations + double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0}, + torqueAll[3] = {0, 0, 0}; -// Law Torque calculations - double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, - torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0}; + double scalarFac = 0; + scalarFac = kAlign * alpha / normCrossSun; + VectorOperations::mulScalar(crossSun, scalarFac, torqueAlign, 3); - double scalarFac = 0; - scalarFac = kAlign * alpha / normCrossSun; - VectorOperations::mulScalar(crossSun, scalarFac, torqueAlign, 3); + double rateSafeMode[3] = {0, 0, 0}; + VectorOperations::subtract(satRateMekf, satRatRef, rateSafeMode, 3); + VectorOperations::mulScalar(rateSafeMode, -kRate, torqueRate, 3); - double rateSafeMode[3] = {0,0,0}; - VectorOperations::subtract(satRateMekf, satRatRef, rateSafeMode, 3); - VectorOperations::mulScalar(rateSafeMode, -kRate, torqueRate, 3); + VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); + // Adding factor of inertia for axes + MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); - VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); -// Adding factor of inertia for axes - MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); - -// MagMom B (orthogonal torque) - double torqueMgt[3] = {0,0,0}; - VectorOperations::cross(magFieldB, torqueCmd, torqueMgt); - double normMag = VectorOperations::norm(magFieldB, 3); - VectorOperations::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3); - *outputValid = true; - - return returnvalue::OK; + // MagMom B (orthogonal torque) + double torqueMgt[3] = {0, 0, 0}; + VectorOperations::cross(magFieldB, torqueCmd, torqueMgt); + double normMag = VectorOperations::norm(magFieldB, 3); + VectorOperations::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3); + *outputValid = true; + return returnvalue::OK; } // Will be the version in worst case scenario in event of no working MEKF (nor RMUs) -void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, - double *sunRateB, bool *sunRateBValid, - double *magFieldB, bool *magFieldBValid, - double *magRateB, bool *magRateBValid, - double *sunDirRef, double *satRateRef, - double *outputMagMomB, bool *outputValid){ +void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB, + bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, + double *magRateB, bool *magRateBValid, double *sunDirRef, + double *satRateRef, double *outputMagMomB, bool *outputValid) { + // Check for invalid Inputs + if (!susDirBValid || !magFieldBValid || !magRateBValid) { + *outputValid = false; + return; + } -// Check for invalid Inputs - if ( !susDirBValid || !magFieldBValid || !magRateBValid) { - *outputValid = false; - return; - } + // normalize sunDir and magDir + double magDirB[3] = {0, 0, 0}; + VectorOperations::normalize(magFieldB, magDirB, 3); + VectorOperations::normalize(susDirB, susDirB, 3); -// normalize sunDir and magDir - double magDirB[3] = {0, 0, 0}; - VectorOperations::normalize(magFieldB, magDirB, 3); - VectorOperations::normalize(susDirB, susDirB, 3); + // Cosinus angle between sunDir and magDir + double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); -// Cosinus angle between sunDir and magDir - double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); + // Rate parallel to sun direction and magnetic field direction + double rateParaSun = 0, rateParaMag = 0; + double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0; + dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); + dotmagRateSun = VectorOperations::dot(magRateB, susDirB); + rateFactor = 1 - pow(cosAngleSunMag, 2); + rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; + rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor; -// Rate parallel to sun direction and magnetic field direction - double rateParaSun = 0, rateParaMag = 0; - double dotSunRateMag = 0, dotmagRateSun = 0, - rateFactor = 0; - dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); - dotmagRateSun = VectorOperations::dot(magRateB, susDirB); - rateFactor = 1 - pow(cosAngleSunMag,2); - rateParaSun = ( dotmagRateSun + cosAngleSunMag * dotSunRateMag ) / rateFactor; - rateParaMag = ( dotSunRateMag + cosAngleSunMag * dotmagRateSun ) / rateFactor; + // Full rate or estimate + double estSatRate[3] = {0, 0, 0}; + double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0}; + VectorOperations::mulScalar(susDirB, rateParaSun, estSatRateSun, 3); + VectorOperations::add(sunRateB, estSatRateSun, estSatRateSun, 3); + VectorOperations::mulScalar(magDirB, rateParaMag, estSatRateMag, 3); + VectorOperations::add(magRateB, estSatRateMag, estSatRateMag, 3); + VectorOperations::add(estSatRateSun, estSatRateMag, estSatRate, 3); + VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); -// Full rate or estimate - double estSatRate[3] = {0, 0, 0}; - double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0}; - VectorOperations::mulScalar(susDirB, rateParaSun, estSatRateSun, 3); - VectorOperations::add(sunRateB, estSatRateSun, estSatRateSun, 3); - VectorOperations::mulScalar(magDirB, rateParaMag, estSatRateMag, 3); - VectorOperations::add(magRateB, estSatRateMag, estSatRateMag, 3); - VectorOperations::add(estSatRateSun, estSatRateMag, estSatRate, 3); - VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); + /* Only valid if angle between sun direction and magnetic field direction + is sufficiently large */ -/* Only valid if angle between sun direction and magnetic field direction - is sufficiently large */ + double sinAngle = 0; + sinAngle = sin(acos(cos(cosAngleSunMag))); - double sinAngle = 0; - sinAngle = sin(acos(cos(cosAngleSunMag))); + if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) { + return; + } - if ( !(sinAngle > sin( safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) { - return; - } + // Rate for Torque Calculation + double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ + VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); -// Rate for Torque Calculation - double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ - VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); + // Torque Align calculation + double kRateNoMekf = 0, kAlignNoMekf = 0; + kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; + kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; -// Torque Align calculation - double kRateNoMekf = 0, kAlignNoMekf = 0; - kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; - kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); + double crossSusSunRef[3] = {0, 0, 0}; + VectorOperations::cross(sunDirRef, susDirB, crossSusSunRef); + double sinAngleAlignErr = VectorOperations::norm(crossSusSunRef, 3); - double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); - double crossSusSunRef[3] = {0, 0, 0}; - VectorOperations::cross(sunDirRef, susDirB, crossSusSunRef); - double sinAngleAlignErr = VectorOperations::norm(crossSusSunRef, 3); + double torqueAlign[3] = {0, 0, 0}; + double angleAlignErr = acos(cosAngleAlignErr); + double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr; + VectorOperations::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3); - double torqueAlign[3] = {0, 0, 0}; - double angleAlignErr = acos(cosAngleAlignErr); - double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr; - VectorOperations::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3); + // Torque Rate Calculations + double torqueRate[3] = {0, 0, 0}; + VectorOperations::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3); -//Torque Rate Calculations - double torqueRate[3] = {0, 0, 0}; - VectorOperations::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3); + // Final torque + double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; + VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); + MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, + 1); -//Final torque - double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; - VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, 1); + // Magnetic moment + double magMomB[3] = {0, 0, 0}; + double crossMagFieldTorque[3] = {0, 0, 0}; + VectorOperations::cross(magFieldB, torqueB, crossMagFieldTorque); + double magMomFactor = pow(VectorOperations::norm(magFieldB, 3), 2); + VectorOperations::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); -//Magnetic moment - double magMomB[3] = {0, 0, 0}; - double crossMagFieldTorque[3] = {0, 0, 0}; - VectorOperations::cross(magFieldB, torqueB, crossMagFieldTorque); - double magMomFactor = pow( VectorOperations::norm(magFieldB, 3), 2 ); - VectorOperations::mulScalar(crossMagFieldTorque, 1/magMomFactor, magMomB, 3); - - outputMagMomB[0] = magMomB[0]; - outputMagMomB[1] = magMomB[1]; - outputMagMomB[2] = magMomB[2]; - - *outputValid = true; + outputMagMomB[0] = magMomB[0]; + outputMagMomB[1] = magMomB[1]; + outputMagMomB[2] = magMomB[2]; + *outputValid = true; } - - diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index c6d47324..70426c9f 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -8,57 +8,46 @@ #ifndef SAFECTRL_H_ #define SAFECTRL_H_ -#include "../SensorValues.h" -#include "../OutputValues.h" -#include "../AcsParameters.h" -#include "../config/classIds.h" -#include #include +#include #include -#include +#include "../AcsParameters.h" +#include "../OutputValues.h" +#include "../SensorValues.h" +#include "../config/classIds.h" -class SafeCtrl{ +class SafeCtrl { + public: + SafeCtrl(AcsParameters *acsParameters_); + virtual ~SafeCtrl(); -public: + static const uint8_t INTERFACE_ID = CLASS_ID::SAFE; + static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - SafeCtrl(AcsParameters *acsParameters_); - virtual ~SafeCtrl(); + void loadAcsParameters(AcsParameters *acsParameters_); - static const uint8_t INTERFACE_ID = CLASS_ID::SAFE; - static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, double *magFieldModel, + bool *magFieldModelValid, double *sunDirModel, bool *sunDirModelValid, + double *satRateMekf, bool *rateMekfValid, double *sunDirRef, + double *satRatRef, // From Guidance (!) + double *outputMagMomB, bool *outputValid); - void loadAcsParameters(AcsParameters *acsParameters_); + void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB, + bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, double *magRateB, + bool *magRateBValid, double *sunDirRef, double *satRateRef, double *outputMagMomB, + bool *outputValid); - ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, - double *magFieldModel, bool *magFieldModelValid, - double *sunDirModel, bool *sunDirModelValid, - double *satRateMekf, bool *rateMekfValid, - double *sunDirRef, double *satRatRef, // From Guidance (!) - double *outputMagMomB, bool *outputValid); + void idleSunPointing(); // with reaction wheels - void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, - double *sunRateB, bool *sunRateBValid, - double *magFieldB, bool *magFieldBValid, - double *magRateB, bool *magRateBValid, - double *sunDirRef, double *satRateRef, - double *outputMagMomB, bool *outputValid); - - void idleSunPointing(); // with reaction wheels - -protected: - -private: - AcsParameters::SafeModeControllerParameters* safeModeControllerParameters; - AcsParameters::InertiaEIVE* inertiaEIVE; - double gainMatrixInertia[3][3]; - - double magFieldBState[3]; - timeval magFieldBStateTime; + protected: + private: + AcsParameters::SafeModeControllerParameters *safeModeControllerParameters; + AcsParameters::InertiaEIVE *inertiaEIVE; + double gainMatrixInertia[3][3]; + double magFieldBState[3]; + timeval magFieldBStateTime; }; - - #endif /* ACS_CONTROL_SAFECTRL_H_ */ - From 06941fcf5b0dfcaaa545c42c1f766108f304b696 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 20 Oct 2022 11:08:21 +0200 Subject: [PATCH 070/244] fixed int32_t to double warnings --- mission/controller/acs/ActuatorCmd.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 920989ed..0dba2016 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -45,7 +45,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1 } // Calculating the commanded speed in RPM for every reaction wheel - double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double deltaSpeed[4] = {0, 0, 0, 0}; double commandTime = acsParameters.onBoardParams.sampleTime, inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; From 62bf792888d937a49c934041398a36a1945f143d Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 20 Oct 2022 11:09:02 +0200 Subject: [PATCH 071/244] removed comment --- mission/controller/acs/SensorProcessing.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index a6571496..8c89dda4 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -107,9 +107,6 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const for (uint8_t i = 0; i < 3; i++) { magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } - sif::debug << "magFeildEst: " << magFieldEst[0] << " , " << magFieldEst[1] << " , " - << magFieldEst[2] << std::endl; - ; validMagField = true; //-----------------------Mag Rate Computation --------------------------------------------------- From 0d3509b99164d4277ebfdea6a2f2a0d62c476f82 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 20 Oct 2022 11:09:52 +0200 Subject: [PATCH 072/244] added actuator command values DataSet --- .../AcsCtrlDefinitions.h | 33 ++++++++++++++----- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c122be43..4a608f71 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -16,7 +16,8 @@ enum SetIds : uint32_t { GYR_SENSOR_DATA, GYR_PROCESSED_DATA, GPS_SENSOR_DATA, - MEKF_DATA + MEKF_DATA, + ACTUATOR_CMD_DATA }; enum PoolIds : lp_id_t { @@ -81,22 +82,27 @@ enum PoolIds : lp_id_t { GYR_2_VEC, GYR_3_VEC, GYR_VEC_TOT, - // GPS PROCESSED + // GPS Processed GC_LATITUDE, GD_LONGITUDE, // MEKF SAT_ROT_RATE_MEKF, QUAT_MEKF, + // Actuator Cmd + RW_TARGET_TORQUE, + RW_TARGET_SPEED, + MTQ_TARGET_DIPOLE, }; static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10; -static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 7 * sizeof(float) + 1 * sizeof(double); +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 = 14 * sizeof(float) + 1 * sizeof(double); -static constexpr uint8_t GYR_SET_RAW_ENTRIES = 2 * sizeof(float) + 2 * sizeof(double); -static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5 * sizeof(double); -static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 2 * sizeof(double); -static constexpr uint8_t MEKF_SET_ENTRIES = 14 * sizeof(float) + 1 * sizeof(double); +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 MEKF_SET_ENTRIES = 2; +static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; /** * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. @@ -227,6 +233,17 @@ class MekfData : public StaticLocalDataSet { private: }; +class ActuatorCmdData : public StaticLocalDataSet { + public: + ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {} + + lp_vec_t rwTargetTorque = lp_vec_t(sid.objectId, RW_TARGET_TORQUE, this); + lp_vec_t rwTargetSpeed = lp_vec_t(sid.objectId, RW_TARGET_SPEED, this); + lp_vec_t mtqTargetDipole = lp_vec_t(sid.objectId, MTQ_TARGET_DIPOLE, this); + + private: +}; + } // namespace acsctrl #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */ From 8b23fd3dd261d64090fc5c22c7963f880bc73d1a Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 24 Oct 2022 10:29:57 +0200 Subject: [PATCH 073/244] fixed int32_t to double warnings. reformats --- .../acs/MultiplicativeKalmanFilter.cpp | 2274 ++++++++--------- .../acs/MultiplicativeKalmanFilter.h | 148 +- mission/controller/acs/Navigation.cpp | 8 +- mission/controller/acs/control/PtgCtrl.cpp | 4 +- 4 files changed, 1186 insertions(+), 1248 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5f7facd2..8cfa0ad3 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -6,1199 +6,1143 @@ */ #include "MultiplicativeKalmanFilter.h" -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" -#include -#include + #include #include #include +#include +#include + +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" /*Initialisation of values for parameters in constructor*/ -MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) : - initialQuaternion { 0.5, 0.5, 0.5, 0.5 },initialCovarianceMatrix - {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}, - {0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}}{ - loadAcsParameters(acsParameters_); - +MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) + : initialQuaternion{0.5, 0.5, 0.5, 0.5}, + initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} { + loadAcsParameters(acsParameters_); } -MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() { +MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} +void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) { + inertiaEIVE = &(acsParameters_->inertiaEIVE); + kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ } -void MultiplicativeKalmanFilter::loadAcsParameters( - AcsParameters *acsParameters_) { - inertiaEIVE = &(acsParameters_->inertiaEIVE); - kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/ -} +void MultiplicativeKalmanFilter::reset() {} -void MultiplicativeKalmanFilter::reset() { +void MultiplicativeKalmanFilter::init( + const double *magneticField_, const bool *validMagField_, const double *sunDir_, + const bool *validSS, const double *sunDirJ, const bool *validSSModel, const double *magFieldJ, + const bool *validMagModel) { // valids for "model measurements"? + // 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 -void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *validMagField_, - const double *sunDir_, const bool *validSS, - const double *sunDirJ, const bool *validSSModel, - const double *magFieldJ,const bool *validMagModel) { // valids for "model measurements"? - // 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; + double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, + normSunJ[3] = {0, 0, 0}; + VectorOperations::normalize(magneticField_, normMagB, 3); // b2 + VectorOperations::normalize(sunDir_, normSunB, 3); // b1 + VectorOperations::normalize(magFieldJ, normMagJ, 3); // r2 + VectorOperations::normalize(sunDirJ, normSunJ, 3); // r1 - sigmaGyro = 0.1*3.141/180; // acs parameters + double thirdAxis_B[3] = {0, 0, 0}, thirdAxis_J[3] = {0, 0, 0}; + VectorOperations::cross(normSunJ, normMagJ, thirdAxis_J); + VectorOperations::cross(normSunB, normMagB, thirdAxis_B); + VectorOperations::normalize(thirdAxis_J, thirdAxis_J, 3); // rx + VectorOperations::normalize(thirdAxis_B, thirdAxis_B, 3); // bx - double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0}, - normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0}; - VectorOperations::normalize(magneticField_, normMagB, 3); // b2 - VectorOperations::normalize(sunDir_, normSunB, 3); //b1 - VectorOperations::normalize(magFieldJ, normMagJ, 3); //r2 - VectorOperations::normalize(sunDirJ, normSunJ, 3); //r1 + double weigthSun = 1 / (sigmaSun * sigmaSun); // a1 + double weigthMag = 1 / (sigmaMag * sigmaMag); // a2 - double thirdAxis_B[3] = {0,0,0}, thirdAxis_J[3] = {0,0,0}; - VectorOperations::cross(normSunJ, normMagJ, thirdAxis_J); - VectorOperations::cross(normSunB, normMagB, thirdAxis_B); - VectorOperations::normalize(thirdAxis_J, thirdAxis_J, 3); //rx - VectorOperations::normalize(thirdAxis_B, thirdAxis_B, 3); //bx + double a[3] = {0, 0, 0}, b[3] = {0, 0, 0}; + VectorOperations::mulScalar(normSunB, weigthSun, a, 3); // a + VectorOperations::mulScalar(normMagB, weigthMag, b, 3); // b - double weigthSun= 1 / (sigmaSun * sigmaSun); //a1 - double weigthMag= 1 / (sigmaMag * sigmaMag); //a2 + double thirdAxisCross[3] = {0, 0, 0}, weightACross[3] = {0, 0, 0}, weightBCross[3] = {0, 0, 0}, + weigthCrossSum[3] = {0, 0, 0}; + VectorOperations::cross(thirdAxis_B, thirdAxis_J, + thirdAxisCross); // cross(bx,rx) + VectorOperations::cross(a, normSunJ, weightACross); + VectorOperations::cross(b, normMagJ, weightBCross); + VectorOperations::add(weightACross, weightBCross, weigthCrossSum); - double a[3] = {0,0,0}, b[3] = {0,0,0}; - VectorOperations::mulScalar(normSunB, weigthSun, a, 3); //a - VectorOperations::mulScalar(normMagB, weigthMag, b, 3); //b + double thirdAxisSum[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}; + VectorOperations::add(thirdAxis_B, thirdAxis_J, thirdAxisSum); + VectorOperations::add(weightACross, weightBCross, sum2); - double thirdAxisCross[3] = {0,0,0}, weightACross[3] = {0,0,0}, - weightBCross[3] = {0,0,0},weigthCrossSum[3] = {0,0,0}; - VectorOperations::cross(thirdAxis_B, thirdAxis_J, - thirdAxisCross); //cross(bx,rx) - VectorOperations::cross(a, normSunJ, weightACross); - VectorOperations::cross(b, normMagJ, weightBCross); - VectorOperations::add(weightACross, weightBCross, - weigthCrossSum); + double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0; + alpha = (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)) * + (VectorOperations::dot(a, normSunJ) + + VectorOperations::dot(b, normMagJ)) + + VectorOperations::dot(thirdAxisCross, weigthCrossSum); + beta = VectorOperations::dot(thirdAxisSum, sum2); + gamma = sqrt(alpha * alpha + beta * beta); + constPlus = 1 / (2 * sqrt(gamma * (gamma + alpha) * + (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)))); + constMin = 1 / (2 * sqrt(gamma * (gamma - alpha) * + (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)))); - double thirdAxisSum[3] = {0,0,0}, sum2[3] = {0,0,0}; - VectorOperations::add(thirdAxis_B, thirdAxis_J, thirdAxisSum); - VectorOperations::add(weightACross, weightBCross, sum2); + /*Quaternion Computation*/ + double quat[3] = {0, 0, 0}, var1[3] = {0, 0, 0}, var2[3] = {0, 0, 0}; + if (alpha >= 0) { + VectorOperations::mulScalar(thirdAxisCross, gamma + alpha, var1, 3); + VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); + VectorOperations::mulScalar(var2, beta, var2, 3); + VectorOperations::add(var1, var2, quat); - double alpha = 0, beta = 0, gamma = 0, constPlus = 0, constMin = 0; - alpha = (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)) - * (VectorOperations::dot(a, normSunJ) - + VectorOperations::dot(b, normMagJ)) - + VectorOperations::dot(thirdAxisCross, weigthCrossSum); - beta = VectorOperations::dot(thirdAxisSum, sum2); - gamma = sqrt(alpha * alpha + beta * beta); - constPlus = 1 - / (2 - * sqrt( - gamma * (gamma + alpha) - * (1 - + VectorOperations::dot( - thirdAxis_B, - thirdAxis_J)))); - constMin = 1 - / (2 - * sqrt( - gamma * (gamma - alpha) - * (1 - + VectorOperations::dot( - thirdAxis_B, - thirdAxis_J)))); + for (int i = 0; i < 3; i++) { + initialQuaternion[i] = quat[i]; + } + initialQuaternion[3] = + (gamma + alpha) * (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)); + VectorOperations::mulScalar(initialQuaternion, constPlus, initialQuaternion, 4); + } else { + double diffGammaAlpha = gamma - alpha; + VectorOperations::mulScalar(thirdAxisCross, beta, var1, 3); + VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); + VectorOperations::mulScalar(var2, diffGammaAlpha, var2, 3); + VectorOperations::add(var1, var2, quat); + for (int i = 0; i < 3; i++) { + initialQuaternion[i] = quat[i]; + } + initialQuaternion[3] = beta * (1 + VectorOperations::dot(thirdAxis_B, thirdAxis_J)); + VectorOperations::mulScalar(initialQuaternion, constMin, initialQuaternion, 4); + } + propagatedQuaternion[0] = initialQuaternion[0]; + propagatedQuaternion[1] = initialQuaternion[1]; + propagatedQuaternion[2] = initialQuaternion[2]; + propagatedQuaternion[3] = initialQuaternion[3]; - /*Quaternion Computation*/ - double quat[3] = {0,0,0}, var1[3] = {0,0,0}, var2[3] = {0,0,0}; - if (alpha >= 0) { - VectorOperations::mulScalar(thirdAxisCross, gamma + alpha, - var1, 3); - VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); - VectorOperations::mulScalar(var2, beta, var2, 3); - VectorOperations::add(var1, var2, quat); + /*Initial Covariance Matrix------------------------------------ */ + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0}, + magEstB[3] = {0, 0, 0}; + QuaternionOperations::toDcm(initialQuaternion, dcmBJ); + MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1); - for (int i = 0; i < 3; i++) { - initialQuaternion[i] = quat[i]; - } - initialQuaternion[3] = (gamma + alpha) - * (1 - + VectorOperations::dot(thirdAxis_B, - thirdAxis_J)); - VectorOperations::mulScalar(initialQuaternion, - constPlus, initialQuaternion, 4); - } else { - double diffGammaAlpha = gamma - alpha; - VectorOperations::mulScalar(thirdAxisCross, beta, var1, 3); - VectorOperations::add(thirdAxis_B, thirdAxis_J, var2, 3); - VectorOperations::mulScalar(var2, diffGammaAlpha, var2, 3); - VectorOperations::add(var1, var2, quat); - for (int i = 0; i < 3; i++) { - initialQuaternion[i] = quat[i]; - } - initialQuaternion[3] = beta - * (1 - + VectorOperations::dot(thirdAxis_B, - thirdAxis_J)); - VectorOperations::mulScalar(initialQuaternion, - constMin, initialQuaternion, 4); + double matrixSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + 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); + MatrixOperations::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3); - } - propagatedQuaternion[0] = initialQuaternion[0]; - propagatedQuaternion[1] = initialQuaternion[1]; - propagatedQuaternion[2] = initialQuaternion[2]; - propagatedQuaternion[3] = initialQuaternion[3]; + double partA[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, partB = 0; + MatrixOperations::multiplyScalar(*matrixSun, weigthSun * weigthSun, *matrixSun, 3, 3); + MatrixOperations::multiplyScalar(*matrixMag, weigthMag * weigthMag, *matrixMag, 3, 3); + MatrixOperations::add(*matrixSunMag, *matrixMagSun, *partA, 3, 3); + double factor = weigthMag * weigthSun * VectorOperations::dot(sunEstB, magEstB); + MatrixOperations::multiplyScalar(*partA, factor, *partA, 3, 3); + MatrixOperations::add(*partA, *matrixSun, *partA, 3, 3); + MatrixOperations::add(*partA, *matrixMag, *partA, 3, 3); - /*Initial Covariance Matrix------------------------------------ */ - double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, - sunEstB[3] = {0,0,0}, magEstB[3] = {0,0,0}; - QuaternionOperations::toDcm(initialQuaternion, dcmBJ); - MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); - MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, - 1); + double crossSunMag[3] = {0, 0, 0}; + double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + VectorOperations::cross(sunEstB, magEstB, crossSunMag); + partB = 1 / (weigthMag * weigthSun * pow(VectorOperations::norm(crossSunMag, 3), 2)); + MatrixOperations::multiplyScalar(*partA, partB, *partA, 3, 3); + MatrixOperations::add(*matrixSunMag, *identityMatrix3, *partA, 3, 3); + factor = 1 / (weigthMag + weigthMag); - double matrixSun[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, - 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); - MatrixOperations::multiply(magEstB, sunEstB, *matrixMagSun, 3, 1, 3); - - double partA[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, partB = 0; - MatrixOperations::multiplyScalar(*matrixSun, - weigthSun * weigthSun, *matrixSun, 3, 3); - MatrixOperations::multiplyScalar(*matrixMag, - weigthMag * weigthMag, *matrixMag, 3, 3); - MatrixOperations::add(*matrixSunMag, *matrixMagSun, *partA, 3, - 3); - double factor = weigthMag * weigthSun - * VectorOperations::dot(sunEstB, magEstB); - MatrixOperations::multiplyScalar(*partA, factor, *partA, 3, 3); - MatrixOperations::add(*partA, *matrixSun, *partA, 3, 3); - MatrixOperations::add(*partA, *matrixMag, *partA, 3, 3); - - double crossSunMag[3] = {0,0,0}; - double identityMatrix3[3][3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; - VectorOperations::cross(sunEstB, magEstB, crossSunMag); - partB = - 1 - / (weigthMag * weigthSun - * pow( - VectorOperations::norm( - crossSunMag, 3), 2)); - MatrixOperations::multiplyScalar(*partA, partB, *partA, 3, 3); - MatrixOperations::add(*matrixSunMag, *identityMatrix3, *partA, 3, - 3); - factor = 1 / (weigthMag + weigthMag); - - double questCovMatrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - MatrixOperations::multiplyScalar(*partA, factor, - *questCovMatrix, 3, 3); - double initGyroCov[3][3] = {{pow(sigmaGyro,2),0,0},{0,pow(sigmaGyro,2),0}, - {0,0,pow(sigmaGyro,2)}}; - initialCovarianceMatrix[0][0] = questCovMatrix[0][0]; - initialCovarianceMatrix[0][1] = questCovMatrix[0][1]; - initialCovarianceMatrix[0][2] = questCovMatrix[0][2]; - initialCovarianceMatrix[0][3] = 0; - initialCovarianceMatrix[0][4] = 0; - initialCovarianceMatrix[0][5] = 0; - initialCovarianceMatrix[1][0] = questCovMatrix[1][0]; - initialCovarianceMatrix[1][1] = questCovMatrix[1][1]; - initialCovarianceMatrix[1][2] = questCovMatrix[1][2]; - initialCovarianceMatrix[1][3] = 0; - initialCovarianceMatrix[1][4] = 0; - initialCovarianceMatrix[1][5] = 0; - initialCovarianceMatrix[2][0] = questCovMatrix[2][0]; - initialCovarianceMatrix[2][1] = questCovMatrix[2][1]; - initialCovarianceMatrix[2][2] = questCovMatrix[2][2]; - initialCovarianceMatrix[2][3] = 0; - initialCovarianceMatrix[2][4] = 0; - initialCovarianceMatrix[2][5] = 0; - initialCovarianceMatrix[3][0] = 0; - initialCovarianceMatrix[3][1] = 0; - initialCovarianceMatrix[3][2] = 0; - initialCovarianceMatrix[3][3] = initGyroCov[0][0]; - initialCovarianceMatrix[3][4] = initGyroCov[0][1]; - initialCovarianceMatrix[3][5] = initGyroCov[0][2]; - initialCovarianceMatrix[4][0] = 0; - initialCovarianceMatrix[4][1] = 0; - initialCovarianceMatrix[4][2] = 0; - initialCovarianceMatrix[4][3] = initGyroCov[1][0]; - initialCovarianceMatrix[4][4] = initGyroCov[1][1]; - initialCovarianceMatrix[4][5] = initGyroCov[1][2]; - initialCovarianceMatrix[5][0] = 0; - initialCovarianceMatrix[5][1] = 0; - initialCovarianceMatrix[5][2] = 0; - 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; - } + double questCovMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiplyScalar(*partA, factor, *questCovMatrix, 3, 3); + double initGyroCov[3][3] = { + {pow(sigmaGyro, 2), 0, 0}, {0, pow(sigmaGyro, 2), 0}, {0, 0, pow(sigmaGyro, 2)}}; + initialCovarianceMatrix[0][0] = questCovMatrix[0][0]; + initialCovarianceMatrix[0][1] = questCovMatrix[0][1]; + initialCovarianceMatrix[0][2] = questCovMatrix[0][2]; + initialCovarianceMatrix[0][3] = 0; + initialCovarianceMatrix[0][4] = 0; + initialCovarianceMatrix[0][5] = 0; + initialCovarianceMatrix[1][0] = questCovMatrix[1][0]; + initialCovarianceMatrix[1][1] = questCovMatrix[1][1]; + initialCovarianceMatrix[1][2] = questCovMatrix[1][2]; + initialCovarianceMatrix[1][3] = 0; + initialCovarianceMatrix[1][4] = 0; + initialCovarianceMatrix[1][5] = 0; + initialCovarianceMatrix[2][0] = questCovMatrix[2][0]; + initialCovarianceMatrix[2][1] = questCovMatrix[2][1]; + initialCovarianceMatrix[2][2] = questCovMatrix[2][2]; + initialCovarianceMatrix[2][3] = 0; + initialCovarianceMatrix[2][4] = 0; + initialCovarianceMatrix[2][5] = 0; + initialCovarianceMatrix[3][0] = 0; + initialCovarianceMatrix[3][1] = 0; + initialCovarianceMatrix[3][2] = 0; + initialCovarianceMatrix[3][3] = initGyroCov[0][0]; + initialCovarianceMatrix[3][4] = initGyroCov[0][1]; + initialCovarianceMatrix[3][5] = initGyroCov[0][2]; + initialCovarianceMatrix[4][0] = 0; + initialCovarianceMatrix[4][1] = 0; + initialCovarianceMatrix[4][2] = 0; + initialCovarianceMatrix[4][3] = initGyroCov[1][0]; + initialCovarianceMatrix[4][4] = initGyroCov[1][1]; + initialCovarianceMatrix[4][5] = initGyroCov[1][2]; + initialCovarianceMatrix[5][0] = 0; + initialCovarianceMatrix[5][1] = 0; + initialCovarianceMatrix[5][2] = 0; + 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; + } } // --------------- MEKF DISCRETE TIME STEP ------------------------------- ReturnValue_t MultiplicativeKalmanFilter::mekfEst( - const double *quaternionSTR, const bool *validSTR_, - const double *rateRMUs_, const bool *validRMUs_, - 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 *outputQuat, double *outputSatRate) { - - // Check for RMU Measurements - //AcsParameters mekfEstParams; - //loadAcsParameters(&mekfEstParams); - int MDF = 0; // Matrix Dimension Factor - if (!(*validRMUs_)){ - validMekf = false; - return KALMAN_NO_RMU_MEAS; - } - // Check for Model Calculations - else if (!(*validSSModel) || !(*validMagModel)){ - validMekf = false; - return KALMAN_NO_MODEL; - } - // Check Measurements available from SS, MAG, STR - if (*validSS && *validMagField_ && *validSTR_){ - sensorsAvail = 1; - MDF = 9; - } - else if (*validSS && *validMagField_ && !(*validSTR_)){ - sensorsAvail = 2; - MDF = 6; - } - else if (*validSS && !(*validMagField_) && *validSTR_){ - sensorsAvail = 3; - MDF = 6; - } - else if (!(*validSS) && *validMagField_ && *validSTR_){ - sensorsAvail = 4; - MDF = 6; - } - else if (*validSS && !(*validMagField_) && !(*validSTR_)){ - sensorsAvail = 5; - MDF = 3; - } - else if (!(*validSS) && *validMagField_ && !(*validSTR_)){ - sensorsAvail = 6; - MDF = 3; - } - else if (!(*validSS) && !(*validMagField_) && *validSTR_){ - sensorsAvail = 7; - MDF = 3; - } - else{ - sensorsAvail = 8; //no measurements - validMekf = false; - return returnvalue::FAILED; - } - - // If we are here, MEKF will perform - double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; - sigmaSun = kalmanFilterParameters->sensorNoiseSS; - sigmaMag = kalmanFilterParameters->sensorNoiseMAG; - sigmaStr = kalmanFilterParameters->sensorNoiseSTR; - - double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0}, - normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0}; - VectorOperations::normalize(magneticField_, normMagB, 3); // b2 - VectorOperations::normalize(sunDir_, normSunB, 3); //b1 - VectorOperations::normalize(magFieldJ, normMagJ, 3); //r2 - VectorOperations::normalize(sunDirJ, normSunJ, 3); //r1 - - /*-------GAIN - UPDATE - STEP------*/ - double covMatrix11[3][3] = {{pow(sigmaSun,2),0,0},{0,pow(sigmaSun,2),0},{0,0,pow(sigmaSun,2)}}; - double covMatrix22[3][3] = {{pow(sigmaMag,2),0,0},{0,pow(sigmaMag,2),0},{0,0,pow(sigmaMag,2)}}; - double covMatrix33[3][3] = {{pow(sigmaStr,2),0,0},{0,pow(sigmaStr,2),0},{0,0,pow(sigmaStr,2)}}; - - double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, - sunEstB[3] = {0,0,0}, magEstB[3] = {0,0,0}; - QuaternionOperations::toDcm(propagatedQuaternion, dcmBJ); - MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); - MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, - 1); - double quatEstErr[3] = {0,0,0}; - - //Measurement Sensitivity Matrix - double measSensMatrix11[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; // ss - double measSensMatrix22[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; // mag - double measSensMatrix33[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; // str - MathOperations::skewMatrix(sunEstB, *measSensMatrix11); - MathOperations::skewMatrix(magEstB, *measSensMatrix22); - - double measVecQuat[3] = {0,0,0}; - if (*validSTR_){ - double quatError[4] = {0,0,0,0}; - double invPropagatedQuat[4] = {0,0,0,0}; - QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat); - QuaternionOperations::multiply(quaternionSTR, invPropagatedQuat, quatError); - for (int i = 0 ; i < 3;i++){ - measVecQuat[i]=2*quatError[i]/quatError[3]; - } - } - // Adjust matrices based on valid measurements - double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF], - measEstVec[MDF]; - if (sensorsAvail==1){ //All measurements valid - measSensMatrix[0][0] = measSensMatrix11[0][0]; - measSensMatrix[0][1] = measSensMatrix11[0][1]; - measSensMatrix[0][2] = measSensMatrix11[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix11[1][0]; - measSensMatrix[1][1] = measSensMatrix11[1][1]; - measSensMatrix[1][2] = measSensMatrix11[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix11[2][0]; - measSensMatrix[2][1] = measSensMatrix11[2][1]; - measSensMatrix[2][2] = measSensMatrix11[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - measSensMatrix[3][0] = measSensMatrix22[0][0]; - measSensMatrix[3][1] = measSensMatrix22[0][1]; - measSensMatrix[3][2] = measSensMatrix22[0][2]; - measSensMatrix[3][3] = 0; - measSensMatrix[3][4] = 0; - measSensMatrix[3][5] = 0; - measSensMatrix[4][0] = measSensMatrix22[1][0]; - measSensMatrix[4][1] = measSensMatrix22[1][1]; - measSensMatrix[4][2] = measSensMatrix22[1][2]; - measSensMatrix[4][3] = 0; - measSensMatrix[4][4] = 0; - measSensMatrix[4][5] = 0; - measSensMatrix[5][0] = measSensMatrix22[2][0]; - measSensMatrix[5][1] = measSensMatrix22[2][1]; - measSensMatrix[5][2] = measSensMatrix22[2][2]; - measSensMatrix[5][3] = 0; - measSensMatrix[5][4] = 0; - measSensMatrix[5][5] = 0; - measSensMatrix[6][0] = measSensMatrix33[0][0]; - measSensMatrix[6][1] = measSensMatrix33[0][1]; - measSensMatrix[6][2] = measSensMatrix33[0][2]; - measSensMatrix[6][3] = 0; - measSensMatrix[6][4] = 0; - measSensMatrix[6][5] = 0; - measSensMatrix[7][0] = measSensMatrix33[1][0]; - measSensMatrix[7][1] = measSensMatrix33[1][1]; - measSensMatrix[7][2] = measSensMatrix33[1][2]; - measSensMatrix[7][3] = 0; - measSensMatrix[7][4] = 0; - measSensMatrix[7][5] = 0; - measSensMatrix[8][0] = measSensMatrix33[2][0]; - measSensMatrix[8][1] = measSensMatrix33[2][1]; - measSensMatrix[8][2] = measSensMatrix33[2][2]; - measSensMatrix[8][3] = 0; - measSensMatrix[8][4] = 0; - measSensMatrix[8][5] = 0; - - measCovMatrix[0][0] = covMatrix11[0][0]; - measCovMatrix[0][1] = covMatrix11[0][1]; - measCovMatrix[0][2] = covMatrix11[0][2]; - measCovMatrix[0][3] = 0; - measCovMatrix[0][4] = 0; - measCovMatrix[0][5] = 0; - measCovMatrix[0][6] = 0; - measCovMatrix[0][7] = 0; - measCovMatrix[0][8] = 0; - measCovMatrix[1][0] = covMatrix11[1][0]; - measCovMatrix[1][1] = covMatrix11[1][1]; - measCovMatrix[1][2] = covMatrix11[1][2]; - measCovMatrix[1][3] = 0; - measCovMatrix[1][4] = 0; - measCovMatrix[1][5] = 0; - measCovMatrix[1][6] = 0; - measCovMatrix[1][7] = 0; - measCovMatrix[1][8] = 0; - measCovMatrix[2][0] = covMatrix11[2][0]; - measCovMatrix[2][1] = covMatrix11[2][1]; - measCovMatrix[2][2] = covMatrix11[2][2]; - measCovMatrix[2][3] = 0; - measCovMatrix[2][4] = 0; - measCovMatrix[2][5] = 0; - measCovMatrix[2][6] = 0; - measCovMatrix[2][7] = 0; - measCovMatrix[2][8] = 0; - measCovMatrix[3][0] = 0; - measCovMatrix[3][1] = 0; - measCovMatrix[3][2] = 0; - measCovMatrix[3][3] = covMatrix22[0][0]; - measCovMatrix[3][4] = covMatrix22[0][1]; - measCovMatrix[3][5] = covMatrix22[0][2]; - measCovMatrix[3][6] = 0; - measCovMatrix[3][7] = 0; - measCovMatrix[3][8] = 0; - measCovMatrix[4][0] = 0; - measCovMatrix[4][1] = 0; - measCovMatrix[4][2] = 0; - measCovMatrix[4][3] = covMatrix22[1][0]; - measCovMatrix[4][4] = covMatrix22[1][1]; - measCovMatrix[4][5] = covMatrix22[1][2]; - measCovMatrix[4][6] = 0; - measCovMatrix[4][7] = 0; - measCovMatrix[4][8] = 0; - measCovMatrix[5][0] = 0; - measCovMatrix[5][1] = 0; - measCovMatrix[5][2] = 0; - measCovMatrix[5][3] = covMatrix22[2][0]; - measCovMatrix[5][4] = covMatrix22[2][1]; - measCovMatrix[5][5] = covMatrix22[2][2]; - measCovMatrix[5][6] = 0; - measCovMatrix[5][7] = 0; - measCovMatrix[5][8] = 0; - measCovMatrix[6][0] = 0; - measCovMatrix[6][1] = 0; - measCovMatrix[6][2] = 0; - measCovMatrix[6][3] = 0; - measCovMatrix[6][4] = 0; - measCovMatrix[6][5] = 0; - measCovMatrix[6][6] = covMatrix33[0][0]; - measCovMatrix[6][7] = covMatrix33[0][1]; - measCovMatrix[6][8] = covMatrix33[0][2]; - measCovMatrix[7][0] = 0; - measCovMatrix[7][1] = 0; - measCovMatrix[7][2] = 0; - measCovMatrix[7][3] = 0; - measCovMatrix[7][4] = 0; - measCovMatrix[7][5] = 0; - measCovMatrix[7][6] = covMatrix33[1][0]; - measCovMatrix[7][7] = covMatrix33[1][1]; - measCovMatrix[7][8] = covMatrix33[1][2]; - measCovMatrix[8][0] = 0; - measCovMatrix[8][1] = 0; - measCovMatrix[8][2] = 0; - measCovMatrix[8][3] = 0; - measCovMatrix[8][4] = 0; - measCovMatrix[8][5] = 0; - measCovMatrix[8][6] = covMatrix33[2][0]; - measCovMatrix[8][7] = covMatrix33[2][1]; - measCovMatrix[8][8] = covMatrix33[2][2]; - - measVec[0] = normSunB[0]; - measVec[1] = normSunB[1]; - measVec[2] = normSunB[2]; - measVec[3] = normMagB[0]; - measVec[4] = normMagB[1]; - measVec[5] = normMagB[2]; - measVec[6] = measVecQuat[0]; - measVec[7] = measVecQuat[1]; - measVec[8] = measVecQuat[2]; - - measEstVec[0] = sunEstB[0]; - measEstVec[1] = sunEstB[1]; - measEstVec[2] = sunEstB[2]; - measEstVec[3] = magEstB[0]; - measEstVec[4] = magEstB[1]; - measEstVec[5] = magEstB[2]; - measEstVec[6] = quatEstErr[0]; - measEstVec[7] = quatEstErr[1]; - measEstVec[8] = quatEstErr[2]; - - } - else if(sensorsAvail==2){ // ss / mag valid - measSensMatrix[0][0] = measSensMatrix11[0][0]; - measSensMatrix[0][1] = measSensMatrix11[0][1]; - measSensMatrix[0][2] = measSensMatrix11[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix11[1][0]; - measSensMatrix[1][1] = measSensMatrix11[1][1]; - measSensMatrix[1][2] = measSensMatrix11[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix11[2][0]; - measSensMatrix[2][1] = measSensMatrix11[2][1]; - measSensMatrix[2][2] = measSensMatrix11[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - measSensMatrix[3][0] = measSensMatrix22[0][0]; - measSensMatrix[3][1] = measSensMatrix22[0][1]; - measSensMatrix[3][2] = measSensMatrix22[0][2]; - measSensMatrix[3][3] = 0; - measSensMatrix[3][4] = 0; - measSensMatrix[3][5] = 0; - measSensMatrix[4][0] = measSensMatrix22[1][0]; - measSensMatrix[4][1] = measSensMatrix22[1][1]; - measSensMatrix[4][2] = measSensMatrix22[1][2]; - measSensMatrix[4][3] = 0; - measSensMatrix[4][4] = 0; - measSensMatrix[4][5] = 0; - measSensMatrix[5][0] = measSensMatrix22[2][0]; - measSensMatrix[5][1] = measSensMatrix22[2][1]; - measSensMatrix[5][2] = measSensMatrix22[2][2]; - measSensMatrix[5][3] = 0; - measSensMatrix[5][4] = 0; - measSensMatrix[5][5] = 0; - - measCovMatrix[0][0] = covMatrix11[0][0]; - measCovMatrix[0][1] = covMatrix11[0][1]; - measCovMatrix[0][2] = covMatrix11[0][2]; - measCovMatrix[0][3] = 0; - measCovMatrix[0][4] = 0; - measCovMatrix[0][5] = 0; - measCovMatrix[1][0] = covMatrix11[1][0]; - measCovMatrix[1][1] = covMatrix11[1][1]; - measCovMatrix[1][2] = covMatrix11[1][2]; - measCovMatrix[1][3] = 0; - measCovMatrix[1][4] = 0; - measCovMatrix[1][5] = 0; - measCovMatrix[2][0] = covMatrix11[2][0]; - measCovMatrix[2][1] = covMatrix11[2][1]; - measCovMatrix[2][2] = covMatrix11[2][2]; - measCovMatrix[2][3] = 0; - measCovMatrix[2][4] = 0; - measCovMatrix[2][5] = 0; - measCovMatrix[3][0] = 0; - measCovMatrix[3][1] = 0; - measCovMatrix[3][2] = 0; - measCovMatrix[3][3] = covMatrix22[0][0]; - measCovMatrix[3][4] = covMatrix22[0][1]; - measCovMatrix[3][5] = covMatrix22[0][2]; - measCovMatrix[4][0] = 0; - measCovMatrix[4][1] = 0; - measCovMatrix[4][2] = 0; - measCovMatrix[4][3] = covMatrix22[1][0]; - measCovMatrix[4][4] = covMatrix22[1][1]; - measCovMatrix[4][5] = covMatrix22[1][2]; - measCovMatrix[5][0] = 0; - measCovMatrix[5][1] = 0; - measCovMatrix[5][2] = 0; - measCovMatrix[5][3] = covMatrix22[2][0]; - measCovMatrix[5][4] = covMatrix22[2][1]; - measCovMatrix[5][5] = covMatrix22[2][2]; - - measVec[0] = normSunB[0]; - measVec[1] = normSunB[1]; - measVec[2] = normSunB[2]; - measVec[3] = normMagB[0]; - measVec[4] = normMagB[1]; - measVec[5] = normMagB[2]; - - measEstVec[0] = sunEstB[0]; - measEstVec[1] = sunEstB[1]; - measEstVec[2] = sunEstB[2]; - measEstVec[3] = magEstB[0]; - measEstVec[4] = magEstB[1]; - measEstVec[5] = magEstB[2]; - - } - else if(sensorsAvail==3){ // ss / str valid - - measSensMatrix[0][0] = measSensMatrix11[0][0]; - measSensMatrix[0][1] = measSensMatrix11[0][1]; - measSensMatrix[0][2] = measSensMatrix11[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix11[1][0]; - measSensMatrix[1][1] = measSensMatrix11[1][1]; - measSensMatrix[1][2] = measSensMatrix11[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix11[2][0]; - measSensMatrix[2][1] = measSensMatrix11[2][1]; - measSensMatrix[2][2] = measSensMatrix11[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - measSensMatrix[3][0] = measSensMatrix33[0][0]; - measSensMatrix[3][1] = measSensMatrix33[0][1]; - measSensMatrix[3][2] = measSensMatrix33[0][2]; - measSensMatrix[3][3] = 0; - measSensMatrix[3][4] = 0; - measSensMatrix[3][5] = 0; - measSensMatrix[4][0] = measSensMatrix33[1][0]; - measSensMatrix[4][1] = measSensMatrix33[1][1]; - measSensMatrix[4][2] = measSensMatrix33[1][2]; - measSensMatrix[4][3] = 0; - measSensMatrix[4][4] = 0; - measSensMatrix[4][5] = 0; - measSensMatrix[5][0] = measSensMatrix33[2][0]; - measSensMatrix[5][1] = measSensMatrix33[2][1]; - measSensMatrix[5][2] = measSensMatrix33[2][2]; - measSensMatrix[5][3] = 0; - measSensMatrix[5][4] = 0; - measSensMatrix[5][5] = 0; - - measCovMatrix[0][0] = covMatrix11[0][0]; - measCovMatrix[0][1] = covMatrix11[0][1]; - measCovMatrix[0][2] = covMatrix11[0][2]; - measCovMatrix[0][3] = 0; - measCovMatrix[0][4] = 0; - measCovMatrix[0][5] = 0; - measCovMatrix[1][0] = covMatrix11[1][0]; - measCovMatrix[1][1] = covMatrix11[1][1]; - measCovMatrix[1][2] = covMatrix11[1][2]; - measCovMatrix[1][3] = 0; - measCovMatrix[1][4] = 0; - measCovMatrix[1][5] = 0; - measCovMatrix[2][0] = covMatrix11[2][0]; - measCovMatrix[2][1] = covMatrix11[2][1]; - measCovMatrix[2][2] = covMatrix11[2][2]; - measCovMatrix[2][3] = 0; - measCovMatrix[2][4] = 0; - measCovMatrix[2][5] = 0; - measCovMatrix[3][0] = 0; - measCovMatrix[3][1] = 0; - measCovMatrix[3][2] = 0; - measCovMatrix[3][3] = covMatrix33[0][0]; - measCovMatrix[3][4] = covMatrix33[0][1]; - measCovMatrix[3][5] = covMatrix33[0][2]; - measCovMatrix[4][0] = 0; - measCovMatrix[4][1] = 0; - measCovMatrix[4][2] = 0; - measCovMatrix[4][3] = covMatrix33[1][0]; - measCovMatrix[4][4] = covMatrix33[1][1]; - measCovMatrix[4][5] = covMatrix33[1][2]; - measCovMatrix[5][0] = 0; - measCovMatrix[5][1] = 0; - measCovMatrix[5][2] = 0; - measCovMatrix[5][3] = covMatrix33[2][0]; - measCovMatrix[5][4] = covMatrix33[2][1]; - measCovMatrix[5][5] = covMatrix33[2][2]; - - measVec[0] = normSunB[0]; - measVec[1] = normSunB[1]; - measVec[2] = normSunB[2]; - measVec[3] = measVecQuat[0]; - measVec[4] = measVecQuat[1]; - measVec[5] = measVecQuat[2]; - - measEstVec[0] = sunEstB[0]; - measEstVec[1] = sunEstB[1]; - measEstVec[2] = sunEstB[2]; - measEstVec[3] = quatEstErr[0]; - measEstVec[4] = quatEstErr[1]; - measEstVec[5] = quatEstErr[2]; - - } - else if (sensorsAvail == 4){ // mag / str avail - - measSensMatrix[0][0] = measSensMatrix22[0][0]; - measSensMatrix[0][1] = measSensMatrix22[0][1]; - measSensMatrix[0][2] = measSensMatrix22[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix22[1][0]; - measSensMatrix[1][1] = measSensMatrix22[1][1]; - measSensMatrix[1][2] = measSensMatrix22[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix22[2][0]; - measSensMatrix[2][1] = measSensMatrix22[2][1]; - measSensMatrix[2][2] = measSensMatrix22[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - measSensMatrix[3][0] = measSensMatrix33[0][0]; - measSensMatrix[3][1] = measSensMatrix33[0][1]; - measSensMatrix[3][2] = measSensMatrix33[0][2]; - measSensMatrix[3][3] = 0; - measSensMatrix[3][4] = 0; - measSensMatrix[3][5] = 0; - measSensMatrix[4][0] = measSensMatrix33[1][0]; - measSensMatrix[4][1] = measSensMatrix33[1][1]; - measSensMatrix[4][2] = measSensMatrix33[1][2]; - measSensMatrix[4][3] = 0; - measSensMatrix[4][4] = 0; - measSensMatrix[4][5] = 0; - measSensMatrix[5][0] = measSensMatrix33[2][0]; - measSensMatrix[5][1] = measSensMatrix33[2][1]; - measSensMatrix[5][2] = measSensMatrix33[2][2]; - measSensMatrix[5][3] = 0; - measSensMatrix[5][4] = 0; - measSensMatrix[5][5] = 0; - - measCovMatrix[0][0] = covMatrix22[0][0]; - measCovMatrix[0][1] = covMatrix22[0][1]; - measCovMatrix[0][2] = covMatrix22[0][2]; - measCovMatrix[0][3] = 0; - measCovMatrix[0][4] = 0; - measCovMatrix[0][5] = 0; - measCovMatrix[1][0] = covMatrix22[1][0]; - measCovMatrix[1][1] = covMatrix22[1][1]; - measCovMatrix[1][2] = covMatrix22[1][2]; - measCovMatrix[1][3] = 0; - measCovMatrix[1][4] = 0; - measCovMatrix[1][5] = 0; - measCovMatrix[2][0] = covMatrix22[2][0]; - measCovMatrix[2][1] = covMatrix22[2][1]; - measCovMatrix[2][2] = covMatrix22[2][2]; - measCovMatrix[2][3] = 0; - measCovMatrix[2][4] = 0; - measCovMatrix[2][5] = 0; - measCovMatrix[3][0] = 0; - measCovMatrix[3][1] = 0; - measCovMatrix[3][2] = 0; - measCovMatrix[3][3] = covMatrix33[0][0]; - measCovMatrix[3][4] = covMatrix33[0][1]; - measCovMatrix[3][5] = covMatrix33[0][2]; - measCovMatrix[4][0] = 0; - measCovMatrix[4][1] = 0; - measCovMatrix[4][2] = 0; - measCovMatrix[4][3] = covMatrix33[1][0]; - measCovMatrix[4][4] = covMatrix33[1][1]; - measCovMatrix[4][5] = covMatrix33[1][2]; - measCovMatrix[5][0] = 0; - measCovMatrix[5][1] = 0; - measCovMatrix[5][2] = 0; - measCovMatrix[5][3] = covMatrix33[2][0]; - measCovMatrix[5][4] = covMatrix33[2][1]; - measCovMatrix[5][5] = covMatrix33[2][2]; - - measVec[0] = normMagB[0]; - measVec[1] = normMagB[1]; - measVec[2] = normMagB[2]; - measVec[3] = measVecQuat[0]; - measVec[4] = measVecQuat[1]; - measVec[5] = measVecQuat[2]; - - measEstVec[0] = magEstB[0]; - measEstVec[1] = magEstB[1]; - measEstVec[2] = magEstB[2]; - measEstVec[3] = quatEstErr[0]; - measEstVec[4] = quatEstErr[1]; - measEstVec[5] = quatEstErr[2]; - - } - else if (sensorsAvail == 5){ // only ss - - measSensMatrix[0][0] = measSensMatrix11[0][0]; - measSensMatrix[0][1] = measSensMatrix11[0][1]; - measSensMatrix[0][2] = measSensMatrix11[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix11[1][0]; - measSensMatrix[1][1] = measSensMatrix11[1][1]; - measSensMatrix[1][2] = measSensMatrix11[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix11[2][0]; - measSensMatrix[2][1] = measSensMatrix11[2][1]; - measSensMatrix[2][2] = measSensMatrix11[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - - measCovMatrix[0][0] = covMatrix11[0][0]; - measCovMatrix[0][1] = covMatrix11[0][1]; - measCovMatrix[0][2] = covMatrix11[0][2]; - measCovMatrix[1][0] = covMatrix11[1][0]; - measCovMatrix[1][1] = covMatrix11[1][1]; - measCovMatrix[1][2] = covMatrix11[1][2]; - measCovMatrix[2][0] = covMatrix11[2][0]; - measCovMatrix[2][1] = covMatrix11[2][1]; - measCovMatrix[2][2] = covMatrix11[2][2]; - - measVec[0] = normSunB[0]; - measVec[1] = normSunB[1]; - measVec[2] = normSunB[2]; - - measEstVec[0] = sunEstB[0]; - measEstVec[1] = sunEstB[1]; - measEstVec[2] = sunEstB[2]; - - } - else if (sensorsAvail == 6){ // only mag - - measSensMatrix[0][0] = measSensMatrix22[0][0]; - measSensMatrix[0][1] = measSensMatrix22[0][1]; - measSensMatrix[0][2] = measSensMatrix22[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix22[1][0]; - measSensMatrix[1][1] = measSensMatrix22[1][1]; - measSensMatrix[1][2] = measSensMatrix22[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix22[2][0]; - measSensMatrix[2][1] = measSensMatrix22[2][1]; - measSensMatrix[2][2] = measSensMatrix22[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - - measCovMatrix[0][0] = covMatrix22[0][0]; - measCovMatrix[0][1] = covMatrix22[0][1]; - measCovMatrix[0][2] = covMatrix22[0][2]; - measCovMatrix[1][0] = covMatrix22[1][0]; - measCovMatrix[1][1] = covMatrix22[1][1]; - measCovMatrix[1][2] = covMatrix22[1][2]; - measCovMatrix[2][0] = covMatrix22[2][0]; - measCovMatrix[2][1] = covMatrix22[2][1]; - measCovMatrix[2][2] = covMatrix22[2][2]; - - measVec[0] = normMagB[0]; - measVec[1] = normMagB[1]; - measVec[2] = normMagB[2]; - - measEstVec[0] = magEstB[0]; - measEstVec[1] = magEstB[1]; - measEstVec[2] = magEstB[2]; - - } - else if (sensorsAvail == 7){ // only str - - measSensMatrix[0][0] = measSensMatrix33[0][0]; - measSensMatrix[0][1] = measSensMatrix33[0][1]; - measSensMatrix[0][2] = measSensMatrix33[0][2]; - measSensMatrix[0][3] = 0; - measSensMatrix[0][4] = 0; - measSensMatrix[0][5] = 0; - measSensMatrix[1][0] = measSensMatrix33[1][0]; - measSensMatrix[1][1] = measSensMatrix33[1][1]; - measSensMatrix[1][2] = measSensMatrix33[1][2]; - measSensMatrix[1][3] = 0; - measSensMatrix[1][4] = 0; - measSensMatrix[1][5] = 0; - measSensMatrix[2][0] = measSensMatrix33[2][0]; - measSensMatrix[2][1] = measSensMatrix33[2][1]; - measSensMatrix[2][2] = measSensMatrix33[2][2]; - measSensMatrix[2][3] = 0; - measSensMatrix[2][4] = 0; - measSensMatrix[2][5] = 0; - - measCovMatrix[0][0] = covMatrix33[0][0]; - measCovMatrix[0][1] = covMatrix33[0][1]; - measCovMatrix[0][2] = covMatrix33[0][2]; - measCovMatrix[1][0] = covMatrix33[1][0]; - measCovMatrix[1][1] = covMatrix33[1][1]; - measCovMatrix[1][2] = covMatrix33[1][2]; - measCovMatrix[2][0] = covMatrix33[2][0]; - measCovMatrix[2][1] = covMatrix33[2][1]; - measCovMatrix[2][2] = covMatrix33[2][2]; - - measVec[0] = measVecQuat[0]; - measVec[1] = measVecQuat[1]; - measVec[2] = measVecQuat[2]; - - measEstVec[0] = quatEstErr[0]; - measEstVec[1] = quatEstErr[1]; - measEstVec[2] = quatEstErr[2]; - - } - // Kalman Gain: [K = P * H' / (H * P * H' + R)] - double kalmanGain[6][MDF] = {{0}}, kalmanGain1[6][MDF] = {{0}}; - - double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF], - residualCov1[6][MDF]; - // H * P * H' - MatrixOperations::transpose(*measSensMatrix,*measSensMatrixTrans,6,MDF); - MatrixOperations::multiply(*initialCovarianceMatrix,*measSensMatrixTrans,*residualCov1,6,6,MDF); - MatrixOperations::multiply(*measSensMatrix,*residualCov1,*residualCov,MDF,6,MDF); - // negative values, restrictions ? - // (H * P * H' + R) - MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); - // <> - double invResidualCov1[MDF] = {0}; - double invResidualCov[MDF][MDF] = {{0}}; - int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov,*invResidualCov,invResidualCov1,MDF); - if(inversionFailed) - { - validMekf = false; - return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed - } - - - // [K = P * H' / (H * P * H' + R)] - MatrixOperations::multiply(*measSensMatrixTrans,*invResidualCov,*kalmanGain1,6,MDF,MDF); - MatrixOperations::multiply(*initialCovarianceMatrix,*kalmanGain1,*kalmanGain,6,6,MDF); - - /* ------- UPDATE -STEP ---------*/ - - // Update Covariance Matrix: P_plus = (I-K*H)*P_min - double covMatPlus[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 identityMatrix[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 updateCov1[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}}; - MatrixOperations::multiply(*kalmanGain,*measSensMatrix,*updateCov1,6,MDF,MDF); - MatrixOperations::subtract(*identityMatrix,*updateCov1,*updateCov1,6,6); - MatrixOperations::multiply(*updateCov1,*initialCovarianceMatrix,*covMatPlus,6,6,6); - - // Error State Vector - double errStateVec[6] = {0,0,0,0,0,0}; - double errStateVec1[MDF] = {0}; - VectorOperations::subtract(measVec,measEstVec,errStateVec1,MDF); - MatrixOperations::multiply(*kalmanGain,errStateVec1,errStateVec,6,MDF,1); - - double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]}; - double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]}; - - // State Vector Elements - double xi1[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, - xi2[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - MathOperations::skewMatrix(propagatedQuaternion, *xi2); - double identityMatrix3[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; - MatrixOperations::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); - MatrixOperations::add(*xi1, *xi2, *xi1, 3, 3); - double xi[4][3] = {{xi1[0][0],xi1[0][1],xi1[0][2]}, - {xi1[1][0],xi1[1][1],xi1[1][2]}, - {xi1[2][0],xi1[2][1],xi1[2][2]}, - {-propagatedQuaternion[0],-propagatedQuaternion[1],-propagatedQuaternion[2]}}; - - double errQuatTerm[4] = {0,0,0,0}; - MatrixOperations::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1); - VectorOperations::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4); - VectorOperations::add(propagatedQuaternion, errQuatTerm, quatBJ, 4); - VectorOperations::normalize(quatBJ, quatBJ, 4); - - outputQuat[0] = quatBJ[0]; - outputQuat[1] = quatBJ[1]; - outputQuat[2] = quatBJ[2]; - outputQuat[3] = quatBJ[3]; - - double updatedGyroBias[3] = {0,0,0}; - VectorOperations::add(biasRMU, errStateGyroBias, updatedGyroBias, 3); - // Bias RMU State - biasRMU[0] = updatedGyroBias[0]; - biasRMU[1] = updatedGyroBias[1]; - biasRMU[2] = updatedGyroBias[2]; - - - /* ----------- 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 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}; - VectorOperations::subtract(rateRMUs_, 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}}; - - // Corrected Sat Rate via Bias - outputSatRate[0] = rotRateEst[0]; - outputSatRate[1] = rotRateEst[1]; - outputSatRate[2] = rotRateEst[2]; - - // 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); - - 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]; - } - 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 covQ21[3][3], covQ22[3][3], covQ23[3][3]; - double fact4 = (0.5*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::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]; - } - - // 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}}, - 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 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); - - 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]; - - // Propagated Quaternion - double rotSin[3] = {0,0,0}, - omega1[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); - - 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]}, - {-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]; - 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); - validMekf = true; - - - // Discrete Time Step - - // Check for new data in measurement -> SensorProcessing ? - - return returnvalue::OK; + 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 *outputQuat, double *outputSatRate) { + // Check for GYR Measurements + // AcsParameters mekfEstParams; + // loadAcsParameters(&mekfEstParams); + int MDF = 0; // Matrix Dimension Factor + if (!(*validGYRs_)) { + validMekf = false; + return KALMAN_NO_GYR_MEAS; + } + // Check for Model Calculations + else if (!(*validSSModel) || !(*validMagModel)) { + validMekf = false; + return KALMAN_NO_MODEL; + } + // Check Measurements available from SS, MAG, STR + if (*validSS && *validMagField_ && *validSTR_) { + sensorsAvail = 1; + MDF = 9; + } else if (*validSS && *validMagField_ && !(*validSTR_)) { + sensorsAvail = 2; + MDF = 6; + } else if (*validSS && !(*validMagField_) && *validSTR_) { + sensorsAvail = 3; + MDF = 6; + } else if (!(*validSS) && *validMagField_ && *validSTR_) { + sensorsAvail = 4; + MDF = 6; + } else if (*validSS && !(*validMagField_) && !(*validSTR_)) { + sensorsAvail = 5; + MDF = 3; + } else if (!(*validSS) && *validMagField_ && !(*validSTR_)) { + sensorsAvail = 6; + MDF = 3; + } else if (!(*validSS) && !(*validMagField_) && *validSTR_) { + sensorsAvail = 7; + MDF = 3; + } else { + sensorsAvail = 8; // no measurements + validMekf = false; + return returnvalue::FAILED; + } + + // If we are here, MEKF will perform + double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; + sigmaSun = kalmanFilterParameters->sensorNoiseSS; + sigmaMag = kalmanFilterParameters->sensorNoiseMAG; + sigmaStr = kalmanFilterParameters->sensorNoiseSTR; + + double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, + normSunJ[3] = {0, 0, 0}; + VectorOperations::normalize(magneticField_, normMagB, 3); // b2 + VectorOperations::normalize(sunDir_, normSunB, 3); // b1 + VectorOperations::normalize(magFieldJ, normMagJ, 3); // r2 + VectorOperations::normalize(sunDirJ, normSunJ, 3); // r1 + + /*-------GAIN - UPDATE - STEP------*/ + double covMatrix11[3][3] = { + {pow(sigmaSun, 2), 0, 0}, {0, pow(sigmaSun, 2), 0}, {0, 0, pow(sigmaSun, 2)}}; + double covMatrix22[3][3] = { + {pow(sigmaMag, 2), 0, 0}, {0, pow(sigmaMag, 2), 0}, {0, 0, pow(sigmaMag, 2)}}; + double covMatrix33[3][3] = { + {pow(sigmaStr, 2), 0, 0}, {0, pow(sigmaStr, 2), 0}, {0, 0, pow(sigmaStr, 2)}}; + + double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, sunEstB[3] = {0, 0, 0}, + magEstB[3] = {0, 0, 0}; + QuaternionOperations::toDcm(propagatedQuaternion, dcmBJ); + MatrixOperations::multiply(*dcmBJ, normSunJ, sunEstB, 3, 3, 1); + MatrixOperations::multiply(*dcmBJ, normMagJ, magEstB, 3, 3, 1); + double quatEstErr[3] = {0, 0, 0}; + + // Measurement Sensitivity Matrix + double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss + double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag + double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str + MathOperations::skewMatrix(sunEstB, *measSensMatrix11); + MathOperations::skewMatrix(magEstB, *measSensMatrix22); + + double measVecQuat[3] = {0, 0, 0}; + if (*validSTR_) { + double quatError[4] = {0, 0, 0, 0}; + double invPropagatedQuat[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat); + QuaternionOperations::multiply(quaternionSTR, invPropagatedQuat, quatError); + for (int i = 0; i < 3; i++) { + measVecQuat[i] = 2 * quatError[i] / quatError[3]; + } + } + // Adjust matrices based on valid measurements + double measSensMatrix[MDF][6], measCovMatrix[MDF][MDF], measVec[MDF], measEstVec[MDF]; + if (sensorsAvail == 1) { // All measurements valid + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix22[0][0]; + measSensMatrix[3][1] = measSensMatrix22[0][1]; + measSensMatrix[3][2] = measSensMatrix22[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix22[1][0]; + measSensMatrix[4][1] = measSensMatrix22[1][1]; + measSensMatrix[4][2] = measSensMatrix22[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix22[2][0]; + measSensMatrix[5][1] = measSensMatrix22[2][1]; + measSensMatrix[5][2] = measSensMatrix22[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + measSensMatrix[6][0] = measSensMatrix33[0][0]; + measSensMatrix[6][1] = measSensMatrix33[0][1]; + measSensMatrix[6][2] = measSensMatrix33[0][2]; + measSensMatrix[6][3] = 0; + measSensMatrix[6][4] = 0; + measSensMatrix[6][5] = 0; + measSensMatrix[7][0] = measSensMatrix33[1][0]; + measSensMatrix[7][1] = measSensMatrix33[1][1]; + measSensMatrix[7][2] = measSensMatrix33[1][2]; + measSensMatrix[7][3] = 0; + measSensMatrix[7][4] = 0; + measSensMatrix[7][5] = 0; + measSensMatrix[8][0] = measSensMatrix33[2][0]; + measSensMatrix[8][1] = measSensMatrix33[2][1]; + measSensMatrix[8][2] = measSensMatrix33[2][2]; + measSensMatrix[8][3] = 0; + measSensMatrix[8][4] = 0; + measSensMatrix[8][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[0][6] = 0; + measCovMatrix[0][7] = 0; + measCovMatrix[0][8] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[1][6] = 0; + measCovMatrix[1][7] = 0; + measCovMatrix[1][8] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[2][6] = 0; + measCovMatrix[2][7] = 0; + measCovMatrix[2][8] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix22[0][0]; + measCovMatrix[3][4] = covMatrix22[0][1]; + measCovMatrix[3][5] = covMatrix22[0][2]; + measCovMatrix[3][6] = 0; + measCovMatrix[3][7] = 0; + measCovMatrix[3][8] = 0; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix22[1][0]; + measCovMatrix[4][4] = covMatrix22[1][1]; + measCovMatrix[4][5] = covMatrix22[1][2]; + measCovMatrix[4][6] = 0; + measCovMatrix[4][7] = 0; + measCovMatrix[4][8] = 0; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix22[2][0]; + measCovMatrix[5][4] = covMatrix22[2][1]; + measCovMatrix[5][5] = covMatrix22[2][2]; + measCovMatrix[5][6] = 0; + measCovMatrix[5][7] = 0; + measCovMatrix[5][8] = 0; + measCovMatrix[6][0] = 0; + measCovMatrix[6][1] = 0; + measCovMatrix[6][2] = 0; + measCovMatrix[6][3] = 0; + measCovMatrix[6][4] = 0; + measCovMatrix[6][5] = 0; + measCovMatrix[6][6] = covMatrix33[0][0]; + measCovMatrix[6][7] = covMatrix33[0][1]; + measCovMatrix[6][8] = covMatrix33[0][2]; + measCovMatrix[7][0] = 0; + measCovMatrix[7][1] = 0; + measCovMatrix[7][2] = 0; + measCovMatrix[7][3] = 0; + measCovMatrix[7][4] = 0; + measCovMatrix[7][5] = 0; + measCovMatrix[7][6] = covMatrix33[1][0]; + measCovMatrix[7][7] = covMatrix33[1][1]; + measCovMatrix[7][8] = covMatrix33[1][2]; + measCovMatrix[8][0] = 0; + measCovMatrix[8][1] = 0; + measCovMatrix[8][2] = 0; + measCovMatrix[8][3] = 0; + measCovMatrix[8][4] = 0; + measCovMatrix[8][5] = 0; + measCovMatrix[8][6] = covMatrix33[2][0]; + measCovMatrix[8][7] = covMatrix33[2][1]; + measCovMatrix[8][8] = covMatrix33[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = normMagB[0]; + measVec[4] = normMagB[1]; + measVec[5] = normMagB[2]; + measVec[6] = measVecQuat[0]; + measVec[7] = measVecQuat[1]; + measVec[8] = measVecQuat[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = magEstB[0]; + measEstVec[4] = magEstB[1]; + measEstVec[5] = magEstB[2]; + measEstVec[6] = quatEstErr[0]; + measEstVec[7] = quatEstErr[1]; + measEstVec[8] = quatEstErr[2]; + + } else if (sensorsAvail == 2) { // ss / mag valid + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix22[0][0]; + measSensMatrix[3][1] = measSensMatrix22[0][1]; + measSensMatrix[3][2] = measSensMatrix22[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix22[1][0]; + measSensMatrix[4][1] = measSensMatrix22[1][1]; + measSensMatrix[4][2] = measSensMatrix22[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix22[2][0]; + measSensMatrix[5][1] = measSensMatrix22[2][1]; + measSensMatrix[5][2] = measSensMatrix22[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix22[0][0]; + measCovMatrix[3][4] = covMatrix22[0][1]; + measCovMatrix[3][5] = covMatrix22[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix22[1][0]; + measCovMatrix[4][4] = covMatrix22[1][1]; + measCovMatrix[4][5] = covMatrix22[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix22[2][0]; + measCovMatrix[5][4] = covMatrix22[2][1]; + measCovMatrix[5][5] = covMatrix22[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = normMagB[0]; + measVec[4] = normMagB[1]; + measVec[5] = normMagB[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = magEstB[0]; + measEstVec[4] = magEstB[1]; + measEstVec[5] = magEstB[2]; + + } else if (sensorsAvail == 3) { // ss / str valid + + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix33[0][0]; + measSensMatrix[3][1] = measSensMatrix33[0][1]; + measSensMatrix[3][2] = measSensMatrix33[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix33[1][0]; + measSensMatrix[4][1] = measSensMatrix33[1][1]; + measSensMatrix[4][2] = measSensMatrix33[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix33[2][0]; + measSensMatrix[5][1] = measSensMatrix33[2][1]; + measSensMatrix[5][2] = measSensMatrix33[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix33[0][0]; + measCovMatrix[3][4] = covMatrix33[0][1]; + measCovMatrix[3][5] = covMatrix33[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix33[1][0]; + measCovMatrix[4][4] = covMatrix33[1][1]; + measCovMatrix[4][5] = covMatrix33[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix33[2][0]; + measCovMatrix[5][4] = covMatrix33[2][1]; + measCovMatrix[5][5] = covMatrix33[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + measVec[3] = measVecQuat[0]; + measVec[4] = measVecQuat[1]; + measVec[5] = measVecQuat[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + measEstVec[3] = quatEstErr[0]; + measEstVec[4] = quatEstErr[1]; + measEstVec[5] = quatEstErr[2]; + + } else if (sensorsAvail == 4) { // mag / str avail + + measSensMatrix[0][0] = measSensMatrix22[0][0]; + measSensMatrix[0][1] = measSensMatrix22[0][1]; + measSensMatrix[0][2] = measSensMatrix22[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix22[1][0]; + measSensMatrix[1][1] = measSensMatrix22[1][1]; + measSensMatrix[1][2] = measSensMatrix22[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix22[2][0]; + measSensMatrix[2][1] = measSensMatrix22[2][1]; + measSensMatrix[2][2] = measSensMatrix22[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + measSensMatrix[3][0] = measSensMatrix33[0][0]; + measSensMatrix[3][1] = measSensMatrix33[0][1]; + measSensMatrix[3][2] = measSensMatrix33[0][2]; + measSensMatrix[3][3] = 0; + measSensMatrix[3][4] = 0; + measSensMatrix[3][5] = 0; + measSensMatrix[4][0] = measSensMatrix33[1][0]; + measSensMatrix[4][1] = measSensMatrix33[1][1]; + measSensMatrix[4][2] = measSensMatrix33[1][2]; + measSensMatrix[4][3] = 0; + measSensMatrix[4][4] = 0; + measSensMatrix[4][5] = 0; + measSensMatrix[5][0] = measSensMatrix33[2][0]; + measSensMatrix[5][1] = measSensMatrix33[2][1]; + measSensMatrix[5][2] = measSensMatrix33[2][2]; + measSensMatrix[5][3] = 0; + measSensMatrix[5][4] = 0; + measSensMatrix[5][5] = 0; + + measCovMatrix[0][0] = covMatrix22[0][0]; + measCovMatrix[0][1] = covMatrix22[0][1]; + measCovMatrix[0][2] = covMatrix22[0][2]; + measCovMatrix[0][3] = 0; + measCovMatrix[0][4] = 0; + measCovMatrix[0][5] = 0; + measCovMatrix[1][0] = covMatrix22[1][0]; + measCovMatrix[1][1] = covMatrix22[1][1]; + measCovMatrix[1][2] = covMatrix22[1][2]; + measCovMatrix[1][3] = 0; + measCovMatrix[1][4] = 0; + measCovMatrix[1][5] = 0; + measCovMatrix[2][0] = covMatrix22[2][0]; + measCovMatrix[2][1] = covMatrix22[2][1]; + measCovMatrix[2][2] = covMatrix22[2][2]; + measCovMatrix[2][3] = 0; + measCovMatrix[2][4] = 0; + measCovMatrix[2][5] = 0; + measCovMatrix[3][0] = 0; + measCovMatrix[3][1] = 0; + measCovMatrix[3][2] = 0; + measCovMatrix[3][3] = covMatrix33[0][0]; + measCovMatrix[3][4] = covMatrix33[0][1]; + measCovMatrix[3][5] = covMatrix33[0][2]; + measCovMatrix[4][0] = 0; + measCovMatrix[4][1] = 0; + measCovMatrix[4][2] = 0; + measCovMatrix[4][3] = covMatrix33[1][0]; + measCovMatrix[4][4] = covMatrix33[1][1]; + measCovMatrix[4][5] = covMatrix33[1][2]; + measCovMatrix[5][0] = 0; + measCovMatrix[5][1] = 0; + measCovMatrix[5][2] = 0; + measCovMatrix[5][3] = covMatrix33[2][0]; + measCovMatrix[5][4] = covMatrix33[2][1]; + measCovMatrix[5][5] = covMatrix33[2][2]; + + measVec[0] = normMagB[0]; + measVec[1] = normMagB[1]; + measVec[2] = normMagB[2]; + measVec[3] = measVecQuat[0]; + measVec[4] = measVecQuat[1]; + measVec[5] = measVecQuat[2]; + + measEstVec[0] = magEstB[0]; + measEstVec[1] = magEstB[1]; + measEstVec[2] = magEstB[2]; + measEstVec[3] = quatEstErr[0]; + measEstVec[4] = quatEstErr[1]; + measEstVec[5] = quatEstErr[2]; + + } else if (sensorsAvail == 5) { // only ss + + measSensMatrix[0][0] = measSensMatrix11[0][0]; + measSensMatrix[0][1] = measSensMatrix11[0][1]; + measSensMatrix[0][2] = measSensMatrix11[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix11[1][0]; + measSensMatrix[1][1] = measSensMatrix11[1][1]; + measSensMatrix[1][2] = measSensMatrix11[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix11[2][0]; + measSensMatrix[2][1] = measSensMatrix11[2][1]; + measSensMatrix[2][2] = measSensMatrix11[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix11[0][0]; + measCovMatrix[0][1] = covMatrix11[0][1]; + measCovMatrix[0][2] = covMatrix11[0][2]; + measCovMatrix[1][0] = covMatrix11[1][0]; + measCovMatrix[1][1] = covMatrix11[1][1]; + measCovMatrix[1][2] = covMatrix11[1][2]; + measCovMatrix[2][0] = covMatrix11[2][0]; + measCovMatrix[2][1] = covMatrix11[2][1]; + measCovMatrix[2][2] = covMatrix11[2][2]; + + measVec[0] = normSunB[0]; + measVec[1] = normSunB[1]; + measVec[2] = normSunB[2]; + + measEstVec[0] = sunEstB[0]; + measEstVec[1] = sunEstB[1]; + measEstVec[2] = sunEstB[2]; + + } else if (sensorsAvail == 6) { // only mag + + measSensMatrix[0][0] = measSensMatrix22[0][0]; + measSensMatrix[0][1] = measSensMatrix22[0][1]; + measSensMatrix[0][2] = measSensMatrix22[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix22[1][0]; + measSensMatrix[1][1] = measSensMatrix22[1][1]; + measSensMatrix[1][2] = measSensMatrix22[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix22[2][0]; + measSensMatrix[2][1] = measSensMatrix22[2][1]; + measSensMatrix[2][2] = measSensMatrix22[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix22[0][0]; + measCovMatrix[0][1] = covMatrix22[0][1]; + measCovMatrix[0][2] = covMatrix22[0][2]; + measCovMatrix[1][0] = covMatrix22[1][0]; + measCovMatrix[1][1] = covMatrix22[1][1]; + measCovMatrix[1][2] = covMatrix22[1][2]; + measCovMatrix[2][0] = covMatrix22[2][0]; + measCovMatrix[2][1] = covMatrix22[2][1]; + measCovMatrix[2][2] = covMatrix22[2][2]; + + measVec[0] = normMagB[0]; + measVec[1] = normMagB[1]; + measVec[2] = normMagB[2]; + + measEstVec[0] = magEstB[0]; + measEstVec[1] = magEstB[1]; + measEstVec[2] = magEstB[2]; + + } else if (sensorsAvail == 7) { // only str + + measSensMatrix[0][0] = measSensMatrix33[0][0]; + measSensMatrix[0][1] = measSensMatrix33[0][1]; + measSensMatrix[0][2] = measSensMatrix33[0][2]; + measSensMatrix[0][3] = 0; + measSensMatrix[0][4] = 0; + measSensMatrix[0][5] = 0; + measSensMatrix[1][0] = measSensMatrix33[1][0]; + measSensMatrix[1][1] = measSensMatrix33[1][1]; + measSensMatrix[1][2] = measSensMatrix33[1][2]; + measSensMatrix[1][3] = 0; + measSensMatrix[1][4] = 0; + measSensMatrix[1][5] = 0; + measSensMatrix[2][0] = measSensMatrix33[2][0]; + measSensMatrix[2][1] = measSensMatrix33[2][1]; + measSensMatrix[2][2] = measSensMatrix33[2][2]; + measSensMatrix[2][3] = 0; + measSensMatrix[2][4] = 0; + measSensMatrix[2][5] = 0; + + measCovMatrix[0][0] = covMatrix33[0][0]; + measCovMatrix[0][1] = covMatrix33[0][1]; + measCovMatrix[0][2] = covMatrix33[0][2]; + measCovMatrix[1][0] = covMatrix33[1][0]; + measCovMatrix[1][1] = covMatrix33[1][1]; + measCovMatrix[1][2] = covMatrix33[1][2]; + measCovMatrix[2][0] = covMatrix33[2][0]; + measCovMatrix[2][1] = covMatrix33[2][1]; + measCovMatrix[2][2] = covMatrix33[2][2]; + + measVec[0] = measVecQuat[0]; + measVec[1] = measVecQuat[1]; + measVec[2] = measVecQuat[2]; + + measEstVec[0] = quatEstErr[0]; + measEstVec[1] = quatEstErr[1]; + measEstVec[2] = quatEstErr[2]; + } + // Kalman Gain: [K = P * H' / (H * P * H' + R)] + double kalmanGain[6][MDF] = {{0}}, kalmanGain1[6][MDF] = {{0}}; + + double measSensMatrixTrans[6][MDF], residualCov[MDF][MDF], residualCov1[6][MDF]; + // H * P * H' + MatrixOperations::transpose(*measSensMatrix, *measSensMatrixTrans, 6, MDF); + MatrixOperations::multiply(*initialCovarianceMatrix, *measSensMatrixTrans, *residualCov1, + 6, 6, MDF); + MatrixOperations::multiply(*measSensMatrix, *residualCov1, *residualCov, MDF, 6, MDF); + // negative values, restrictions ? + // (H * P * H' + R) + MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); + // <> + double invResidualCov1[MDF] = {0}; + double invResidualCov[MDF][MDF] = {{0}}; + int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov, *invResidualCov, + invResidualCov1, MDF); + if (inversionFailed) { + validMekf = false; + return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed + } + + // [K = P * H' / (H * P * H' + R)] + MatrixOperations::multiply(*measSensMatrixTrans, *invResidualCov, *kalmanGain1, 6, MDF, + MDF); + MatrixOperations::multiply(*initialCovarianceMatrix, *kalmanGain1, *kalmanGain, 6, 6, + MDF); + + /* ------- UPDATE -STEP ---------*/ + + // Update Covariance Matrix: P_plus = (I-K*H)*P_min + double covMatPlus[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 identityMatrix[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 updateCov1[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}}; + MatrixOperations::multiply(*kalmanGain, *measSensMatrix, *updateCov1, 6, MDF, MDF); + MatrixOperations::subtract(*identityMatrix, *updateCov1, *updateCov1, 6, 6); + MatrixOperations::multiply(*updateCov1, *initialCovarianceMatrix, *covMatPlus, 6, 6, 6); + + // Error State Vector + double errStateVec[6] = {0, 0, 0, 0, 0, 0}; + double errStateVec1[MDF] = {0}; + VectorOperations::subtract(measVec, measEstVec, errStateVec1, MDF); + MatrixOperations::multiply(*kalmanGain, errStateVec1, errStateVec, 6, MDF, 1); + + double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]}; + double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]}; + + // State Vector Elements + double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, + xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MathOperations::skewMatrix(propagatedQuaternion, *xi2); + double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + MatrixOperations::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); + MatrixOperations::add(*xi1, *xi2, *xi1, 3, 3); + double xi[4][3] = { + {xi1[0][0], xi1[0][1], xi1[0][2]}, + {xi1[1][0], xi1[1][1], xi1[1][2]}, + {xi1[2][0], xi1[2][1], xi1[2][2]}, + {-propagatedQuaternion[0], -propagatedQuaternion[1], -propagatedQuaternion[2]}}; + + double errQuatTerm[4] = {0, 0, 0, 0}; + MatrixOperations::multiply(*xi, errStateQuat, errQuatTerm, 4, 3, 1); + VectorOperations::mulScalar(errQuatTerm, 0.5, errQuatTerm, 4); + VectorOperations::add(propagatedQuaternion, errQuatTerm, quatBJ, 4); + VectorOperations::normalize(quatBJ, quatBJ, 4); + + outputQuat[0] = quatBJ[0]; + outputQuat[1] = quatBJ[1]; + outputQuat[2] = quatBJ[2]; + outputQuat[3] = quatBJ[3]; + + double updatedGyroBias[3] = {0, 0, 0}; + VectorOperations::add(biasGYR, errStateGyroBias, updatedGyroBias, 3); + // Bias GYR State + biasGYR[0] = updatedGyroBias[0]; + biasGYR[1] = updatedGyroBias[1]; + 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 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}; + 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}}; + + // Corrected Sat Rate via Bias + outputSatRate[0] = rotRateEst[0]; + outputSatRate[1] = rotRateEst[1]; + outputSatRate[2] = rotRateEst[2]; + + // 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); + + 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]; + } 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 covQ21[3][3], covQ22[3][3], covQ23[3][3]; + double fact4 = + (0.5 * 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::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]; + } + + // 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}}, + 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 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); + + 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]; + + // Propagated Quaternion + double rotSin[3] = {0, 0, 0}, omega1[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); + + 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]}, + {-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]; + 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); + validMekf = true; + + // Discrete Time Step + + // Check for new data in measurement -> SensorProcessing ? + + return returnvalue::OK; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index 542db996..f1d2d7a0 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -7,101 +7,95 @@ * @brief: This class handles the calculation of an estimated quaternion and the gyro bias by * means of the spacecraft attitude sensors * - * @note: A description of the used algorithms can be found in the bachelor thesis of Robin Marquardt + * @note: A description of the used algorithms can be found in the bachelor thesis of Robin + * Marquardt * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811 */ #ifndef MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_ +#include //uint8_t +#include /*purpose, timeval ?*/ + #include "config/classIds.h" -#include //uint8_t -#include /*purpose, timeval ?*/ //#include <_timeval.h> #include "AcsParameters.h" -class MultiplicativeKalmanFilter{ -public: - /* @brief: Constructor - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - MultiplicativeKalmanFilter(AcsParameters *acsParameters_); - virtual ~MultiplicativeKalmanFilter(); +class MultiplicativeKalmanFilter { + public: + /* @brief: Constructor + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + MultiplicativeKalmanFilter(AcsParameters *acsParameters_); + virtual ~MultiplicativeKalmanFilter(); - void reset(); // NOT YET DEFINED - should only reset all mekf variables + void reset(); // NOT YET DEFINED - should only reset all mekf variables - /* @brief: init() - This function initializes the Kalman Filter and will provide the first quaternion through - * the QUEST algorithm - * @param: magneticField_ magnetic field vector in the body frame - * sunDir_ sun direction vector in the body frame - * sunDirJ sun direction vector in the ECI frame - * magFieldJ magnetic field vector in the ECI frame - */ - void init(const double *magneticField_, const bool *validMagField_, - const double *sunDir_, const bool *validSS, - const double *sunDirJ, const bool *validSSModel, - const double *magFieldJ,const bool *validMagModel); + /* @brief: init() - This function initializes the Kalman Filter and will provide the first + * quaternion through the QUEST algorithm + * @param: magneticField_ magnetic field vector in the body frame + * sunDir_ sun direction vector in the body frame + * sunDirJ sun direction vector in the ECI frame + * magFieldJ magnetic field vector in the ECI frame + */ + void init(const double *magneticField_, const bool *validMagField_, const double *sunDir_, + const bool *validSS, const double *sunDirJ, const bool *validSSModel, + const double *magFieldJ, const bool *validMagModel); - /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter for the current step - * after the initalization - * @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame - * rateRMUs_ Estimated satellite rotation rate from the Rate Measurement Units [rad/s] - * magneticField_ magnetic field vector in the body frame - * sunDir_ sun direction vector in the body frame - * sunDirJ sun direction vector in the ECI frame - * magFieldJ magnetic field vector in the ECI frame - * outputQuat Stores the calculated quaternion - * outputSatRate Stores the adjusted satellite rate - * @return ReturnValue_t Feedback of this class, KALMAN_NO_RMU_MEAS if no satellite rate from the sensors was provided, - * KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model calculations, - * KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible, - * RETURN_OK else - */ - ReturnValue_t mekfEst( - const double *quaternionSTR, const bool *validSTR_, - const double *rateRMUs_, const bool *validRMUs_, - 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 *outputQuat, double *outputSatRate); + /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter + * for the current step after the initalization + * @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame + * rateGYRs_ Estimated satellite rotation rate from the + * Gyroscopes [rad/s] magneticField_ magnetic field vector in the body frame sunDir_ + * sun direction vector in the body frame sunDirJ sun direction vector in the ECI + * frame magFieldJ magnetic field vector in the ECI frame + * outputQuat Stores the calculated quaternion + * outputSatRate Stores the adjusted satellite rate + * @return ReturnValue_t Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from + * the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model + * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible, + * RETURN_OK else + */ + ReturnValue_t 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 *outputQuat, double *outputSatRate); + // Declaration of Events (like init) and memberships + // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND + // (/config/returnvalues/classIDs.h) static const Event RESET = + // MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be + // resetting Mekf + static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN; + static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03); - // Declaration of Events (like init) and memberships - //static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND (/config/returnvalues/classIDs.h) - //static const Event RESET = MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be - // resetting Mekf - static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN; - static const ReturnValue_t KALMAN_NO_RMU_MEAS = MAKE_RETURN_CODE(0x01); - static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02); - static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03); + private: + /*Parameters*/ + AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters::KalmanFilterParameters *kalmanFilterParameters; + double quaternion_STR_SB[4]; + bool validInit; + double sampleTime = 0.1; -private: -/*Parameters*/ - AcsParameters::InertiaEIVE* inertiaEIVE; - AcsParameters::KalmanFilterParameters* kalmanFilterParameters; - double quaternion_STR_SB[4]; - bool validInit; - double sampleTime = 0.1; + /*States*/ + double initialQuaternion[4]; /*after reset?QUEST*/ + double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/ + double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ + bool validMekf; + uint8_t sensorsAvail; - -/*States*/ - double initialQuaternion[4]; /*after reset?QUEST*/ - double initialCovarianceMatrix[6][6];/*after reset?QUEST*/ - double propagatedQuaternion[4]; /*Filter Quaternion for next step*/ - bool validMekf; - uint8_t sensorsAvail; - -/*Outputs*/ - double quatBJ[4]; /* Output Quaternion */ - double biasRMU[3]; /*Between measured and estimated sat Rate*/ -/*Parameter INIT*/ - //double alpha, gamma, beta; -/*Functions*/ - void loadAcsParameters(AcsParameters *acsParameters_); + /*Outputs*/ + double quatBJ[4]; /* Output Quaternion */ + double biasGYR[3]; /*Between measured and estimated sat Rate*/ + /*Parameter INIT*/ + // double alpha, gamma, beta; + /*Functions*/ + void loadAcsParameters(AcsParameters *acsParameters_); }; - - #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */ diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index deb50aa1..2c1596e5 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -36,8 +36,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ, outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? - } - else { + } else { multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, @@ -45,7 +44,8 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *out kalmanInit = true; *mekfValid = 0; - // Maybe we need feedback from kalmanfilter to identify if there was a problem with the init - //of kalman filter where does this class know from that kalman filter was not initialized ? + // Maybe we need feedback from kalmanfilter to identify if there was a problem with the + // init of kalman filter where does this class know from that kalman filter was not + // initialized ? } } diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 6201a32a..7160fb47 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -123,7 +123,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl } // calculating momentum of satellite and momentum of reaction wheels - double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; VectorOperations::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4); MatrixOperations::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4, @@ -145,7 +145,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { - double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3}; + double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double wheelMomentum[4] = {0, 0, 0, 0}; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; // Conversion to [rad/s] for further calculations From 4bd2a2dac847ddde2037ebddf70a8c29a55552ff Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 24 Oct 2022 10:41:28 +0200 Subject: [PATCH 074/244] added and implemented all ACS related DataPools to AcsController --- mission/controller/AcsController.cpp | 311 ++++++++++++++---- mission/controller/AcsController.h | 128 ++++--- mission/controller/acs/OutputValues.cpp | 16 +- mission/controller/acs/OutputValues.h | 63 ++-- .../AcsCtrlDefinitions.h | 5 +- 5 files changed, 352 insertions(+), 171 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 22c871fd..b42d9c83 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -12,8 +12,15 @@ AcsController::AcsController(object_id_t objectId) detumble(&acsParameters), ptgCtrl(&acsParameters), detumbleCounter{0}, - mgmData(this), - susData(this) {} + mgmDataRaw(this), + mgmDataProcessed(this), + susDataRaw(this), + susDataProcessed(this), + gyrDataRaw(this), + gyrDataProcessed(this), + gpsDataProcessed(this), + mekfData(this), + actuatorCmdData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { return returnvalue::OK; @@ -38,11 +45,9 @@ void AcsController::performControlOperation() { case SUBMODE_SAFE: performSafe(); break; - case SUBMODE_DETUMBLE: performDetumble(); break; - case SUBMODE_PTG_GS: performPointingCtrl(); break; @@ -55,17 +60,23 @@ void AcsController::performControlOperation() { } { - PoolReadGuard pg(&mgmData); + PoolReadGuard pg(&mgmDataRaw); if (pg.getReadResult() == returnvalue::OK) { copyMgmData(); } } { - PoolReadGuard pg(&susData); + PoolReadGuard pg(&susDataRaw); if (pg.getReadResult() == returnvalue::OK) { copySusData(); } } + { + PoolReadGuard pg(&gyrDataRaw); + if (pg.getReadResult() == returnvalue::OK) { + copyGyrData(); + } + } // DEBUG : REMOVE AFTER COMPLETION mode = MODE_ON; @@ -81,10 +92,13 @@ void AcsController::performSafe() { ACS::SensorValues sensorValues; ACS::OutputValues outputValues; - timeval now; // We need to give the actual time here + timeval now; + Clock::getClock_timeval(&now); + sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); // DOES THIS WORK WITH VALID? + // Give desired satellite rate and sun direction to align double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); @@ -140,10 +154,12 @@ void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); + // Clock::getUptime(&now); sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); + double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(outputValues.magneticFieldVectorDerivative, &outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst, @@ -172,11 +188,13 @@ 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; navigation.useMekf(&sensorValues, &outputValues, &validMekf); + double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; @@ -204,38 +222,106 @@ void AcsController::performPointingCtrl() { ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - // MGM - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec); + // 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({mgmData.getSid(), false, 5.0}); - // SUS - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec); - localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec); - poolManager.subscribeForRegularPeriodicPacket({susData.getSid(), false, 5.0}); + 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_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); + 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}); + // 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) { - if (sid == mgmData.getSid()) { - return &mgmData; - } else if (sid == susData.getSid()) { - return &susData; + 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::ACTUATOR_CMD_DATA: + return &actuatorCmdData; + default: + return nullptr; } - return nullptr; } ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, @@ -261,112 +347,195 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {} void AcsController::announceMode(bool recursive) {} void AcsController::copyMgmData() { + ACS::SensorValues sensorValues; { - PoolReadGuard pg(&mgm0Lis3Set); + PoolReadGuard pg(&sensorValues.mgm0Lis3Set); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float)); + std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid()); } } { - PoolReadGuard pg(&mgm1Rm3100Set); + PoolReadGuard pg(&sensorValues.mgm1Rm3100Set); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); + std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid()); } } { - PoolReadGuard pg(&mgm2Lis3Set); + PoolReadGuard pg(&sensorValues.mgm2Lis3Set); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float)); + std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value, + 3 * sizeof(float)); + mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid()); } } { - PoolReadGuard pg(&mgm3Rm3100Set); + PoolReadGuard pg(&sensorValues.mgm3Rm3100Set); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float)); + std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value, + 3 + sizeof(float)); + mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid()); } } { - PoolReadGuard pg(&imtqMgmSet); + PoolReadGuard pg(&sensorValues.imtqMgmSet); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float)); - mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value; + 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(&susSets[0]); + PoolReadGuard pg(&sensorValues.susSets[0]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus0.value, susSets[0].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid()); } } { - PoolReadGuard pg(&susSets[1]); + PoolReadGuard pg(&sensorValues.susSets[1]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus1.value, susSets[1].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid()); } } { - PoolReadGuard pg(&susSets[2]); + PoolReadGuard pg(&sensorValues.susSets[2]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus2.value, susSets[2].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid()); } } { - PoolReadGuard pg(&susSets[3]); + PoolReadGuard pg(&sensorValues.susSets[3]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus3.value, susSets[3].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid()); } } { - PoolReadGuard pg(&susSets[4]); + PoolReadGuard pg(&sensorValues.susSets[4]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus4.value, susSets[4].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid()); } } { - PoolReadGuard pg(&susSets[5]); + PoolReadGuard pg(&sensorValues.susSets[5]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus5.value, susSets[5].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid()); } } { - PoolReadGuard pg(&susSets[6]); + PoolReadGuard pg(&sensorValues.susSets[6]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus6.value, susSets[6].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid()); } } { - PoolReadGuard pg(&susSets[7]); + PoolReadGuard pg(&sensorValues.susSets[7]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus7.value, susSets[7].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid()); } } { - PoolReadGuard pg(&susSets[8]); + PoolReadGuard pg(&sensorValues.susSets[8]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus8.value, susSets[8].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid()); } } { - PoolReadGuard pg(&susSets[9]); + PoolReadGuard pg(&sensorValues.susSets[9]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t)); - sif::debug << susData.sus9.isValid() << std::endl; - sif::debug << susSets[9].channels.isValid() << std::endl; + std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid()); } } { - PoolReadGuard pg(&susSets[10]); + PoolReadGuard pg(&sensorValues.susSets[10]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus10.value, susSets[10].channels.value, 6 * sizeof(uint16_t)); + std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value, + 6 * sizeof(uint16_t)); + susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid()); } } { - PoolReadGuard pg(&susSets[11]); + PoolReadGuard pg(&sensorValues.susSets[11]); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(susData.sus11.value, susSets[11].channels.value, 6 * sizeof(uint16_t)); + 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 a8ae9daa..87aaf37c 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -8,6 +8,7 @@ #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" #include "acs/Navigation.h" +#include "acs/OutputValues.h" #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" #include "acs/control/PtgCtrl.h" @@ -64,62 +65,91 @@ class AcsController : public ExtendedControllerBase { void modeChanged(Mode_t mode, Submode_t submode); void announceMode(bool recursive); + /* ACS Datasets */ // MGMs - acsctrl::MgmDataRaw mgmData; - - MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); - MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = - MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); - RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = - RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); - IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); - - PoolEntry mgm0PoolVec = PoolEntry(3); - PoolEntry mgm1PoolVec = PoolEntry(3); - PoolEntry mgm2PoolVec = PoolEntry(3); - PoolEntry mgm3PoolVec = PoolEntry(3); - PoolEntry imtqMgmPoolVec = PoolEntry(3); + acsctrl::MgmDataRaw mgmDataRaw; + PoolEntry mgm0VecRaw = PoolEntry(3); + PoolEntry mgm1VecRaw = PoolEntry(3); + PoolEntry mgm2VecRaw = PoolEntry(3); + PoolEntry mgm3VecRaw = PoolEntry(3); + PoolEntry imtqMgmVecRaw = PoolEntry(3); PoolEntry imtqCalActStatus = PoolEntry(); - void copyMgmData(); - // Sun Sensors - - acsctrl::SusDataRaw susData; - - std::array susSets{ - SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), - SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), - SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), - SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), - SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF), - SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB), - SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF), - SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB), - SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB), - SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF), - SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), - SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), - }; - - PoolEntry sus0PoolVec = PoolEntry(6); - PoolEntry sus1PoolVec = PoolEntry(6); - PoolEntry sus2PoolVec = PoolEntry(6); - PoolEntry sus3PoolVec = PoolEntry(6); - PoolEntry sus4PoolVec = PoolEntry(6); - PoolEntry sus5PoolVec = PoolEntry(6); - PoolEntry sus6PoolVec = PoolEntry(6); - PoolEntry sus7PoolVec = PoolEntry(6); - PoolEntry sus8PoolVec = PoolEntry(6); - PoolEntry sus9PoolVec = PoolEntry(6); - PoolEntry sus10PoolVec = PoolEntry(6); - PoolEntry sus11PoolVec = PoolEntry(6); + acsctrl::MgmDataProcessed mgmDataProcessed; + PoolEntry mgm0VecProc = PoolEntry(3); + PoolEntry mgm1VecProc = PoolEntry(3); + PoolEntry mgm2VecProc = PoolEntry(3); + PoolEntry mgm3VecProc = PoolEntry(3); + PoolEntry mgm4VecProc = PoolEntry(3); + PoolEntry mgmVecTot = PoolEntry(3); + PoolEntry mgmVecTotDer = PoolEntry(3); + PoolEntry magIgrf = PoolEntry(3); + // SUSs + acsctrl::SusDataRaw susDataRaw; + PoolEntry sus0ValRaw = PoolEntry(6); + PoolEntry sus1ValRaw = PoolEntry(6); + PoolEntry sus2ValRaw = PoolEntry(6); + PoolEntry sus3ValRaw = PoolEntry(6); + PoolEntry sus4ValRaw = PoolEntry(6); + PoolEntry sus5ValRaw = PoolEntry(6); + PoolEntry sus6ValRaw = PoolEntry(6); + PoolEntry sus7ValRaw = PoolEntry(6); + PoolEntry sus8ValRaw = PoolEntry(6); + PoolEntry sus9ValRaw = PoolEntry(6); + PoolEntry sus10ValRaw = PoolEntry(6); + PoolEntry sus11ValRaw = PoolEntry(6); void copySusData(); + acsctrl::SusDataProcessed susDataProcessed; + PoolEntry sus0VecProc = PoolEntry(3); + PoolEntry sus1VecProc = PoolEntry(3); + PoolEntry sus2VecProc = PoolEntry(3); + PoolEntry sus3VecProc = PoolEntry(3); + PoolEntry sus4VecProc = PoolEntry(3); + PoolEntry sus5VecProc = PoolEntry(3); + PoolEntry sus6VecProc = PoolEntry(3); + PoolEntry sus7VecProc = PoolEntry(3); + PoolEntry sus8VecProc = PoolEntry(3); + PoolEntry sus9VecProc = PoolEntry(3); + PoolEntry sus10VecProc = PoolEntry(3); + PoolEntry sus11VecProc = PoolEntry(3); + PoolEntry susVecTot = PoolEntry(3); + PoolEntry susVecTotDer = PoolEntry(3); + PoolEntry sunIjk = PoolEntry(3); + + // GYRs + acsctrl::GyrDataRaw gyrDataRaw; + PoolEntry gyr0VecRaw = PoolEntry(3); + PoolEntry gyr1VecRaw = PoolEntry(3); + PoolEntry gyr2VecRaw = PoolEntry(3); + PoolEntry gyr3VecRaw = PoolEntry(3); + void copyGyrData(); + + acsctrl::GyrDataProcessed gyrDataProcessed; + PoolEntry gyr0VecProc = PoolEntry(3); + PoolEntry gyr1VecProc = PoolEntry(3); + PoolEntry gyr2VecProc = PoolEntry(3); + PoolEntry gyr3VecProc = PoolEntry(3); + PoolEntry gyrVecTot = PoolEntry(3); + + // GPS + acsctrl::GpsDataProcessed gpsDataProcessed; + PoolEntry gcLatitude = PoolEntry(); + PoolEntry gdLongitude = PoolEntry(); + + // MEKF + acsctrl::MekfData mekfData; + PoolEntry quatMekf = PoolEntry(4); + PoolEntry satRotRateMekf = PoolEntry(3); + + // Actuator CMD + acsctrl::ActuatorCmdData actuatorCmdData; + PoolEntry rwTargetTorque = PoolEntry(4); + PoolEntry rwTargetSpeed = PoolEntry(4); + PoolEntry mtqTargetDipole = PoolEntry(3); + // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/controller/acs/OutputValues.cpp b/mission/controller/acs/OutputValues.cpp index d035f621..42730fb3 100644 --- a/mission/controller/acs/OutputValues.cpp +++ b/mission/controller/acs/OutputValues.cpp @@ -1,19 +1,9 @@ -/* - * OutputValues.cpp - * - * Created on: 30 Mar 2022 - * Author: rooob - */ #include "OutputValues.h" namespace ACS { -OutputValues::OutputValues(){ +OutputValues::OutputValues() {} -} +OutputValues::~OutputValues() {} -OutputValues::~OutputValues(){ - -} - -} +} // namespace ACS diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h index c9fc71f1..bd182f42 100644 --- a/mission/controller/acs/OutputValues.h +++ b/mission/controller/acs/OutputValues.h @@ -1,51 +1,44 @@ -/* - * OutputValues.h - * - * Created on: 30 Mar 2022 - * Author: rooob - */ +#include + +#include "../controllerdefinitions/AcsCtrlDefinitions.h" #ifndef OUTPUTVALUES_H_ #define OUTPUTVALUES_H_ - namespace ACS { class OutputValues { + public: + OutputValues(); + virtual ~OutputValues(); -public: + double magFieldEst[3]; // sensor fusion (G) // output value + bool magFieldEstValid; + double magFieldModel[3]; // igrf (IJK) // + bool magFieldModelValid; + double magneticFieldVectorDerivative[3]; + bool magneticFieldVectorDerivativeValid; - OutputValues(); - virtual ~OutputValues(); + bool mgmUpdated; // ToDo: ???? - double magFieldEst[3]; // sensor fusion (G) - bool magFieldEstValid; - double magFieldModel[3]; //igrf (IJK) - bool magFieldModelValid; - double magneticFieldVectorDerivative[3]; - bool magneticFieldVectorDerivativeValid; + double sunDirEst[3]; // sensor fusion (G) + bool sunDirEstValid; + double sunDirModel[3]; // sun model (IJK) + bool sunDirModelValid; - bool mgmUpdated; + double quatMekfBJ[4]; // mekf + bool quatMekfBJValid; - double sunDirEst[3]; // sensor fusion (G) - bool sunDirEstValid; - double sunDirModel[3]; //sun model (IJK) - bool sunDirModelValid; + double satRateEst[3]; + bool satRateEstValid; + double satRateMekf[3]; // after mekf with correction of bias + bool satRateMekfValid; + double sunVectorDerivative[3]; + bool sunVectorDerivativeValid; - double quatMekfBJ[4]; //mekf - bool quatMekfBJValid; - - double satRateEst[3]; - bool satRateEstValid; - double satRateMekf[3]; // after mekf with correction of bias - bool satRateMekfValid; - double sunVectorDerivative[3]; - bool sunVectorDerivativeValid; - - double gcLatitude; // geocentric latitude, radian - double gdLongitude; // Radian longitude + double gcLatitude; // geocentric latitude, radian + double gdLongitude; // Radian longitude }; -} - +} // namespace ACS #endif /*OUTPUTVALUES_H_*/ diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 4a608f71..bb0a40ae 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -15,7 +15,7 @@ enum SetIds : uint32_t { SUS_PROCESSED_DATA, GYR_SENSOR_DATA, GYR_PROCESSED_DATA, - GPS_SENSOR_DATA, + GPS_PROCESSED_DATA, MEKF_DATA, ACTUATOR_CMD_DATA }; @@ -214,8 +214,7 @@ class GyrDataProcessed : public StaticLocalDataSet { class GpsDataProcessed : public StaticLocalDataSet { public: - GpsDataProcessed(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, GPS_SET_PROCESSED_ENTRIES) {} + GpsDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GPS_PROCESSED_DATA) {} lp_var_t gcLatitude = lp_var_t(sid.objectId, GC_LATITUDE, this); lp_var_t gdLongitude = lp_var_t(sid.objectId, GD_LONGITUDE, this); From 69099881bd112c4938726d21227bc9743c56273b Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 25 Oct 2022 11:31:54 +0200 Subject: [PATCH 075/244] small stuff --- mission/controller/AcsController.cpp | 33 +++++++++++---------------- mission/controller/AcsController.h | 2 +- mission/controller/acs/OutputValues.h | 2 +- 3 files changed, 15 insertions(+), 22 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b42d9c83..e82cbb7d 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -97,7 +97,7 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &outputValues, &validMekf); // DOES THIS WORK WITH VALID? + 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}; @@ -127,25 +127,18 @@ void AcsController::performSafe() { if (outputValues.satRateMekfValid && VectorOperations::norm(outputValues.satRateMekf, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; - } - - else if (outputValues.satRateEstValid && - VectorOperations::norm(outputValues.satRateEst, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { + } else if (outputValues.satRateEstValid && + VectorOperations::norm(outputValues.satRateEst, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; - - } - - else { + } else { detumbleCounter = 0; } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { submode = SUBMODE_DETUMBLE; detumbleCounter = 0; } - - // commanding.magnetorquesDipol(); + // commanding.magnetorquesDipol(); } void AcsController::performDetumble() { @@ -154,7 +147,6 @@ void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); - // Clock::getUptime(&now); sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; @@ -170,14 +162,13 @@ void AcsController::performDetumble() { if (outputValues.satRateMekfValid && VectorOperations::norm(outputValues.satRateMekf, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; - } - - else if (outputValues.satRateEstValid && - VectorOperations::norm(outputValues.satRateEst, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { + } else if (outputValues.satRateEstValid && + VectorOperations::norm(outputValues.satRateEst, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; + } else { + detumbleCounter = 0; } - if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { submode = SUBMODE_SAFE; detumbleCounter = 0; @@ -235,6 +226,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD 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); @@ -539,3 +531,4 @@ void AcsController::copyGyrData() { } } } + diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 87aaf37c..dcd78621 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -82,7 +82,7 @@ class AcsController : public ExtendedControllerBase { PoolEntry mgm2VecProc = PoolEntry(3); PoolEntry mgm3VecProc = PoolEntry(3); PoolEntry mgm4VecProc = PoolEntry(3); - PoolEntry mgmVecTot = PoolEntry(3); + PoolEntry mgmVecTot = PoolEntry(3); PoolEntry mgmVecTotDer = PoolEntry(3); PoolEntry magIgrf = PoolEntry(3); diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h index bd182f42..bb1ed007 100644 --- a/mission/controller/acs/OutputValues.h +++ b/mission/controller/acs/OutputValues.h @@ -19,7 +19,7 @@ class OutputValues { double magneticFieldVectorDerivative[3]; bool magneticFieldVectorDerivativeValid; - bool mgmUpdated; // ToDo: ???? + bool mgmUpdated; // ToDo: relic from FLP. most likely not used double sunDirEst[3]; // sensor fusion (G) bool sunDirEstValid; From 44dda9455d9105e203dd896ff96e93097c8a26cb Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 26 Oct 2022 17:13:23 +0200 Subject: [PATCH 076/244] SensorProcessing writes to AcsController DataSets now --- mission/controller/AcsController.cpp | 9 +- mission/controller/acs/SensorProcessing.cpp | 595 ++++++++++++-------- mission/controller/acs/SensorProcessing.h | 31 +- 3 files changed, 380 insertions(+), 255 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index e82cbb7d..0cb4f330 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -95,7 +95,8 @@ void AcsController::performSafe() { timeval now; Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); @@ -148,7 +149,8 @@ void AcsController::performDetumble() { timeval now; Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); @@ -182,7 +184,8 @@ void AcsController::performPointingCtrl() { timeval now; Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, + &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 8c89dda4..1baa4007 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,5 +1,6 @@ #include "SensorProcessing.h" +#include #include #include #include @@ -13,27 +14,35 @@ using namespace Math; -SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} { - validMagField = false; - validGcLatitude = false; -} +SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) + : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {} SensorProcessing::~SensorProcessing() {} -bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, +void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, const AcsParameters::MgmHandlingParameters *mgmParameters, - const double gpsLatitude, const double gpsLongitude, - const double gpsAltitude, bool gpsValid, double *magFieldEst, - bool *outputValid, double *magFieldModel, - bool *magFieldModelValid, double *magneticFieldVectorDerivative, - bool *magneticFieldVectorDerivativeValid) { + acsctrl::GpsDataProcessed *gpsDataProcessed, + const double gpsAltitude, bool gpsValid, + acsctrl::MgmDataProcessed *mgmDataProcessed) { if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) { - *outputValid = false; - validMagField = false; - return false; + { + 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)); + mgmDataProcessed->setValidity(false, true); + } + } + return; } float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0}, mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0}, @@ -104,34 +113,26 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; } } + float mgmVecTot[3] = {0.0, 0.0, 0.0}; for (uint8_t i = 0; i < 3; i++) { - magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; + mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } - validMagField = true; - //-----------------------Mag Rate Computation --------------------------------------------------- + //-----------------------Mgm Rate Computation --------------------------------------------------- + float mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; + bool mgmVecTotDerivativeValid = false; double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); - for (uint8_t i = 0; i < 3; i++) { - magneticFieldVectorDerivative[i] = (magFieldEst[i] - savedMagFieldEst[i]) / timeDiff; - savedMagFieldEst[i] = magFieldEst[i]; + if (timeOfSavedMagFieldEst.tv_sec != 0) { + for (uint8_t i = 0; i < 3; i++) { + mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; + savedMgmVecTot[i] = mgmVecTot[i]; + } } - - *magneticFieldVectorDerivativeValid = true; - if (timeOfSavedMagFieldEst.tv_sec == 0) { - magneticFieldVectorDerivative[0] = 0; - magneticFieldVectorDerivative[1] = 0; - magneticFieldVectorDerivative[2] = 0; - *magneticFieldVectorDerivativeValid = false; - } - timeOfSavedMagFieldEst = timeOfMgmMeasurement; - *outputValid = true; - // ---------------- IGRF- 13 Implementation here ------------------------------------------------ - if (!gpsValid) { - *magFieldModelValid = false; - } else { + 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 @@ -139,12 +140,32 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const 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(gpsLongitude, gpsLatitude, gpsAltitude, timeOfMgmMeasurement, - magFieldModel); - *magFieldModelValid = false; + igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, + gpsAltitude, timeOfMgmMeasurement, magIgrfModel); + } + { + PoolReadGuard pg(mgmDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm0vec.setValid(mgm0valid); + std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm1vec.setValid(mgm1valid); + std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm2vec.setValid(mgm2valid); + std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm3vec.setValid(mgm3valid); + std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float)); + mgmDataProcessed->mgm4vec.setValid(mgm4valid); + std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(float)); + mgmDataProcessed->mgmVecTot.setValid(true); + std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative, + 3 * sizeof(float)); + mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid); + std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); + mgmDataProcessed->magIgrfModel.setValid(gpsValid); + mgmDataProcessed->setValidity(true, false); + } } - - return true; } void SensorProcessing::processSus( @@ -155,9 +176,8 @@ void SensorProcessing::processSus( const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid, timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, - const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, - bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid) { + const AcsParameters::SunModelParameters *sunModelParameters, + acsctrl::SusDataProcessed *susDataProcessed) { if (sus0valid) { sus0valid = susConverter.checkSunSensorData(sus0Value); } @@ -197,142 +217,176 @@ void SensorProcessing::processSus( if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { - *sunDirEstValid = false; - return; - } else { - // WARNING: NOT TRANSFORMED IN BODY FRAME YET - // Transformation into Geomtry Frame - float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, - sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, - sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, - sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - - if (sus0valid) { - MatrixOperations::multiply( - susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, - susParameters->sus0coeffBeta), - sus0VecBody, 3, 3, 1); - } - if (sus1valid) { - MatrixOperations::multiply( - susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, - susParameters->sus1coeffBeta), - sus1VecBody, 3, 3, 1); - } - if (sus2valid) { - MatrixOperations::multiply( - susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, - susParameters->sus2coeffBeta), - sus2VecBody, 3, 3, 1); - } - if (sus3valid) { - MatrixOperations::multiply( - susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, - susParameters->sus3coeffBeta), - sus3VecBody, 3, 3, 1); - } - if (sus4valid) { - MatrixOperations::multiply( - susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, - susParameters->sus4coeffBeta), - sus4VecBody, 3, 3, 1); - } - if (sus5valid) { - MatrixOperations::multiply( - susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, - susParameters->sus5coeffBeta), - sus5VecBody, 3, 3, 1); - } - if (sus6valid) { - MatrixOperations::multiply( - susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, - susParameters->sus6coeffBeta), - sus6VecBody, 3, 3, 1); - } - if (sus7valid) { - MatrixOperations::multiply( - susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, - susParameters->sus7coeffBeta), - sus7VecBody, 3, 3, 1); - } - if (sus8valid) { - MatrixOperations::multiply( - susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, - susParameters->sus8coeffBeta), - sus8VecBody, 3, 3, 1); - } - if (sus9valid) { - MatrixOperations::multiply( - susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, - susParameters->sus9coeffBeta), - sus9VecBody, 3, 3, 1); - } - if (sus10valid) { - MatrixOperations::multiply( - susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, - susParameters->sus10coeffBeta), - sus10VecBody, 3, 3, 1); - } - if (sus11valid) { - MatrixOperations::multiply( - susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, - susParameters->sus11coeffBeta), - sus11VecBody, 3, 3, 1); - } - - /* ------ Mean Value: susDirEst ------ */ - bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, - sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; - float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], - sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], - sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, - {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1], - sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1], - sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]}, - {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2], - sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], - sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; - - double susMeanValue[3] = {0, 0, 0}; - for (uint8_t i = 0; i < 12; i++) { - if (validIds[i]) { - susMeanValue[0] += susVecBody[0][i]; - susMeanValue[1] += susVecBody[1][i]; - susMeanValue[2] += susVecBody[2][i]; + { + 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)); + susDataProcessed->setValidity(false, true); } } - VectorOperations::normalize(susMeanValue, sunDirEst, 3); - *sunDirEstValid = true; + return; } + // WARNING: NOT TRANSFORMED IN BODY FRAME YET + // Transformation into Geomtry Frame + float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0}, + sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0}, + sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, + sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; + + if (sus0valid) { + MatrixOperations::multiply( + susParameters->sus0orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, + 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], + susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, + 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], + susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, + susParameters->sus2coeffBeta), + sus2VecBody, 3, 3, 1); + } + if (sus3valid) { + MatrixOperations::multiply( + susParameters->sus3orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, + susParameters->sus3coeffBeta), + sus3VecBody, 3, 3, 1); + } + if (sus4valid) { + MatrixOperations::multiply( + susParameters->sus4orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, + susParameters->sus4coeffBeta), + sus4VecBody, 3, 3, 1); + } + if (sus5valid) { + MatrixOperations::multiply( + susParameters->sus5orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, + susParameters->sus5coeffBeta), + sus5VecBody, 3, 3, 1); + } + if (sus6valid) { + MatrixOperations::multiply( + susParameters->sus6orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, + susParameters->sus6coeffBeta), + sus6VecBody, 3, 3, 1); + } + if (sus7valid) { + MatrixOperations::multiply( + susParameters->sus7orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, + susParameters->sus7coeffBeta), + sus7VecBody, 3, 3, 1); + } + if (sus8valid) { + MatrixOperations::multiply( + susParameters->sus8orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, + susParameters->sus8coeffBeta), + sus8VecBody, 3, 3, 1); + } + if (sus9valid) { + MatrixOperations::multiply( + susParameters->sus9orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, + susParameters->sus9coeffBeta), + sus9VecBody, 3, 3, 1); + } + if (sus10valid) { + MatrixOperations::multiply( + susParameters->sus10orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, + susParameters->sus10coeffBeta), + sus10VecBody, 3, 3, 1); + } + if (sus11valid) { + MatrixOperations::multiply( + susParameters->sus11orientationMatrix[0], + susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, + susParameters->sus11coeffBeta), + sus11VecBody, 3, 3, 1); + } + + /* ------ Mean Value: susDirEst ------ */ + bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; + float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], + sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], + sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, + {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1], + sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1], + sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]}, + {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2], + sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], + sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; + + float susMeanValue[3] = {0, 0, 0}; + for (uint8_t i = 0; i < 12; i++) { + if (validIds[i]) { + susMeanValue[0] += susVecBody[0][i]; + susMeanValue[1] += susVecBody[1][i]; + susMeanValue[2] += susVecBody[2][i]; + } + } + float susVecTot[3] = {0.0, 0.0, 0.0}; + VectorOperations::normalize(susMeanValue, susVecTot, 3); /* -------- Sun Derivatiative --------------------- */ + float susVecTotDerivative[3] = {0.0, 0.0, 0.0}; + bool susVecTotDerivativeValid = false; double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); - for (uint8_t i = 0; i < 3; i++) { - sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff; - savedSunVector[i] = sunDirEst[i]; + if (timeOfSavedSusDirEst.tv_sec != 0) { + for (uint8_t i = 0; i < 3; i++) { + susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; + savedSusVecTot[i] = susVecTot[i]; + } } - - *sunVectorDerivativeValid = true; - if (timeOfSavedSusDirEst.tv_sec == 0) { - sunVectorDerivative[0] = 0; - sunVectorDerivative[1] = 0; - sunVectorDerivative[2] = 0; - *sunVectorDerivativeValid = false; - } - timeOfSavedSusDirEst = timeOfSusMeasurement; /* -------- Sun Model Direction (IJK frame) ------- */ @@ -340,6 +394,7 @@ void SensorProcessing::processSus( double JD2000 = MathOperations::convertUnixToJD2000(timeOfSusMeasurement); // Julean Centuries + double sunIjkModel[3] = {0.0, 0.0, 0.0}; double JC2000 = JD2000 / 36525; double meanLongitude = @@ -351,11 +406,46 @@ void SensorProcessing::processSus( double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000; - sunVectorInertial[0] = cos(eclipticLongitude); - sunVectorInertial[1] = sin(eclipticLongitude) * cos(epsilon); - sunVectorInertial[2] = sin(eclipticLongitude) * sin(epsilon); - - *sunVectorInertialValid = true; + sunIjkModel[0] = cos(eclipticLongitude); + sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon); + sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon); + { + PoolReadGuard pg(susDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float)); + susDataProcessed->sus0vec.setValid(sus0valid); + std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float)); + susDataProcessed->sus1vec.setValid(sus1valid); + std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float)); + susDataProcessed->sus2vec.setValid(sus2valid); + std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float)); + susDataProcessed->sus3vec.setValid(sus3valid); + std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float)); + susDataProcessed->sus4vec.setValid(sus4valid); + std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float)); + susDataProcessed->sus5vec.setValid(sus5valid); + std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float)); + susDataProcessed->sus6vec.setValid(sus6valid); + std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float)); + susDataProcessed->sus7vec.setValid(sus7valid); + std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float)); + susDataProcessed->sus8vec.setValid(sus8valid); + std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float)); + susDataProcessed->sus9vec.setValid(sus9valid); + std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float)); + susDataProcessed->sus10vec.setValid(sus10valid); + std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float)); + susDataProcessed->sus11vec.setValid(sus11valid); + std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(float)); + susDataProcessed->susVecTot.setValid(true); + std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative, + 3 * sizeof(float)); + susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid); + std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); + susDataProcessed->sunIjkModel.setValid(true); + susDataProcessed->setValidity(true, false); + } + } } void SensorProcessing::processGyr( @@ -366,133 +456,166 @@ void SensorProcessing::processGyr( const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters, - double *satRatEst, bool *satRateEstValid) { - if (!gyr0axXvalid && !gyr0axYvalid && !gyr0axZvalid && !gyr1axXvalid && !gyr1axYvalid && - !gyr1axZvalid && !gyr2axXvalid && !gyr2axYvalid && !gyr2axZvalid && !gyr3axXvalid && - !gyr3axYvalid && !gyr3axZvalid) { - *satRateEstValid = false; + acsctrl::GyrDataProcessed *gyrDataProcessed) { + bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid); + bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid); + bool gyr2valid = (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid); + bool gyr3valid = (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid); + if (!gyr0valid && !gyr1valid && !gyr2valid && !gyr3valid) { + { + PoolReadGuard pg(gyrDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + 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)); + std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double)); + gyrDataProcessed->setValidity(false, true); + } + } return; } // Transforming Values to the Body Frame (actually it is the geometry frame atm) double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0}, gyr3ValueBody[3] = {0, 0, 0}; - bool validUnit[4] = {false, false, false, false}; - if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) { + if (gyr0valid) { const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); - validUnit[0] = true; } - if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) { + if (gyr1valid) { const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); - validUnit[1] = true; } - if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) { + if (gyr2valid) { const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); - validUnit[2] = true; } - if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) { + if (gyr3valid) { const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); - validUnit[3] = true; } /* -------- SatRateEst: Middle Value ------- */ // take ADIS measurements, if both avail // if just one ADIS measurement avail, perform sensor fusion - if (validUnit[0] && validUnit[2]) { + double gyrVecTot[3] = {0.0, 0.0, 0.0}; + if (gyr0valid && gyr2valid) { double gyr02ValuesSum[3]; VectorOperations::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); - VectorOperations::mulScalar(gyr02ValuesSum, .5, satRatEst, 3); - } else if ((validUnit[0] || validUnit[2]) && !(validUnit[1] || validUnit[3])) { - if (validUnit[0]) { - satRatEst = gyr0ValueBody; - } else if (validUnit[2]) { - satRatEst = gyr2ValueBody; + VectorOperations::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); + } else if ((gyr0valid || gyr2valid) && !(gyr1valid || gyr3valid)) { + if (gyr0valid) { + std::memcpy(gyrVecTot, gyr0ValueBody, 3 * sizeof(double)); + } else if (gyr2valid) { + std::memcpy(gyrVecTot, gyr2ValueBody, 3 * sizeof(double)); } - } else if ((validUnit[1]) && (validUnit[3])) { + } else if (gyr1valid && gyr3valid) { double gyr13ValuesSum[3]; double gyr13ValuesMean[3]; VectorOperations::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); VectorOperations::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); - if (validUnit[0]) { - satRatEst[0] = + if (gyr0valid) { + gyrVecTot[0] = ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); - satRatEst[1] = + gyrVecTot[1] = ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); - satRatEst[2] = + gyrVecTot[2] = ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); - } else if (validUnit[2]) { - satRatEst[0] = + } else if (gyr2valid) { + gyrVecTot[0] = ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) + (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); - satRatEst[1] = + gyrVecTot[1] = ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) + (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); - satRatEst[2] = + gyrVecTot[2] = ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) + (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); - } else - satRatEst = gyr13ValuesMean; - } else if (validUnit[1]) { - satRatEst = gyr1ValueBody; - } else if (validUnit[3]) { - satRatEst = gyr3ValueBody; + } else { + std::memcpy(gyrVecTot, gyr13ValuesMean, 3 * sizeof(double)); + } + } else if (gyr1valid) { + std::memcpy(gyrVecTot, gyr1ValueBody, 3 * sizeof(double)); + } else if (gyr3valid) { + std::memcpy(gyrVecTot, gyr3ValueBody, 3 * sizeof(double)); + } + { + PoolReadGuard pg(gyrDataProcessed); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double)); + gyrDataProcessed->gyr0vec.setValid(gyr0valid); + std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double)); + std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double)); + gyrDataProcessed->setValidity(true, false); + } } - *satRateEstValid = true; } void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude, - const bool validGps, double *gcLatitude, double *gdLongitude) { + 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; + gdLongitude = gps0longitude * PI / 180; double latitudeRad = gps0latitude * PI / 180; double eccentricityWgs84 = 0.0818195; double factor = 1 - pow(eccentricityWgs84, 2); - *gcLatitude = atan(factor * tan(latitudeRad)); - validGcLatitude = true; + gcLatitude = atan(factor * tan(latitudeRad)); + // validGcLatitude = true; + } + { + PoolReadGuard pg(gpsDataProcessed); + 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; + } + } } } void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, - ACS::OutputValues *outputValues, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters) { sensorValues->update(); processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, - sensorValues->gpsSet.isValid(), &outputValues->gcLatitude, &outputValues->gdLongitude); + sensorValues->gpsSet.isValid(), gpsDataProcessed); - outputValues->mgmUpdated = processMgm( - sensorValues->mgm0Lis3Set.fieldStrengths.value, - sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), - sensorValues->mgm1Rm3100Set.fieldStrengths.value, - sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(), - sensorValues->mgm2Lis3Set.fieldStrengths.value, - sensorValues->mgm2Lis3Set.fieldStrengths.isValid(), - sensorValues->mgm3Rm3100Set.fieldStrengths.value, - sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, - sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters, - outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value, - sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid, - outputValues->magneticFieldVectorDerivative, - &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? + processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, + sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), + sensorValues->mgm1Rm3100Set.fieldStrengths.value, + sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(), + sensorValues->mgm2Lis3Set.fieldStrengths.value, + sensorValues->mgm2Lis3Set.fieldStrengths.isValid(), + sensorValues->mgm3Rm3100Set.fieldStrengths.value, + sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), + sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), + now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, + sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.isValid(), mgmDataProcessed); processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), @@ -507,10 +630,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(), sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(), now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, - outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel, - &outputValues->sunDirModelValid, outputValues->sunVectorDerivative, - &outputValues->sunVectorDerivativeValid); - // VALID outputs ? + susDataProcessed); processGyr( sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), @@ -525,6 +645,5 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(), sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(), sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, - &acsParameters->gyrHandlingParameters, outputValues->satRateEst, - &outputValues->satRateEstValid); + &acsParameters->gyrHandlingParameters, gyrDataProcessed); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 50b78238..11d2ebeb 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -23,19 +23,21 @@ class SensorProcessing { SensorProcessing(AcsParameters *acsParameters_); virtual ~SensorProcessing(); - void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + void process(timeval now, ACS::SensorValues *sensorValues, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters); // Will call protected functions private: protected: // short description needed for every function - bool processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, + void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement, const AcsParameters::MgmHandlingParameters *mgmParameters, - const double gpsLatitude, const double gpsLongitude, const double gpsAltitude, - bool gpsValid, double *magFieldEst, bool *outputValid, double *magFieldModel, - bool *magFieldModelValid, double *magneticFieldVectorDerivative, - bool *magneticFieldVectorDerivativeValid); // Output + acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude, + bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed); void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid, const uint16_t *sus2Value, bool sus2valid, @@ -47,9 +49,8 @@ class SensorProcessing { bool sus10valid, const uint16_t *sus11Value, bool sus11valid, timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, - const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, - bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid); + const AcsParameters::SunModelParameters *sunModelParameters, + acsctrl::SusDataProcessed *susDataProcessed); void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid, @@ -60,21 +61,23 @@ class SensorProcessing { const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, timeval timeOfGyrMeasurement, - const AcsParameters::GyrHandlingParameters *gyrParameters, double *satRatEst, - bool *satRateEstValid); + const AcsParameters::GyrHandlingParameters *gyrParameters, + acsctrl::GyrDataProcessed *gyrDataProcessed); void processStr(); void processGps(const double gps0latitude, const double gps0longitude, const bool validGps, - double *gcLatitude, double *gdLongitude); + acsctrl::GpsDataProcessed *gpsDataProcessed); - double savedMagFieldEst[3]; + double savedMgmVecTot[3]; timeval timeOfSavedMagFieldEst; - double savedSunVector[3]; + double savedSusVecTot[3]; timeval timeOfSavedSusDirEst; bool validMagField; bool validGcLatitude; + const float zeroVector[3] = {0.0, 0.0, 0.0}; + SusConverter susConverter; AcsParameters acsParameters; }; From a13ccb43d23df5bbfb5b18162f24a8ddd5a36092 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 3 Nov 2022 10:43:27 +0100 Subject: [PATCH 077/244] removed OutputValues amended sumbode list inserted writes to output DataPools --- mission/controller/AcsController.cpp | 166 +++++++++++---- mission/controller/AcsController.h | 18 +- mission/controller/acs/ActuatorCmd.cpp | 50 +++-- mission/controller/acs/ActuatorCmd.h | 56 +++-- mission/controller/acs/CMakeLists.txt | 1 - mission/controller/acs/Guidance.cpp | 40 ++-- mission/controller/acs/Guidance.h | 42 ++-- mission/controller/acs/Igrf13Model.cpp | 194 +++++++++--------- mission/controller/acs/Igrf13Model.h | 170 +++++++-------- .../acs/MultiplicativeKalmanFilter.cpp | 86 +++++--- .../acs/MultiplicativeKalmanFilter.h | 21 +- mission/controller/acs/Navigation.cpp | 34 +-- mission/controller/acs/Navigation.h | 32 +-- mission/controller/acs/OutputValues.cpp | 9 - mission/controller/acs/OutputValues.h | 44 ---- mission/controller/acs/SensorProcessing.h | 1 - mission/controller/acs/SensorValues.cpp | 1 - mission/controller/acs/control/Detumble.cpp | 45 ++-- mission/controller/acs/control/Detumble.h | 10 +- mission/controller/acs/control/PtgCtrl.cpp | 2 +- mission/controller/acs/control/PtgCtrl.h | 3 +- mission/controller/acs/control/SafeCtrl.cpp | 41 ++-- mission/controller/acs/control/SafeCtrl.h | 17 +- .../AcsCtrlDefinitions.h | 29 ++- 24 files changed, 590 insertions(+), 522 deletions(-) delete mode 100644 mission/controller/acs/OutputValues.cpp delete mode 100644 mission/controller/acs/OutputValues.h diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0cb4f330..953a91a1 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -20,6 +20,7 @@ AcsController::AcsController(object_id_t objectId) gyrDataProcessed(this), gpsDataProcessed(this), mekfData(this), + ctrlValData(this), actuatorCmdData(this) {} ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { @@ -48,7 +49,9 @@ void AcsController::performControlOperation() { case SUBMODE_DETUMBLE: performDetumble(); break; - case SUBMODE_PTG_GS: + case SUBMODE_PTG_TARGET: + case SUBMODE_PTG_NADIR: + case SUBMODE_PTG_INERTIAL: performPointingCtrl(); break; } @@ -80,7 +83,7 @@ void AcsController::performControlOperation() { // DEBUG : REMOVE AFTER COMPLETION mode = MODE_ON; - submode = SUBMODE_DETUMBLE; + submode = SUBMODE_SAFE; // DEBUG END } @@ -90,7 +93,6 @@ void AcsController::performSafe() { // another place since we have to do it for every mode regardless of safe or not ACS::SensorValues sensorValues; - ACS::OutputValues outputValues; timeval now; Clock::getClock_timeval(&now); @@ -98,38 +100,54 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &outputValues, &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}; + double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; if (validMekf == returnvalue::OK) { - safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid), - (outputValues.magFieldModel), &(outputValues.magFieldModelValid), - (outputValues.sunDirModel), &(outputValues.sunDirModelValid), - (outputValues.satRateMekf), &(outputValues.satRateMekfValid), sunTargetDir, - satRateSafe, magMomMtq, &magMomMtqValid); + 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, outputValues.sunDirEst, &outputValues.sunDirEstValid, - outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid), - outputValues.magFieldEst, &(outputValues.magFieldEstValid), - outputValues.magneticFieldVectorDerivative, - &(outputValues.magneticFieldVectorDerivativeValid), sunTargetDir, - satRateSafe, magMomMtq, &magMomMtqValid); + 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 (outputValues.satRateMekfValid && VectorOperations::norm(outputValues.satRateMekf, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; - } else if (outputValues.satRateEstValid && - VectorOperations::norm(outputValues.satRateEst, 3) > + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else { @@ -139,12 +157,31 @@ void AcsController::performSafe() { submode = SUBMODE_DETUMBLE; detumbleCounter = 0; } - // commanding.magnetorquesDipol(); + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + double zeroVec[3] = {0, 0, 0, 0}; + std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], + //torqueDuration); + // } } void AcsController::performDetumble() { ACS::SensorValues sensorValues; - ACS::OutputValues outputValues; timeval now; Clock::getClock_timeval(&now); @@ -152,20 +189,22 @@ void AcsController::performDetumble() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &outputValues, &validMekf); + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); double magMomMtq[3] = {0, 0, 0}; - detumble.bDotLaw(outputValues.magneticFieldVectorDerivative, - &outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst, - &outputValues.magFieldEstValid, magMomMtq); + 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 (outputValues.satRateMekfValid && VectorOperations::norm(outputValues.satRateMekf, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { + if (mekfData.satRotRateMekf.isValid() && + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; - } else if (outputValues.satRateEstValid && - VectorOperations::norm(outputValues.satRateEst, 3) < + } else if (gyrDataProcessed.gyrVecTot.isValid() && + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } else { @@ -175,11 +214,31 @@ void AcsController::performDetumble() { submode = SUBMODE_SAFE; detumbleCounter = 0; } + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + double zeroVec[3] = {0, 0, 0, 0}; + std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); + actuatorCmdData.rwTargetTorque.setValid(false); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); + actuatorCmdData.rwTargetSpeed.setValid(false); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double)); + actuatorCmdData.mtqTargetDipole.setValid(true); + actuatorCmdData.setValidity(true, false); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], + // torqueDuration); + // } } void AcsController::performPointingCtrl() { ACS::SensorValues sensorValues; - ACS::OutputValues outputValues; timeval now; Clock::getClock_timeval(&now); @@ -187,12 +246,14 @@ void AcsController::performPointingCtrl() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); ReturnValue_t validMekf; - navigation.useMekf(&sensorValues, &outputValues, &validMekf); + navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, + &mekfData, &validMekf); double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; - guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate); - double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0}; - guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate); + guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate); + 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; @@ -207,11 +268,29 @@ void AcsController::performPointingCtrl() { &(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); + 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); + + { + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolUnits, 3 * sizeof(double)); + actuatorCmdData.setValidity(true, true); + } + } + // { + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], + // torqueDuration); + // } } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, @@ -286,6 +365,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD 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); @@ -312,6 +396,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { return &gpsDataProcessed; case acsctrl::MEKF_DATA: return &mekfData; + case acsctrl::CTRL_VAL_DATA: + return &ctrlValData; case acsctrl::ACTUATOR_CMD_DATA: return &actuatorCmdData; default: @@ -328,7 +414,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, return INVALID_SUBMODE; } } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) { - if ((submode > 5) || (submode < 2)) { + if ((submode > 6) || (submode < 2)) { return INVALID_SUBMODE; } else { return returnvalue::OK; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index dcd78621..18c8d824 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -8,7 +8,6 @@ #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" #include "acs/Navigation.h" -#include "acs/OutputValues.h" #include "acs/SensorProcessing.h" #include "acs/control/Detumble.h" #include "acs/control/PtgCtrl.h" @@ -27,8 +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_GS = 4; + static const Submode_t SUBMODE_PTG_TARGET = 4; static const Submode_t SUBMODE_PTG_NADIR = 5; + static const Submode_t SUBMODE_PTG_INERTIAL = 6; protected: void performSafe(); @@ -83,7 +83,7 @@ class AcsController : public ExtendedControllerBase { PoolEntry mgm3VecProc = PoolEntry(3); PoolEntry mgm4VecProc = PoolEntry(3); PoolEntry mgmVecTot = PoolEntry(3); - PoolEntry mgmVecTotDer = PoolEntry(3); + PoolEntry mgmVecTotDer = PoolEntry(3); PoolEntry magIgrf = PoolEntry(3); // SUSs @@ -115,9 +115,9 @@ class AcsController : public ExtendedControllerBase { PoolEntry sus9VecProc = PoolEntry(3); PoolEntry sus10VecProc = PoolEntry(3); PoolEntry sus11VecProc = PoolEntry(3); - PoolEntry susVecTot = PoolEntry(3); - PoolEntry susVecTotDer = PoolEntry(3); - PoolEntry sunIjk = PoolEntry(3); + PoolEntry susVecTot = PoolEntry(3); + PoolEntry susVecTotDer = PoolEntry(3); + PoolEntry sunIjk = PoolEntry(3); // GYRs acsctrl::GyrDataRaw gyrDataRaw; @@ -144,6 +144,12 @@ class AcsController : public ExtendedControllerBase { PoolEntry quatMekf = PoolEntry(4); PoolEntry satRotRateMekf = PoolEntry(3); + // Ctrl Values + acsctrl::CtrlValData ctrlValData; + PoolEntry tgtQuat = PoolEntry(4); + PoolEntry errQuat = PoolEntry(4); + PoolEntry errAng = PoolEntry(); + // Actuator CMD acsctrl::ActuatorCmdData actuatorCmdData; PoolEntry rwTargetTorque = PoolEntry(4); diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 0dba2016..2a0a9425 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -5,23 +5,21 @@ * Author: Robin Marquardt */ - #include "ActuatorCmd.h" -#include "util/MathOperations.h" -#include "util/CholeskyDecomposition.h" -#include + #include -#include #include #include +#include -ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { - acsParameters = *acsParameters_; -} +#include -ActuatorCmd::~ActuatorCmd(){ +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" -} +ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; } + +ActuatorCmd::~ActuatorCmd() {} void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, @@ -57,22 +55,20 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1 } void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) { + // Convert to Unit frame + MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, + dipolMoment, dipolMomentUnits, 3, 3, 1); + // Scaling along largest element if dipol exceeds maximum + double maxDipol = acsParameters.magnetorquesParameter.DipolMax; + double maxValue = 0; + for (int i = 0; i < 3; i++) { + if (abs(dipolMomentUnits[i]) > maxDipol) { + maxValue = abs(dipolMomentUnits[i]); + } + } -// Convert to Unit frame - MatrixOperations::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, dipolMoment, dipolMomentUnits, 3, 3, 1); -// Scaling along largest element if dipol exceeds maximum - double maxDipol = acsParameters.magnetorquesParameter.DipolMax; - double maxValue = 0; - for (int i = 0; i < 3; i++) { - if (abs(dipolMomentUnits[i]) > maxDipol) { - maxValue = abs(dipolMomentUnits[i]); - } - } - - if (maxValue > maxDipol) { - - double scalingFactor = maxDipol / maxValue; - VectorOperations::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3); - - } + if (maxValue > maxDipol) { + double scalingFactor = maxDipol / maxValue; + VectorOperations::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3); + } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 1da2c89a..5cb3ff00 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -8,42 +8,40 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ - #include "AcsParameters.h" -#include "SensorProcessing.h" #include "MultiplicativeKalmanFilter.h" +#include "SensorProcessing.h" #include "SensorValues.h" -#include "OutputValues.h" -class ActuatorCmd{ -public: - ActuatorCmd(AcsParameters *acsParameters_); //Input mode ? - virtual ~ActuatorCmd(); +class ActuatorCmd { + public: + ActuatorCmd(AcsParameters *acsParameters_); // Input mode ? + virtual ~ActuatorCmd(); - /* - * @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 *rwTrqIn, - const double *rwTrqNS, 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 *rwTrqIn, const double *rwTrqNS, + 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: - AcsParameters acsParameters; + protected: + private: + AcsParameters acsParameters; }; #endif /* ACTUATORCMD_H_ */ diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index c949a067..a379197d 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -6,7 +6,6 @@ target_sources( Igrf13Model.cpp MultiplicativeKalmanFilter.cpp Navigation.cpp - OutputValues.cpp SensorProcessing.cpp SensorValues.cpp SusConverter.cpp) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index f6ebee5f..35a7295a 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -7,6 +7,7 @@ #include "Guidance.h" +#include #include #include #include @@ -29,8 +30,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 // memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24); } -void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, - timeval now, double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, timeval now, + double targetQuat[4], double refSatRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to groundstation //------------------------------------------------------------------------------------- @@ -77,10 +79,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues 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); @@ -137,23 +137,16 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues double sunDirJ[3] = {0, 0, 0}; double 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)); } double exclAngle = acsParameters.strParameters.exclusionAngle, blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop; - double sightAngleSun = VectorOperations::dot(acsParameters.strParameters.boresightAxis, sunDirB); @@ -190,8 +183,8 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues } } -void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, - double refSatRate[3], double quatError[3], double deltaRate[3]) { +void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, 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]; @@ -199,9 +192,7 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3]; double satRate[3] = {0, 0, 0}; - satRate[0] = outputValues->satRateMekf[0]; - satRate[1] = outputValues->satRateMekf[1]; - satRate[2] = outputValues->satRateMekf[2]; + std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); VectorOperations::subtract(satRate, refSatRate, deltaRate, 3); // valid checks ? double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]}, @@ -209,7 +200,6 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, {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) { @@ -225,8 +215,8 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, } void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { - if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() && - sensorValues->rw4Set.isValid()) { + if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && + sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) { 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..4c699561 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -8,35 +8,37 @@ #ifndef GUIDANCE_H_ #define GUIDANCE_H_ - -#include "AcsParameters.h" -#include "SensorValues.h" -#include "OutputValues.h" #include +#include "../controllerdefinitions/AcsCtrlDefinitions.h" +#include "AcsParameters.h" +#include "SensorValues.h" class Guidance { -public: - Guidance(AcsParameters *acsParameters_); - virtual ~Guidance(); + public: + Guidance(AcsParameters *acsParameters_); + virtual ~Guidance(); - void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); + 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 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 from gps position and position + // of the ground station + void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData, + acsctrl::SusDataProcessed *susDataProcessed, 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] ); + // @note: compares target Quaternion and reference quaternion, also actual satellite rate and + // desired + void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], + double quatErrorComplete[4], double quatError[3], double deltaRate[3]); -// @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); + // @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); - -private: - AcsParameters acsParameters; - bool strBlindAvoidFlag = false; + private: + AcsParameters acsParameters; + bool strBlindAvoidFlag = false; }; #endif /* ACS_GUIDANCE_H_ */ diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index 75d3c9a1..fcd95b68 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -6,120 +6,120 @@ */ #include "Igrf13Model.h" -#include "util/MathOperations.h" -#include -#include -#include -//#include + #include #include #include #include +#include +#include +#include +#include "util/MathOperations.h" -Igrf13Model::Igrf13Model(){ -} -Igrf13Model::~Igrf13Model(){ -} +Igrf13Model::Igrf13Model() {} +Igrf13Model::~Igrf13Model() {} void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, - const double altitude, timeval timeOfMagMeasurement, double* magFieldModelInertial) { + const double altitude, timeval timeOfMagMeasurement, + double* magFieldModelInertial) { + double phi = longitude, theta = gcLatitude; // geocentric + /* Here is the co-latitude needed*/ + theta -= 90 * Math::PI / 180; + theta *= (-1); - double phi = longitude, theta = gcLatitude; //geocentric - /* Here is the co-latitude needed*/ - theta -= 90*Math::PI/180; - theta *= (-1); + double rE = 6371200.0; // radius earth [m] + /* Predefine recursive associated Legendre polynomials */ + double P11 = 1; + double P10 = P11; // P10 = P(n-1,m-0) + double dP11 = 0; // derivative + double dP10 = dP11; // derivative - double rE = 6371200.0; // radius earth [m] - /* Predefine recursive associated Legendre polynomials */ - double P11 = 1; - double P10 = P11; //P10 = P(n-1,m-0) - double dP11 = 0; //derivative - double dP10 = dP11; //derivative + double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0; - double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0; + for (int m = 0; m <= igrfOrder; m++) { + for (int n = 1; n <= igrfOrder; n++) { + if (m <= n) { + /* Calculation of Legendre Polynoms (normalised) */ + if (n == m) { + P2 = sin(theta) * P11; + dP2 = sin(theta) * dP11 - cos(theta) * P11; + P11 = P2; + P10 = P11; + P20 = 0; + dP11 = dP2; + dP10 = dP11; + dP20 = 0; + } else if (n == 1) { + P2 = cos(theta) * P10; + dP2 = cos(theta) * dP10 - sin(theta) * P10; + P20 = P10; + P10 = P2; + dP20 = dP10; + dP10 = dP2; + } else { + K = (pow((n - 1), 2) - pow(m, 2)) / ((2 * n - 1) * (2 * n - 3)); + P2 = cos(theta) * P10 - K * P20; + dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20; + P20 = P10; + P10 = P2; + dP20 = dP10; + dP10 = dP2; + } + /* gradient of scalar potential towards radius */ + 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 */ + 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 */ + magFieldModel[2] += + pow(rE / (altitude + rE), (n + 2)) * + ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m); + } + } + } - for (int m = 0; m <= igrfOrder; m++) { - for (int n = 1; n <= igrfOrder; n++) { - if (m <= n) { - /* Calculation of Legendre Polynoms (normalised) */ - if (n == m) { - P2 = sin(theta) * P11; - dP2 = sin(theta) * dP11 - cos(theta) * P11; - P11 = P2; - P10 = P11; - P20 = 0; - dP11 = dP2; - dP10 = dP11; - dP20 = 0; - } else if (n == 1) { - P2 = cos(theta) * P10; - dP2 = cos(theta) * dP10 - sin(theta) * P10; - P20 = P10; - P10 = P2; - dP20 = dP10; - dP10 = dP2; - } else { - K = (pow((n - 1), 2) - pow(m, 2)) - / ((2 * n - 1) * (2 * n - 3)); - P2 = cos(theta) * P10 - K * P20; - dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20; - P20 = P10; - P10 = P2; - dP20 = dP10; - dP10 = dP2; - } - /* gradient of scalar potential towards radius */ - 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 */ - 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 */ - magFieldModel[2]+=pow(rE/(altitude+rE),(n+2))* - ((-updatedG[m][n-1]*sin(m*phi) + updatedH[m][n-1]*cos(m*phi))*P2*m); - } - } - } + magFieldModel[1] *= -1; + magFieldModel[2] *= (-1 / sin(theta)); - magFieldModel[1] *= -1; - magFieldModel[2] *= (-1 / sin(theta)); + /* Next step: transform into inertial KOS (IJK)*/ + // Julean Centuries + double JD2000Floor = 0; + double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + JD2000Floor = floor(JD2000); + double JC2000 = JD2000Floor / 36525; - /* Next step: transform into inertial KOS (IJK)*/ - //Julean Centuries - double JD2000Floor = 0; - double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); - JD2000Floor = floor(JD2000); - double JC2000 = JD2000Floor / 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 = 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 lst = gst + longitude; // local sidereal time [rad] - double lst = gst + longitude; //local sidereal time [rad] + 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(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); - - double normVecMagFieldInert[3] = {0,0,0}; - VectorOperations::normalize(magFieldModelInertial, normVecMagFieldInert, 3); + double normVecMagFieldInert[3] = {0, 0, 0}; + VectorOperations::normalize(magFieldModelInertial, normVecMagFieldInert, 3); } -void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement){ - - double JD2000Igrf = (2458850.0-2451545); //Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000 - double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); - double days = ceil(JD2000-JD2000Igrf); - for(int i = 0;i <= igrfOrder; i++){ - for(int j = 0;j <= (igrfOrder-1); j++){ - updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days/365); - updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days/365); - } - } +void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { + double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000 + double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + double days = ceil(JD2000 - JD2000Igrf); + for (int i = 0; i <= igrfOrder; i++) { + for (int j = 0; j <= (igrfOrder - 1); j++) { + updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days / 365); + updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days / 365); + } + } } diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index d89865ca..aa7e8b73 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -16,103 +16,107 @@ #ifndef IGRF13MODEL_H_ #define IGRF13MODEL_H_ -#include +#include #include #include #include -#include +#include // Output should be transformed to [T] instead of [nT] // Updating Coefficients has to be implemented yet. Question, updating everyday ? -class Igrf13Model/*:public HasParametersIF*/{ +class Igrf13Model /*:public HasParametersIF*/ { + public: + Igrf13Model(); + virtual ~Igrf13Model(); -public: - Igrf13Model(); - virtual ~Igrf13Model(); + // Main Function + void magFieldComp(const double longitude, const double gcLatitude, const double altitude, + timeval timeOfMagMeasurement, double* magFieldModelInertial); + // Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is + // oriented from the satellite to earth COM Difference up to 25 km, which is 5 % of the total + // flight altitude + /* Inputs: + * - longitude: geocentric longitude [rad] + * - latitude: geocentric latitude [rad] + * - altitude: [m] + * - timeOfMagMeasurement: time of actual measurement [s] + * + * Outputs: + * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ - // Main Function - void magFieldComp(const double longitude, const double gcLatitude, const double altitude, timeval timeOfMagMeasurement,double* magFieldModelInertial); - // Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is oriented from the satellite to earth COM - // Difference up to 25 km, which is 5 % of the total flight altitude - /* Inputs: - * - longitude: geocentric longitude [rad] - * - latitude: geocentric latitude [rad] - * - altitude: [m] - * - timeOfMagMeasurement: time of actual measurement [s] - * - * Outputs: - * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ + // Coefficient wary over year, could be updated sometimes. + void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) + double magFieldModel[3]; + private: + const 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}, + {0.0, 0.0, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7}, + {0.0, 0.0, 0.0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3}, + {0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8}, + {0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -11.9, -2.4, -0.6, -0.5, 0.4}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3.8, 0.2, 0.1, 0.1}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, -1.1, 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.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 - // Coefficient wary over year, could be updated sometimes. - void updateCoeffGH(timeval timeOfMagMeasurement); //Secular variation (SV) - double magFieldModel[3]; + const 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}, + {0.0, 0.0, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4}, + {0.0, 0.0, 0.0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4}, + {0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3}, + {0.0, 0.0, 0.0, 0.0, 0.0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6, -0.1, -3.0, 0.2, 0.5}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.8, -2.0, -0.9, 0.5}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.6, 0.0, -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.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 -private: - const 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}, - { 0.0, 0.0, 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7}, - { 0.0, 0.0, 0.0, 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3}, - { 0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8}, - { 0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4,-0.3, 0.0}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-11.9,-2.4,-0.6,-0.5, 0.4}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-3.8, 0.2, 0.1, 0.1}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1,-1.1, 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.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 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}, + {0.0, 0.0, -12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, -5.1, 1.3, -1.4, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.9, 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.9, -0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, -0.1, 0.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, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 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 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}, - { 0.0, 0.0,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4}, - { 0.0, 0.0, 0.0,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4}, - { 0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4,-3.4,-1.6, 0.6,-0.1}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6,-0.1,-3.0, 0.2, 0.5}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-8.8,-2.0,-0.9, 0.5}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-2.6, 0.0,-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.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 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}, + {0.0, 0.0, 0.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, -5.0, 3.0, 0.8, -0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.3, 0.0, -1.1, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1, -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.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.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, 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 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}, - {0.0, 0.0,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0,-5.1, 1.3,-1.4, 0.1,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.9, 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.9,-0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8,-0.1, 0.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, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, 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] = {{ 0.0, 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}, - { 0.0, 0.0, 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0,-5.0, 3.0, 0.8,-0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0, 0.0, 0.3, 0.0,-1.1,-0.3, 0.0, 0.0, 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1,-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.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.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, - { 0.0, 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 updatedG[14][13]; - double updatedH[14][13]; - static const int igrfOrder = 13; // degree of truncation + double updatedG[14][13]; + double updatedH[14][13]; + static const int igrfOrder = 13; // degree of truncation }; #endif /* IGRF13MODEL_H_ */ diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 8cfa0ad3..cc136f61 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -7,6 +7,7 @@ #include "MultiplicativeKalmanFilter.h" +#include #include #include #include @@ -34,11 +35,11 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_ void MultiplicativeKalmanFilter::reset() {} void MultiplicativeKalmanFilter::init( - const double *magneticField_, const bool *validMagField_, const double *sunDir_, - const bool *validSS, const double *sunDirJ, const bool *validSSModel, const double *magFieldJ, - const bool *validMagModel) { // valids for "model measurements"? + const double *magneticField_, const bool validMagField_, const double *sunDir_, + const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ, + const bool validMagModel) { // valids for "model measurements"? // check for valid mag/sun - if (*validMagField_ && *validSS && *validSSModel && *validMagModel) { + if (validMagField_ && validSS && validSSModel && validMagModel) { validInit = true; // AcsParameters mekfEstParams; // loadAcsParameters(&mekfEstParams); @@ -208,48 +209,78 @@ 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, double *outputQuat, double *outputSatRate) { + 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) { // Check for GYR Measurements // AcsParameters mekfEstParams; // loadAcsParameters(&mekfEstParams); int MDF = 0; // Matrix Dimension Factor - if (!(*validGYRs_)) { + if (!validGYRs_) { + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->setValidity(false, true); + } + } validMekf = false; return KALMAN_NO_GYR_MEAS; } // Check for Model Calculations - else if (!(*validSSModel) || !(*validMagModel)) { + else if (!validSSModel || !validMagModel) { + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->setValidity(false, true); + } + } validMekf = false; return KALMAN_NO_MODEL; } // Check Measurements available from SS, MAG, STR - if (*validSS && *validMagField_ && *validSTR_) { + if (validSS && validMagField_ && validSTR_) { sensorsAvail = 1; MDF = 9; - } else if (*validSS && *validMagField_ && !(*validSTR_)) { + } else if (validSS && validMagField_ && !validSTR_) { sensorsAvail = 2; MDF = 6; - } else if (*validSS && !(*validMagField_) && *validSTR_) { + } else if (validSS && !validMagField_ && validSTR_) { sensorsAvail = 3; MDF = 6; - } else if (!(*validSS) && *validMagField_ && *validSTR_) { + } else if (!validSS && validMagField_ && validSTR_) { sensorsAvail = 4; MDF = 6; - } else if (*validSS && !(*validMagField_) && !(*validSTR_)) { + } else if (validSS && !validMagField_ && !(validSTR_)) { sensorsAvail = 5; MDF = 3; - } else if (!(*validSS) && *validMagField_ && !(*validSTR_)) { + } else if (!validSS && validMagField_ && !validSTR_) { sensorsAvail = 6; MDF = 3; - } else if (!(*validSS) && !(*validMagField_) && *validSTR_) { + } else if (!validSS && !validMagField_ && validSTR_) { sensorsAvail = 7; MDF = 3; } else { sensorsAvail = 8; // no measurements validMekf = false; + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->setValidity(false, true); + } + } return returnvalue::FAILED; } @@ -289,7 +320,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( MathOperations::skewMatrix(magEstB, *measSensMatrix22); double measVecQuat[3] = {0, 0, 0}; - if (*validSTR_) { + if (validSTR_) { double quatError[4] = {0, 0, 0, 0}; double invPropagatedQuat[4] = {0, 0, 0, 0}; QuaternionOperations::inverse(propagatedQuaternion, invPropagatedQuat); @@ -913,11 +944,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( VectorOperations::add(propagatedQuaternion, errQuatTerm, quatBJ, 4); VectorOperations::normalize(quatBJ, quatBJ, 4); - outputQuat[0] = quatBJ[0]; - outputQuat[1] = quatBJ[1]; - outputQuat[2] = quatBJ[2]; - outputQuat[3] = quatBJ[3]; - double updatedGyroBias[3] = {0, 0, 0}; VectorOperations::add(biasGYR, errStateGyroBias, updatedGyroBias, 3); // Bias GYR State @@ -940,11 +966,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( {rotRateEst[2], 0, -rotRateEst[0]}, {-rotRateEst[1], rotRateEst[0], 0}}; - // Corrected Sat Rate via Bias - outputSatRate[0] = rotRateEst[0]; - outputSatRate[1] = rotRateEst[1]; - outputSatRate[2] = rotRateEst[2]; - // 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}}; @@ -1144,5 +1165,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // Check for new data in measurement -> SensorProcessing ? + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double)); + mekfData->setValidity(true, true); + } + } + return returnvalue::OK; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index f1d2d7a0..5eb47418 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -18,10 +18,9 @@ #include //uint8_t #include /*purpose, timeval ?*/ -#include "config/classIds.h" -//#include <_timeval.h> - +#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" +#include "config/classIds.h" class MultiplicativeKalmanFilter { public: @@ -40,9 +39,9 @@ class MultiplicativeKalmanFilter { * sunDirJ sun direction vector in the ECI frame * magFieldJ magnetic field vector in the ECI frame */ - void init(const double *magneticField_, const bool *validMagField_, const double *sunDir_, - const bool *validSS, const double *sunDirJ, const bool *validSSModel, - const double *magFieldJ, const bool *validMagModel); + void init(const double *magneticField_, const bool validMagField_, const double *sunDir_, + const bool validSS, const double *sunDirJ, const bool validSSModel, + const double *magFieldJ, const bool validMagModel); /* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter * for the current step after the initalization @@ -58,11 +57,11 @@ class MultiplicativeKalmanFilter { * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible, * RETURN_OK else */ - ReturnValue_t 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 *outputQuat, double *outputSatRate); + ReturnValue_t 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); // Declaration of Events (like init) and memberships // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 2c1596e5..921ae604 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -21,28 +21,34 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt Navigation::~Navigation() {} -void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, +void Navigation::useMekf(ACS::SensorValues *sensorValues, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, ReturnValue_t *mekfValid) { double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; - bool quatJBValid = - (sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() && - sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid()); + bool quatJBValid = sensorValues->strSet.caliQx.isValid() && + sensorValues->strSet.caliQy.isValid() && + sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); if (kalmanInit) { *mekfValid = multiplicativeKalmanFilter.mekfEst( - quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid, - outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst, - &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ, - outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ?? + quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value, + gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value, + mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value, + susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value, + susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value, + mgmDataProcessed->magIgrfModel.isValid(), + mekfData); // VALIDS FOR QUAT AND RATE ?? } else { - multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid, - outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->magFieldModel, &outputValues->magFieldModelValid); + multiplicativeKalmanFilter.init( + mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(), + susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(), + susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(), + mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid()); kalmanInit = true; - *mekfValid = 0; + *mekfValid = returnvalue::OK; // Maybe we need feedback from kalmanfilter to identify if there was a problem with the // init of kalman filter where does this class know from that kalman filter was not diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 6691b8aa..2474cb67 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -8,28 +8,28 @@ #ifndef NAVIGATION_H_ #define NAVIGATION_H_ - +#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" -#include "SensorProcessing.h" #include "MultiplicativeKalmanFilter.h" +#include "SensorProcessing.h" #include "SensorValues.h" -#include "OutputValues.h" -class Navigation{ -public: - Navigation(AcsParameters *acsParameters_); //Input mode ? - virtual ~Navigation(); +class Navigation { + public: + Navigation(AcsParameters *acsParameters_); // Input mode ? + virtual ~Navigation(); - void useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid); - void processSensorData(); + void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData, + ReturnValue_t *mekfValid); + void processSensorData(); -protected: - -private: - MultiplicativeKalmanFilter multiplicativeKalmanFilter; - AcsParameters acsParameters; - bool kalmanInit = false; + protected: + private: + MultiplicativeKalmanFilter multiplicativeKalmanFilter; + AcsParameters acsParameters; + bool kalmanInit = false; }; #endif /* ACS_NAVIGATION_H_ */ - diff --git a/mission/controller/acs/OutputValues.cpp b/mission/controller/acs/OutputValues.cpp deleted file mode 100644 index 42730fb3..00000000 --- a/mission/controller/acs/OutputValues.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "OutputValues.h" - -namespace ACS { - -OutputValues::OutputValues() {} - -OutputValues::~OutputValues() {} - -} // namespace ACS diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h deleted file mode 100644 index bb1ed007..00000000 --- a/mission/controller/acs/OutputValues.h +++ /dev/null @@ -1,44 +0,0 @@ -#include - -#include "../controllerdefinitions/AcsCtrlDefinitions.h" - -#ifndef OUTPUTVALUES_H_ -#define OUTPUTVALUES_H_ - -namespace ACS { - -class OutputValues { - public: - OutputValues(); - virtual ~OutputValues(); - - double magFieldEst[3]; // sensor fusion (G) // output value - bool magFieldEstValid; - double magFieldModel[3]; // igrf (IJK) // - bool magFieldModelValid; - double magneticFieldVectorDerivative[3]; - bool magneticFieldVectorDerivativeValid; - - bool mgmUpdated; // ToDo: relic from FLP. most likely not used - - double sunDirEst[3]; // sensor fusion (G) - bool sunDirEstValid; - double sunDirModel[3]; // sun model (IJK) - bool sunDirModelValid; - - double quatMekfBJ[4]; // mekf - bool quatMekfBJValid; - - double satRateEst[3]; - bool satRateEstValid; - double satRateMekf[3]; // after mekf with correction of bias - bool satRateMekfValid; - double sunVectorDerivative[3]; - bool sunVectorDerivativeValid; - - double gcLatitude; // geocentric latitude, radian - double gdLongitude; // Radian longitude -}; -} // namespace ACS - -#endif /*OUTPUTVALUES_H_*/ diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 11d2ebeb..61bd7a96 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -11,7 +11,6 @@ #include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" -#include "OutputValues.h" #include "SensorValues.h" #include "SusConverter.h" #include "config/classIds.h" diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 152f866c..18f0d16f 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -90,4 +90,3 @@ ReturnValue_t SensorValues::update() { } } // namespace ACS - diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 67903c7c..7421226d 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -32,34 +32,27 @@ void Detumble::loadAcsParameters(AcsParameters *acsParameters_){ } - -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 = detumbleCtrlParameters->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 = detumbleCtrlParameters->gainD; + double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); + VectorOperations::mulScalar(magRate, factor, magMom, 3); + return returnvalue::OK; } -ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom) { +ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, + double *magMom) { + if (!magRateValid) { + return DETUMBLE_NO_SENSORDATA; + } - if (!magRateValid) { - return DETUMBLE_NO_SENSORDATA; - } - - double dipolMax = magnetorquesParameter->DipolMax; - for (int i = 0; i<3; i++) { - - magMom[i] = - dipolMax * sign(magRate[i]); - - } - - return returnvalue::OK; + double dipolMax = magnetorquesParameter->DipolMax; + for (int i = 0; i < 3; i++) { + magMom[i] = -dipolMax * sign(magRate[i]); + } + return returnvalue::OK; } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index b85aaf86..375f67aa 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -9,7 +9,6 @@ #define ACS_CONTROL_DETUMBLE_H_ #include "../SensorValues.h" -#include "../OutputValues.h" #include "../AcsParameters.h" #include "../config/classIds.h" #include @@ -32,13 +31,12 @@ public: */ void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t bDotLaw(const double *magRate, const bool *magRateValid, - const double *magField, const bool *magFieldValid, - double *magMom); + ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, + const double *magField, const bool magFieldValid, double *magMom); - ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); + ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); -private: + private: AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters; AcsParameters::MagnetorquesParameter* magnetorquesParameter; }; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 7160fb47..e06b576f 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -112,7 +112,7 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do } -void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, +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)) { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index be67187d..64b4110c 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -19,7 +19,6 @@ #include #include "../AcsParameters.h" -#include "../OutputValues.h" #include "../SensorValues.h" #include "../config/classIds.h" @@ -45,7 +44,7 @@ class PtgCtrl { void ptgGroundstation(const double mode, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, + void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index c892fc05..54ae27ee 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -28,12 +28,12 @@ void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) { inertiaEIVE = &(acsParameters_->inertiaEIVE); } -ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid, - double *magFieldModel, bool *magFieldModelValid, - double *sunDirModel, bool *sunDirModelValid, double *satRateMekf, - bool *rateMekfValid, double *sunDirRef, double *satRatRef, - double *outputMagMomB, bool *outputValid) { - if (!(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) || !(*rateMekfValid)) { +ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, + double *magFieldModel, bool magFieldModelValid, + double *sunDirModel, bool sunDirModelValid, double *satRateMekf, + bool rateMekfValid, double *sunDirRef, double *satRatRef, + double *outputAngle, double *outputMagMomB, bool *outputValid) { + if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) { *outputValid = false; return SAFECTRL_MEKF_INPUT_INVALID; } @@ -80,31 +80,34 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid, VectorOperations::cross(magFieldB, torqueCmd, torqueMgt); double normMag = VectorOperations::norm(magFieldB, 3); VectorOperations::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3); + + *outputAngle = alpha; *outputValid = true; return returnvalue::OK; } // Will be the version in worst case scenario in event of no working MEKF (nor RMUs) -void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB, - bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, - double *magRateB, bool *magRateBValid, double *sunDirRef, - double *satRateRef, double *outputMagMomB, bool *outputValid) { +void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, + bool sunRateBValid, double *magFieldB, bool magFieldBValid, + double *magRateB, bool magRateBValid, double *sunDirRef, + double *satRateRef, double *outputAngle, double *outputMagMomB, + bool *outputValid) { // Check for invalid Inputs if (!susDirBValid || !magFieldBValid || !magRateBValid) { *outputValid = false; return; } - // normalize sunDir and magDir + // normalize sunDir and magDir double magDirB[3] = {0, 0, 0}; VectorOperations::normalize(magFieldB, magDirB, 3); VectorOperations::normalize(susDirB, susDirB, 3); - // Cosinus angle between sunDir and magDir + // Cosinus angle between sunDir and magDir double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); - // Rate parallel to sun direction and magnetic field direction + // Rate parallel to sun direction and magnetic field direction double rateParaSun = 0, rateParaMag = 0; double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0; dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); @@ -113,7 +116,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor; - // Full rate or estimate + // Full rate or estimate double estSatRate[3] = {0, 0, 0}; double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0}; VectorOperations::mulScalar(susDirB, rateParaSun, estSatRateSun, 3); @@ -123,8 +126,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub VectorOperations::add(estSatRateSun, estSatRateMag, estSatRate, 3); VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); - /* Only valid if angle between sun direction and magnetic field direction - is sufficiently large */ + /* Only valid if angle between sun direction and magnetic field direction + is sufficiently large */ double sinAngle = 0; sinAngle = sin(acos(cos(cosAngleSunMag))); @@ -169,9 +172,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, doub double magMomFactor = pow(VectorOperations::norm(magFieldB, 3), 2); VectorOperations::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); - outputMagMomB[0] = magMomB[0]; - outputMagMomB[1] = magMomB[1]; - outputMagMomB[2] = magMomB[2]; - + std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); + *outputAngle = angleAlignErr; *outputValid = true; } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 70426c9f..72e45f08 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -13,7 +13,6 @@ #include #include "../AcsParameters.h" -#include "../OutputValues.h" #include "../SensorValues.h" #include "../config/classIds.h" @@ -27,16 +26,16 @@ class SafeCtrl { void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid, double *magFieldModel, - bool *magFieldModelValid, double *sunDirModel, bool *sunDirModelValid, - double *satRateMekf, bool *rateMekfValid, double *sunDirRef, + ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, + bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, + double *satRateMekf, bool rateMekfValid, double *sunDirRef, double *satRatRef, // From Guidance (!) - double *outputMagMomB, bool *outputValid); + double *outputAngle, double *outputMagMomB, bool *outputValid); - void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB, - bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, double *magRateB, - bool *magRateBValid, double *sunDirRef, double *satRateRef, double *outputMagMomB, - bool *outputValid); + void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, + bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB, + bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle, + double *outputMagMomB, bool *outputValid); void idleSunPointing(); // with reaction wheels diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index bb0a40ae..1b07218f 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -17,6 +17,7 @@ enum SetIds : uint32_t { GYR_PROCESSED_DATA, GPS_PROCESSED_DATA, MEKF_DATA, + CTRL_VAL_DATA, ACTUATOR_CMD_DATA }; @@ -88,6 +89,10 @@ enum PoolIds : lp_id_t { // MEKF SAT_ROT_RATE_MEKF, QUAT_MEKF, + // Ctrl Values + TGT_QUAT, + ERROR_QUAT, + ERROR_ANG, // Actuator Cmd RW_TARGET_TORQUE, RW_TARGET_SPEED, @@ -102,6 +107,7 @@ 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 MEKF_SET_ENTRIES = 2; +static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 99; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; /** @@ -133,9 +139,9 @@ class MgmDataProcessed : public StaticLocalDataSet { lp_vec_t mgm2vec = lp_vec_t(sid.objectId, MGM_2_VEC, this); lp_vec_t mgm3vec = lp_vec_t(sid.objectId, MGM_3_VEC, this); lp_vec_t mgm4vec = lp_vec_t(sid.objectId, MGM_4_VEC, this); - lp_vec_t mgmVecTot = lp_vec_t(sid.objectId, MGM_VEC_TOT, this); - lp_vec_t mgmVecTotDerivative = - lp_vec_t(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this); + lp_vec_t mgmVecTot = lp_vec_t(sid.objectId, MGM_VEC_TOT, this); + lp_vec_t mgmVecTotDerivative = + lp_vec_t(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this); lp_vec_t magIgrfModel = lp_vec_t(sid.objectId, MAG_IGRF_MODEL, this); private: @@ -179,9 +185,9 @@ class SusDataProcessed : public StaticLocalDataSet { 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 susVecTot = lp_vec_t(sid.objectId, SUS_VEC_TOT, this); - lp_vec_t susVecTotDerivative = - lp_vec_t(sid.objectId, SUS_VEC_TOT_DERIVATIVE, 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); lp_vec_t sunIjkModel = lp_vec_t(sid.objectId, SUN_IJK_MODEL, this); private: @@ -232,6 +238,17 @@ class MekfData : public StaticLocalDataSet { private: }; +class CtrlValData : public StaticLocalDataSet { + public: + CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {} + + lp_vec_t tgtQuat = lp_vec_t(sid.objectId, TGT_QUAT, this); + lp_vec_t errQuat = lp_vec_t(sid.objectId, ERROR_QUAT, this); + lp_var_t errAng = lp_var_t(sid.objectId, ERROR_ANG, this); + + private: +}; + class ActuatorCmdData : public StaticLocalDataSet { public: ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {} From 4faf00de942d710f637ed4a4c2d2efa7934c12c1 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 3 Nov 2022 14:23:06 +0100 Subject: [PATCH 078/244] converted MTQ MGM readings from nT to uT --- mission/controller/acs/SensorProcessing.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 1baa4007..bb445485 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -102,8 +102,10 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const } } if (mgm4valid) { - VectorOperations::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias, - 3); + float mgm4ValueNT[3]; + VectorOperations::mulScalar(mgm4Value, 1e3, mgm4ValueNT, 3); // uT to nT + VectorOperations::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset, + mgm4ValueNoBias, 3); MatrixOperations::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias, mgm4ValueCalib, 3, 3, 1); MatrixOperations::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib, From c7bfe4002d7dcf16b13f3f375c225df080198b5f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 3 Nov 2022 14:24:09 +0100 Subject: [PATCH 079/244] added event for SAFE to DETUMBLE transition. changed actuator input from double to int --- mission/controller/AcsController.cpp | 32 +++++++++++++------ mission/controller/AcsController.h | 4 +++ .../AcsCtrlDefinitions.h | 7 ++-- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 953a91a1..799aa0c3 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -156,12 +156,13 @@ void AcsController::performSafe() { if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { submode = SUBMODE_DETUMBLE; detumbleCounter = 0; + triggerEvent(SAFE_RATE_VIOLATION); } { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { - double zeroVec[3] = {0, 0, 0, 0}; + double zeroVec[4] = {0, 0, 0, 0}; std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); actuatorCmdData.rwTargetTorque.setValid(false); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); @@ -175,8 +176,8 @@ void AcsController::performSafe() { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], - //torqueDuration); + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], + // torqueDuration); // } } @@ -215,15 +216,19 @@ void AcsController::performDetumble() { 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) { - double zeroVec[3] = {0, 0, 0, 0}; + double zeroVec[4] = {0, 0, 0, 0}; std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); actuatorCmdData.rwTargetTorque.setValid(false); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } @@ -232,7 +237,7 @@ void AcsController::performDetumble() { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // torqueDuration); // } } @@ -275,12 +280,21 @@ void AcsController::performPointingCtrl() { &(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, cmdSpeedRws, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolUnits, 3 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double)); actuatorCmdData.setValidity(true, true); } } @@ -288,7 +302,7 @@ void AcsController::performPointingCtrl() { // PoolReadGuard pg(&dipoleSet); // MutexGuard mg(torquer::lazyLock()); // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], + // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // torqueDuration); // } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 18c8d824..7f6363ce 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -30,6 +30,10 @@ class AcsController : public ExtendedControllerBase { static const Submode_t SUBMODE_PTG_NADIR = 5; static const Submode_t SUBMODE_PTG_INERTIAL = 6; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; + static const Event SAFE_RATE_VIOLATION = + MAKE_EVENT(0, severity::MEDIUM); //!< The limits for the rotation in safe mode were violated. + protected: void performSafe(); void performDetumble(); diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 1b07218f..2652ca3c 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -107,7 +107,7 @@ 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 MEKF_SET_ENTRIES = 2; -static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 99; +static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; /** @@ -254,8 +254,9 @@ class ActuatorCmdData : public StaticLocalDataSet { ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {} lp_vec_t rwTargetTorque = lp_vec_t(sid.objectId, RW_TARGET_TORQUE, this); - lp_vec_t rwTargetSpeed = lp_vec_t(sid.objectId, RW_TARGET_SPEED, this); - lp_vec_t mtqTargetDipole = lp_vec_t(sid.objectId, MTQ_TARGET_DIPOLE, this); + lp_vec_t rwTargetSpeed = lp_vec_t(sid.objectId, RW_TARGET_SPEED, this); + lp_vec_t mtqTargetDipole = + lp_vec_t(sid.objectId, MTQ_TARGET_DIPOLE, this); private: }; From 1ac9e53b1ff1f902bd345441fe9e83fb2bdb3246 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 09:35:17 +0100 Subject: [PATCH 080/244] add TAS sources --- CMakeLists.txt | 2 +- thirdparty/CMakeLists.txt | 5 + thirdparty/tas/hdlc.c | 80 +++++ thirdparty/tas/hdlc.h | 26 ++ thirdparty/tas/uart.c | 603 ++++++++++++++++++++++++++++++++++++++ thirdparty/tas/uart.h | 124 ++++++++ 6 files changed, 839 insertions(+), 1 deletion(-) create mode 100644 thirdparty/CMakeLists.txt create mode 100644 thirdparty/tas/hdlc.c create mode 100644 thirdparty/tas/hdlc.h create mode 100644 thirdparty/tas/uart.c create mode 100644 thirdparty/tas/uart.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ddf101a4..644b8bcd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -393,7 +393,7 @@ if(EIVE_ADD_JSON_LIB) add_subdirectory(${LIB_JSON_PATH}) endif() -add_subdirectory(thirdparty/rapidcsv) +add_subdirectory(thirdparty) if(EIVE_ADD_LINUX_FILES) add_subdirectory(${LIB_ARCSEC_PATH}) diff --git a/thirdparty/CMakeLists.txt b/thirdparty/CMakeLists.txt new file mode 100644 index 00000000..7455bdae --- /dev/null +++ b/thirdparty/CMakeLists.txt @@ -0,0 +1,5 @@ +if(EIVE_ADD_LINUX_FILES) + add_subdirectory(tas) +endif() + +add_subdirectory(rapidcsv) diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c new file mode 100644 index 00000000..0c668073 --- /dev/null +++ b/thirdparty/tas/hdlc.c @@ -0,0 +1,80 @@ +//************************************************************************************** +/*! \copyright: 2020-2021 Thales Alenia Space Deutschland GmbH +* \project: multiMIND +* \file: (name of source file: hdlc.c) +* \date: (09.02.2022) +* \author: (Stelios Filippopoulos) +* \brief: (hdlc functions) +* \language: (C) +************************************************************************************** +*/ + +#include +#include "hdlc.h" +#include "spacepacket.h" +#include "crc.h" + +static void hdlc_add_byte(uint8_t ch, uint8_t *buff, uint16_t *pos) +{ + uint16_t templen = *pos; + + if ((ch == 0x7E) || + (ch == 0x7D) || + (ch == 0x7C)) + { + buff[templen++] = 0x7D; + ch ^= 0x20; + } + buff[templen++] = ch; + + *pos = templen; +} + +void hdlc_add_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen) +{ + uint16_t tlen = 0; + uint16_t ii; + uint16_t crc16; + uint8_t bt; + + // calc crc16 + crc16 = calc_crc16_buff_reflected( src, slen ); + + dst[tlen++] = 0x7E; + for (ii = 0; ii < slen; ii++) + { + bt = *src++; + hdlc_add_byte(bt, dst, &tlen); + } + + // hdlc crc16 is in little endian format + hdlc_add_byte((uint8_t) (crc16 & 0xFF), dst, &tlen); + hdlc_add_byte((uint8_t) ((crc16 >> 8) & 0xFF), dst, &tlen); + + dst[tlen++] = 0x7C; + *dlen = tlen; +} + +void hdlc_remove_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen) +{ + uint16_t tlen = 0; + uint16_t ii; + uint8_t bt; + + *dlen = 0; + if (slen == 0) return; + if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return; + for (ii = 1; ii < slen-1; ii++) + { + bt = *src++; + + if (bt == 0x7D) + { + bt = *src++ ^ 0x20; + } + dst[tlen++] = bt; + } + *dlen = tlen; +} + + diff --git a/thirdparty/tas/hdlc.h b/thirdparty/tas/hdlc.h new file mode 100644 index 00000000..a728c5c5 --- /dev/null +++ b/thirdparty/tas/hdlc.h @@ -0,0 +1,26 @@ +//************************************************************************************** +/*! \copyright: 2020-2021 Thales Alenia Space Deutschland GmbH +* \project: multiMIND +* \file: (name of source file: hdlc.h) +* \date: (09.02.2022) +* \author: (Stelios Filippopoulos) +* \brief: (hdlc header file) +* \language: (C) +************************************************************************************** +*/ + +#ifndef LIB_HDLC_H_ +#define LIB_HDLC_H_ + +#define HDLC_ENABLE + +#define HDLC_START_BYTE (0x7Eu) +#define HDLC_ESC_BYTE (0x7Du) +#define HDLC_END_BYTE (0x7Cu) +#define HDLC_ESCAPE_CHAR (0x20u) + +void hdlc_add_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen); + +void hdlc_remove_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen); + +#endif /* LIB_HDLC_H_ */ diff --git a/thirdparty/tas/uart.c b/thirdparty/tas/uart.c new file mode 100644 index 00000000..51ce59d8 --- /dev/null +++ b/thirdparty/tas/uart.c @@ -0,0 +1,603 @@ +//************************************************************************************** +/*! \copyright: 2020-2021 Thales Alenia Space Deutschland GmbH + * \project: multiMIND + * \file: (name of source file: uart.c) + * \date: (20.05.2021) + * \author: (Sarthak Kelapure) + * \brief: (UART thread to collect data on serial interface) + * \language: (C) + ************************************************************************************** + */ + +#include "uart.h" +#include "hdlc.h" + +#ifdef HDLC_ENABLE +#define HDLC_RX_STATE_IDLE (0u) +#define HDLC_RX_STATE_RECEIVING (1u) +#define HDLC_RX_STATE_ESCAPE (2u) +#endif + +/** + * @struct Serial device structure. + * Encapsulates a serial connection. + */ +struct serial_s { + int fd; //>! Connection file descriptor. + int state; //>! Signifies connection state. + int running; //>! Signifies thread state. + + char rxbuff[BUFF_SIZE]; //>! Buffer for RX data. + int start, end; //>! Pointers to start and end of buffer. + + pthread_t rx_thread; //>! Listening thread. +}; + +// --------------- Internal Functions --------------- + +/** + * Connect to a serial device. + * @param s - serial structure. + * @param device - serial device name. + * @param baud - baud rate for connection. + * @return -ve on error, 0 on success. + */ +static int serial_connect(serial_t* s, char device[], int baud); + +/** + * Create the serial structure. + * Convenience method to allocate memory + * and instantiate objects. + * @return serial structure. + */ +static serial_t* serial_create(); + +static int serial_resolve_baud(int baud); + +/** + * Recieve data. + * Retrieves data from the serial device. + * @param s - serial structure. + * @param data - pointer to a buffer to read data into. + * @param maxLength - size of input buffer. + * @return amount of data recieved. + */ +static int serial_recieve(serial_t* obj, uint8_t data[], int maxLength); + +/** + * @brief Serial Listener Thread. + * This blocks waiting for data to be recieved from the serial device, + * and calls the serial_callback method with appropriate context when + * data is recieved. + * Exits when close method is called, or serial error occurs. + * @param param - context passed from thread instantiation. + */ +static void *serial_data_listener(void *param); + +/** + * @brief Start the serial threads. + * This spawns the listening and transmitting threads + * if they are not already running. + * @param s - serial structure. + * @return 0 on success, or -1 on error. + */ +static int serial_start(serial_t* s); + +/** + * Stop serial listener thread. + * @param s - serial structure. + * @return 0; + */ +static int serial_stop(serial_t* s); + +/** + * Callback to handle recieved data. + * Puts recieved data into the rx buffer. + * @param s - serial structure. + * @param data - data to be stored. + * @param length - length of recieved data. + */ +static void serial_rx_callback(serial_t* s, char data[], int length); + +// Put character in rx buffer. +static int buffer_put(serial_t* s, char c) +{ + //if there is space in the buffer + if ( s->end != ((s->start + BUFF_SIZE - 1) % BUFF_SIZE)) { + s->rxbuff[s->end] = c; + s->end ++; + s->end = s->end % BUFF_SIZE; + //printf("Put: %x start: %d, end: %d\r\n", c, s->start, s->end); + return 0; //No error + } else { + //buffer is full, this is a bad state + return -1; //Report error + } +} + +// Get character from rx buffer. +static char buffer_get(serial_t* s) +{ + char c = (char)0; + //if there is data to process + if (s->end != s->start) { + c = (s->rxbuff[s->start]); + s->start ++; + //wrap around + s->start = s->start % BUFF_SIZE; + } else { + } + //printf("Get: %x start: %d, end: %d\r\n", c, s->start, s->end); + return c; +} + +//Get data available in the rx buffer. +static int buffer_available(serial_t* s) +{ + return (s->end - s->start + BUFF_SIZE) % BUFF_SIZE; +} + +// --------------- External Functions --------------- + +//Create serial object. +serial_t* serial_create() +{ + //Allocate serial object. + serial_t* s = malloc(sizeof(serial_t)); + //Reconfigure buffer object. + s->start = 0; + s->end = 0; + //Return pointer. + return s; +} + + +void uart_destroy(serial_t* s) +{ + free(s); +} + + +//Connect to serial device. +int serial_connect(serial_t* s, char device[], int baud) +{ + struct termios oldtio; + + // Resolve baud. + int speed = serial_resolve_baud(baud); + if (speed < 0) { + printf("Error: Baud rate not recognized.\r\n"); + return -1; + } + + //Open device. + s->fd = open(device, O_RDWR | O_NOCTTY); + //Catch file open error. + if (s->fd < 0) { + perror(device); + return -2; + } + //Retrieve settings. + tcgetattr(s->fd, &oldtio); + //Set baud rate. + cfsetspeed(&oldtio, speed); + //Flush cache. + tcflush(s->fd, TCIFLUSH); + + //Set UART settings, standard ones. 8N1 + oldtio.c_cflag = (oldtio.c_cflag & ~CSIZE) | CS8; // 8-bit chars + // disable IGNBRK for mismatched speed tests; otherwise receive break + // as \000 chars + oldtio.c_iflag &= ~IGNBRK; // disable break processing + oldtio.c_lflag = 0; // no signaling chars, no echo, + // no canonical processing + oldtio.c_oflag = 0; // no remapping, no delays + oldtio.c_cc[VMIN] = 0; // read doesn't block + oldtio.c_cc[VTIME] = 5; // 0.5 seconds read timeout + + oldtio.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl + oldtio.c_iflag &= ~(IGNCR | ICRNL | INLCR); // CR and LF characters are not affected + + oldtio.c_cflag |= (CLOCAL | CREAD);// ignore modem controls, + // enable reading + oldtio.c_cflag &= ~(PARENB | PARODD); // shut off parity + oldtio.c_cflag |= 0; + oldtio.c_cflag &= ~CSTOPB; + oldtio.c_cflag &= ~(020000000000); + //Apply settings. + if(tcsetattr(s->fd, TCSANOW, &oldtio) !=0){ + printf("ERROR: serial settings failed\r\n"); + return -1; + } + + //Start listener thread. + int res = serial_start(s); + //Catch error. + if (res < 0) { + printf("Error: serial thread could not be spawned\r\n"); + return -3; + } + + //Indicate connection was successful. + s->state = 1; + return 0; +} + +serial_t* uart_init(char device[], int baud) +{ + serial_t* s = serial_create(); + if(serial_connect(s, device, baud)< 0) + { + return NULL; + } + return s; +} + +//Send data. +uint32_t uart_length_send(serial_t* s, uint8_t data[], int length) +{ +// uint16_t ii; +// int res; +// for (ii = 0; ii < length; ii++) +// { +// res = write(s->fd, &data[ii], 1); +// } + int res = write(s->fd, data, length); + return res; +} + +void uart_send(serial_t* s, uint8_t data) +{ + char arr[1]; + arr[0] = data; + write(s->fd, arr, 1); +} + +//Determine characters available. +int uart_available(serial_t* s) +{ + return buffer_available(s); +} + +//Fetch a character. +char uart_get(serial_t* s) +{ + char c = buffer_get(s); + + return c; +} + +int uart_length_get(serial_t* s, char* buff, int len, bool start_of_packet) +{ + int ret = 0; + if (len > 0 && len < BUFF_SIZE) + { +#ifdef HDLC_ENABLE + uint8_t ch; + uint8_t hdlc_rx_state; + int rxb = 0; + + if (start_of_packet) + hdlc_rx_state = HDLC_RX_STATE_IDLE; + else + hdlc_rx_state = HDLC_RX_STATE_RECEIVING; + + while (rxb < len) + { + ch = uart_blocking_get(s); + + switch (hdlc_rx_state) + { + case HDLC_RX_STATE_IDLE: + if (ch == HDLC_START_BYTE) + { + rxb = 0; + ret = 0; + hdlc_rx_state = HDLC_RX_STATE_RECEIVING; + } + break; + case HDLC_RX_STATE_RECEIVING: + if (ch == HDLC_START_BYTE) + { + rxb = 0; + ret = 0; + break; + } + if (ch == HDLC_ESC_BYTE) + { + hdlc_rx_state = HDLC_RX_STATE_ESCAPE; + break; + } + buff[rxb++] = ch; + ret++; + break; + case HDLC_RX_STATE_ESCAPE: + if (ch == HDLC_START_BYTE) + { + rxb = 0; + ret = 0; + break; + } + buff[rxb++] = ch ^ HDLC_ESCAPE_CHAR; + ret++; + hdlc_rx_state = HDLC_RX_STATE_RECEIVING; + break; + } + } +#else + for (int i=0;i 2u) // do not include HDLC CRC16 + { + return buff_pos; + } + buff_pos = 0u; + hdlc_rx_state = HDLC_RX_STATE_IDLE; + break; + } + if (ch == HDLC_ESC_BYTE) + { + hdlc_rx_state = HDLC_RX_STATE_ESCAPE; + break; + } + if (buff_pos >= buff_len) + { + hdlc_rx_state = HDLC_RX_STATE_RECEIVING; + break; + } + buff[buff_pos++] = ch; + break; + + case HDLC_RX_STATE_ESCAPE: + if ((ch == HDLC_START_BYTE) || (ch == HDLC_END_BYTE)) + { + buff_pos = 0; + hdlc_rx_state = HDLC_RX_STATE_RECEIVING; + break; + } + if (buff_pos >= buff_len) + { + hdlc_rx_state = HDLC_RX_STATE_RECEIVING; + break; + } + buff[buff_pos++] = ch ^ HDLC_ESCAPE_CHAR; + hdlc_rx_state = HDLC_RX_STATE_RECEIVING; + break; + + default: + buff_pos = 0u; + hdlc_rx_state = HDLC_RX_STATE_IDLE; + break; + } + } +} + +char uart_blocking_get(serial_t* s) +{ + while (uart_available(s) == 0); + return uart_get(s); +} + +void uart_clear(serial_t* s) +{ + //Clear the buffer. + while (buffer_available(s)) { + buffer_get(s); + } + tcflush(s->fd, TCIFLUSH); +} + +//Close serial port. +int uart_close(serial_t* s) +{ + //Stop thread. + serial_stop(s); + return 0; +} + +void uart_deinit(serial_t* s){ + uart_clear(s); + uart_close(s); + uart_destroy(s); +} + +// --------------- Internal Functions -------------- + +//Stop serial listener thread. +static int serial_stop(serial_t* s) +{ + s->running = 0; + return close(s->fd); +} + +// Resolves standard baud rates to linux constants. +static int serial_resolve_baud(int baud) +{ + int speed; + // Switch common baud rates to temios constants. + switch (baud) { + case 9600: + speed = B9600; + break; + case 19200: + speed = B19200; + break; + case 38400: + speed = B38400; + break; + case 57600: + speed = B57600; + break; + case 115200: + speed = B115200; + break; + case 230400: + speed = B230400; + break; + case 460800: + speed = B460800; + break; + case 500000: + speed = B500000; + break; + case 576000: + speed = B576000; + break; + case 921600: + speed = B921600; + break; + case 1000000: + speed = B1000000; + break; + case 1152000: + speed = B1152000; + break; + case 1500000: + speed = B1500000; + break; + case 2000000: + speed = B2000000; + break; + case 3000000: + speed = B3000000; + break; + default: + speed = -1; + break; + } + // Return. + return speed; +} + +// Start serial listener. +static int serial_start(serial_t* s) +{ + //Only start if it is not currently running. + if (s->running != 1) { + //Set running. + s->running = 1; + //Spawn thread. + int res; + res = pthread_create(&s->rx_thread, NULL, serial_data_listener, (void*) s); + if (res != 0) { + return -2; + } + //Return result. + return 0; + } else { + return -1; + } +} + + + +//Recieve data. +static int serial_recieve(serial_t* s, uint8_t data[], int maxLength) +{ + return read(s->fd, data, maxLength); +} + +//Callback to store data in buffer. +static void serial_rx_callback(serial_t* s, char data[], int length) +{ + //Put data into buffer. + int i; + //Put data into buffer. + for (i = 0; i < length; i++) { + buffer_put(s, data[i]); + } + +} + +//Serial data listener thread. +static void *serial_data_listener(void *param) +{ + int res = 0; + int err = 0; + struct pollfd ufds; + uint8_t buff[BUFF_SIZE]; + + //Retrieve paramaters and store locally. + serial_t* serial = (serial_t*) param; + int fd = serial->fd; + + //Set up poll file descriptors. + ufds.fd = fd; //Attach socket to watch. + ufds.events = POLLIN; //Set events to notify on. + + //Run until ended. + while (serial->running != 0) { + //Poll socket for data. + res = poll(&ufds, 1, POLL_TIMEOUT); + //If data was recieved. + if (res > 0) { + //Fetch the data. + int count = serial_recieve(serial, buff, BUFF_SIZE - 1); + //If data was recieved. + if (count > 0) { + //Pad end of buffer to ensure there is a termination symbol. + buff[count] = '\0'; + // Call the serial callback. + serial_rx_callback(serial, (char *)buff, count); + //If an error occured. + } else if (count < 0) { + //Inform user and exit thread. + printf("Error: Serial disconnect\r\n"); + err = 1; + break; + } + //If there was an error. + } else if (res < 0) { + //Inform user and exit thread. + printf("Error: Polling error in serial thread"); + err = 1; + break; + } + //Otherwise, keep going around. + } + //If there was an error, close socket. + if (err) { + uart_close(serial); + //raise(SIGLOST); + } + //Close file. + res = close(serial->fd); + + return NULL; +} diff --git a/thirdparty/tas/uart.h b/thirdparty/tas/uart.h new file mode 100644 index 00000000..233a79d6 --- /dev/null +++ b/thirdparty/tas/uart.h @@ -0,0 +1,124 @@ +//************************************************************************************** +/*! \copyright: 2020-2021 Thales Alenia Space Deutschland GmbH +* \project: multiMIND +* \file: (name of source file: uart.h) +* \date: (20.05.2021) +* \author: (Sarthak Kelapure) +* \brief: (UART thread to collect data on serial interface) +* \language: (C) +************************************************************************************** +*/ +#ifndef LIB_UART_H +#define LIB_UART_H + +#define BUFF_SIZE 512 +#define POLL_TIMEOUT 2000 + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +typedef struct serial_s serial_t; + +/** + * Destroy the serial structure + */ +void uart_destroy(serial_t* s); + +/** + * Initializes the serial connection + * @param device - serial device name. + * @param baud - baud rate for connection. + * @return serial structure. + */ +serial_t* uart_init(char device[], int baud); + +/** + * Send data. + * @param s - serial structure. + * @param data - character array to transmit. + * @param length - size of the data array. + */ +uint32_t uart_length_send(serial_t* s, uint8_t data[], int length); + +/** + * Send a single character. + * @param s - serial structure. + * @param data - single character to be sent. + */ +void uart_send(serial_t* s, uint8_t data); + +/** + * Determine how much data is available + * in the serial buffer. + * @param s - serial structure. + * @return number of characters available. + */ +int uart_available(serial_t* s); + +/** + * Fetch one char from the serial buffer. + * @param s - serial structure. + * @return character. Null if empty. + */ +char uart_get(serial_t* s); + +/** + * Fetch length of chars from the serial buffer. + * @param s - serial structure. + * @param buff - readback storage + * @param len - length to get + * @return length. zero if empty. + */ +int uart_length_get(serial_t* s, char* buff, int len, bool start_of_packet); + +uint16_t uart_get_hdlc_packet(serial_t* s, uint8_t *buff, uint16_t buff_len); + +/** + * Fetch one char from the serial buffer. + * Blocks until data becomes available. + * @param s - serial structure. + * @return character. + */ +char uart_blocking_get(serial_t* s); + +/** + * Clear the serial buffer. + * @param s - serial structure. + */ +void uart_clear(serial_t* s); + +/** + * Close the serial port. + * @param s - serial structure. + * @return value of close(). + */ +int uart_close(serial_t* s); + +/** + * Deinitializes the UART + * @param s - serial structure. + */ +void uart_deinit(serial_t* s); + +#ifdef __cplusplus +} +#endif + +#endif //LIB_UART_H From 5dc41badc3b241ea59b5d777f532531f8d42ea84 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 09:37:14 +0100 Subject: [PATCH 081/244] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index f62c67a1..f6fab2d4 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f62c67a11e54b99fdb60dab13e55456f16450951 +Subproject commit f6fab2d44aff98174835c8446fd69c1ff589521b From 048cd89053109396dca297053c192c2629207b36 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 09:45:32 +0100 Subject: [PATCH 082/244] use scoped includes --- thirdparty/tas/CMakeLists.txt | 9 ++ thirdparty/tas/crc.c | 195 ++++++++++++++++++++++++++++++++ thirdparty/tas/hdlc.c | 5 +- thirdparty/tas/tas/crc.h | 107 ++++++++++++++++++ thirdparty/tas/{ => tas}/hdlc.h | 0 thirdparty/tas/{ => tas}/uart.h | 0 thirdparty/tas/uart.c | 4 +- 7 files changed, 315 insertions(+), 5 deletions(-) create mode 100644 thirdparty/tas/CMakeLists.txt create mode 100644 thirdparty/tas/crc.c create mode 100644 thirdparty/tas/tas/crc.h rename thirdparty/tas/{ => tas}/hdlc.h (100%) rename thirdparty/tas/{ => tas}/uart.h (100%) diff --git a/thirdparty/tas/CMakeLists.txt b/thirdparty/tas/CMakeLists.txt new file mode 100644 index 00000000..ab4e75e7 --- /dev/null +++ b/thirdparty/tas/CMakeLists.txt @@ -0,0 +1,9 @@ +target_sources(${OBSW_NAME} PRIVATE + hdlc.c + uart.c + crc.c +) + +target_include_directories(${OBSW_NAME} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) diff --git a/thirdparty/tas/crc.c b/thirdparty/tas/crc.c new file mode 100644 index 00000000..15d152b9 --- /dev/null +++ b/thirdparty/tas/crc.c @@ -0,0 +1,195 @@ +/*************************************************************************************** +* \copyright: 2020-2022 Thales Alenia Space Deutschland GmbH +* \project: multiMIND +* \file: crc.c +* \date: 22.02.2022 +* \author: David Woodward +* \brief: CRC algorithms +***************************************************************************************/ + +#include +#include "tas/crc.h" + + +const uint16_t crc16_0x1021_table[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, + 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, + 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, + 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, + 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, + 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, + 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, + 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, + 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, + 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, + 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, + 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, + 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, + 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, + 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 +}; + +const uint16_t crc16_0x1021_table_reverse[256] = +{ + 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF, + 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7, + 0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E, + 0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876, + 0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD, + 0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5, + 0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C, + 0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974, + 0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB, + 0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3, + 0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A, + 0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72, + 0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9, + 0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1, + 0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738, + 0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70, + 0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7, + 0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF, + 0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036, + 0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E, + 0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5, + 0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD, + 0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134, + 0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C, + 0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3, + 0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB, + 0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232, + 0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A, + 0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1, + 0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9, + 0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330, + 0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78 +}; + +// CRC-32 calculation from original implementation (Sarthak) +// The used algorithm is (most likely) CRC32/BZIP2, as found here: +// https://www.cl.cam.ac.uk/research/srg/projects/fairisle/bluebook/21/crc/node6.html +uint32_t Crc32(const uint8_t *msg, int numBytes, uint32_t remainder) { + + int byte; + unsigned char bit; + + // Perform modulo-2 division, a byte at a time. + for (byte = 0; byte < numBytes; ++byte) + { + // Bring the next byte into the remainder. + remainder ^= (*(msg + byte) << 16); + + // Perform modulo-2 division, a bit at a time. + for (bit = 8; bit > 0; --bit) { + + // Try to divide the current data bit. + if (remainder & CRC32_TOPBIT) { + remainder = (remainder << 1) ^ CRC32_POLYNOMIAL; + } + else { + remainder = (remainder << 1); + } + } + } + + // The final remainder is the CRC result. + return remainder; +} + +// ref.: CRC-16/CCITT-FALSE, alias: CRC-16/AUTOSAR +// https://reveng.sourceforge.io/crc-catalogue/16.htm#crc.cat.crc-16-xmodem +// initial: 0xFFFF, xorOut: 0x0000, RefIn: false, RefOut: false, polynomial: 0x1021 +uint16_t calc_crc16_unreflected(const uint8_t *data, uint32_t len, uint16_t remainder, uint16_t final_xor) +{ + uint16_t crc = remainder; + uint16_t temp; + + // unreflected + while (len-- != 0) + { + temp = (*data++ ^ (crc >> 8)) & 0xff; + crc = crc16_0x1021_table[temp] ^ (crc << 8); + } + + crc ^= final_xor; + + return crc; +} + +void calc_crc16_byte_unreflected(uint16_t *crc16, uint8_t bt) +{ + uint16_t temp; + temp = *crc16; + + // unreflected + *crc16 = crc16_0x1021_table[((temp >> 8) ^ bt) & 0xff] ^ (temp << 8); +} + +// initial: 0xFFFF, xorOut: 0x0000, RefIn: false, RefOut: false, polynomial: 0x1021 +uint16_t calc_crc16_buff_unreflected(uint8_t *data, uint16_t len) +{ + uint16_t crc16 = 0xFFFF; + + // unreflected + while (len-- != 0) + { + crc16 = crc16_0x1021_table[((crc16 >> 8) ^ *data++) & 0xff] ^ (crc16 << 8); + } + + return crc16; +} + +// ref.: CRC-16/X25 +// initial: 0xFFFF, xorOut: 0xFFFF, RefIn: true, RefOut: true, polynomial: 0x1021 +uint16_t calc_crc16_reflected(const uint8_t *data, uint32_t len, uint16_t remainder, uint16_t final_xor) +{ + uint16_t crc16 = remainder; + + // reflected + while (len-- != 0) + { + crc16 = crc16_0x1021_table_reverse[(crc16 ^ *data++) & 0xff] ^ (crc16 >> 8); + } + + return (crc16 ^ final_xor); +} + +void calc_crc16_byte_reflected(uint16_t *crc16, uint8_t bt) +{ + uint16_t temp; + temp = *crc16; + + // reflected + *crc16 = crc16_0x1021_table_reverse[(temp ^ bt) & 0xff] ^ (temp >> 8); +} + +// initial: 0xFFFF, xorOut: 0xFFFF, RefIn: true, RefOut: true, polynomial: 0x1021 +uint16_t calc_crc16_buff_reflected(uint8_t *data, uint16_t len) +{ + uint16_t crc16 = 0xFFFF; + + // reflected + while (len-- != 0) + { + crc16 = crc16_0x1021_table_reverse[(crc16 ^ *data++) & 0xff] ^ (crc16 >> 8); + } + + return (crc16 ^ 0xFFFF); +} diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 0c668073..6debec9c 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -10,9 +10,8 @@ */ #include -#include "hdlc.h" -#include "spacepacket.h" -#include "crc.h" +#include "tas/hdlc.h" +#include "tas/crc.h" static void hdlc_add_byte(uint8_t ch, uint8_t *buff, uint16_t *pos) { diff --git a/thirdparty/tas/tas/crc.h b/thirdparty/tas/tas/crc.h new file mode 100644 index 00000000..f92664b2 --- /dev/null +++ b/thirdparty/tas/tas/crc.h @@ -0,0 +1,107 @@ +/*************************************************************************************** +* \copyright: 2020-2022 Thales Alenia Space Deutschland GmbH +* \project: multiMIND +* \file: crc.c +* \date: 22.02.2022 +* \author: David Woodward +* \brief: CRC algorithms +***************************************************************************************/ + +#ifndef TAS_D_C_CRC_H +#define TAS_D_C_CRC_H + +#include +#include + +// NOTE: These defines are in the header as some are needed for (initial) crc function calls +//CRC-32/BZIP2 +#define CRC32_TOPBIT (1UL<<31) +#define CRC32_POLYNOMIAL 0x04C11DB7 +#define CRC32_INITIAL_REMAINDER 0xFFFFFFFF +#define CRC32_FINAL_XOR_VALUE 0xFFFFFFFF + +// CRC-16/CCITT-FALSE +#define CRC16_INITIAL_REMAINDER 0xFFFF +#define CRC16_FINAL_XOR_VALUE 0x0 + +extern const uint16_t crc16_0x1021_table[256]; + +extern const uint16_t crc16_0x1021_table_reverse[256]; + +/** + * \brief CRC-32/BZIP2 algorithm + */ +uint32_t Crc32(const uint8_t *msg, int numBytes, uint32_t remainder); + + +/** + * \brief CRC-16/CCITT-FALSE (alias CRC-16/AUTOSAR) algorithm, +// initial: 0xFFFF, xorOut: 0x0000, RefIn: false, RefOut: false, polynomial: 0x1021 + * using a lookup table + * \param data Data + * \param len Data length + * \param remainder Remainder to be used, + * use initial remainder for non coherent/standalone calculations + * \param final_xor The value that the final result will be xored + * \return CRC result + */ +uint16_t calc_crc16_unreflected(const uint8_t *data, uint32_t len, uint16_t remainder, uint16_t final_xor); + +/** + * generates a 16-bit CRC for the said data + * + * @param data input data for CRC + * @param len length of the data + * @return crc Generated 16-bit CRC + */ +void calc_crc16_byte_unreflected(uint16_t *crc16, uint8_t bt); + +/** + * \brief CRC-16/CCITT-FALSE (alias CRC-16/AUTOSAR) algorithm, + * polynomial: 0x1021, initial: 0xFFFF, final xor: 0x0, + * using a lookup table + * \param data Data + * \param len Data length + * \param remainder Remainder to be used, + * use initial remainder for non coherent/standalone calculations + * \param final_xor The value that the final result will be xored + * \return CRC result + */ +uint16_t calc_crc16_buff_unreflected(uint8_t *data, uint16_t len); + +/** + * \brief CRC-16/X25 algorithm, + * initial: 0xFFFF, xorOut: 0xFFFF, RefIn: true, RefOut: true, polynomial: 0x1021 + * using a lookup table + * \param data Data + * \param len Data length + * \param remainder Remainder to be used, + * use initial remainder for non coherent/standalone calculations + * \param final_xor The value that the final result will be xored + * \return CRC result + */ +uint16_t calc_crc16_reflected(const uint8_t *data, uint32_t len, uint16_t remainder, uint16_t final_xor); + +/** + * \brief CRC-16/X25 algorithm, + * calculates the crc16 for the next byte, given an already calculated crc16 + * + * @param *crc16 : calculated crc16 - the value will be updated + * @param bt : next byte for crc16 calculation + * @return none + */ +void calc_crc16_byte_reflected(uint16_t *crc16, uint8_t bt); + +/** + * \brief CRC-16/X25 algorithm, + * initial: 0xFFFF, xorOut: 0xFFFF, RefIn: true, RefOut: true, polynomial: 0x1021 + * using a lookup table + * \param data Data + * \param len Data length + * \param remainder Remainder to be used, + * use initial remainder for non coherent/standalone calculations + * \param final_xor The value that the final result will be xored + * \return CRC result + */ +uint16_t calc_crc16_buff_reflected(uint8_t *data, uint16_t len); +#endif diff --git a/thirdparty/tas/hdlc.h b/thirdparty/tas/tas/hdlc.h similarity index 100% rename from thirdparty/tas/hdlc.h rename to thirdparty/tas/tas/hdlc.h diff --git a/thirdparty/tas/uart.h b/thirdparty/tas/tas/uart.h similarity index 100% rename from thirdparty/tas/uart.h rename to thirdparty/tas/tas/uart.h diff --git a/thirdparty/tas/uart.c b/thirdparty/tas/uart.c index 51ce59d8..11e118ed 100644 --- a/thirdparty/tas/uart.c +++ b/thirdparty/tas/uart.c @@ -9,8 +9,8 @@ ************************************************************************************** */ -#include "uart.h" -#include "hdlc.h" +#include "tas/uart.h" +#include "tas/hdlc.h" #ifdef HDLC_ENABLE #define HDLC_RX_STATE_IDLE (0u) From de66ac66c6edce8e9fef055958a2639949131712 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 11:08:47 +0100 Subject: [PATCH 083/244] supv helper is a com IF now --- fsfw | 2 +- linux/devices/ScexUartReader.cpp | 17 ++--- linux/devices/ploc/PlocSupervisorHandler.cpp | 2 +- linux/devices/ploc/PlocSupvHelper.cpp | 64 ++++++++++++++++++- linux/devices/ploc/PlocSupvHelper.h | 65 +++++++++++++++++++- 5 files changed, 132 insertions(+), 18 deletions(-) diff --git a/fsfw b/fsfw index 84b9d1ce..672fca51 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 84b9d1ce216c076bdfeeaf1663aa873c7f1c9cff +Subproject commit 672fca5169b017387e58e2ff864913d932c59aa1 diff --git a/linux/devices/ScexUartReader.cpp b/linux/devices/ScexUartReader.cpp index d128fa63..7d632b06 100644 --- a/linux/devices/ScexUartReader.cpp +++ b/linux/devices/ScexUartReader.cpp @@ -98,18 +98,11 @@ ReturnValue_t ScexUartReader::initializeInterface(CookieIF *cookie) { } // Setting up UART parameters tty.c_cflag &= ~PARENB; // Clear parity bit - if (uartCookie->getStopBits() == StopBits::TWO_STOP_BITS) { - // Use two stop bits - tty.c_cflag |= CSTOPB; - } else { - // Clear stop field, only one stop bit used in communication - tty.c_cflag &= ~CSTOPB; - } - - tty.c_cflag &= ~CSIZE; // Clear all the size bits - tty.c_cflag |= CS8; // 8 bits per byte - tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control - tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines (CLOCAL = 1) + uart::setStopbits(tty, uartCookie->getStopBits()); + uart::setBitsPerWord(tty, BitsPerWord::BITS_8); + tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control + uart::enableRead(tty); + uart::ignoreCtrlLines(tty); // Use non-canonical mode and clear echo flag tty.c_lflag &= ~(ICANON | ECHO); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index dcdff2c3..5b4339e9 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -150,7 +150,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, if (size > config::MAX_PATH_SIZE) { return SupvReturnValuesIF::FILENAME_TOO_LONG; } - result = supvHelper->startEventbBufferRequest( + result = supvHelper->startEventBufferRequest( std::string(reinterpret_cast(data), size)); if (result != returnvalue::OK) { return result; diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index 186eef6b..24f436aa 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -1,7 +1,9 @@ #include "PlocSupvHelper.h" #include +#include // Contains file controls like O_RDWR #include +#include #include #include @@ -15,11 +17,14 @@ #include "fsfw/tasks/TaskFactory.h" #include "fsfw/timemanager/Countdown.h" +#include "fsfw_hal/linux/uart/helper.h" #include "mission/utility/Filenaming.h" #include "mission/utility/ProgressPrinter.h" #include "mission/utility/Timestamp.h" -PlocSupvHelper::PlocSupvHelper(object_id_t objectId) : SystemObject(objectId) { +using namespace returnvalue; + +PlocSupvHelper::PlocSupvHelper(object_id_t objectId) : SystemObject(objectId), ringBuf(4096, true) { spParams.maxSize = sizeof(commandBuffer); resetSpParams(); } @@ -187,7 +192,7 @@ void PlocSupvHelper::initiateUpdateContinuation() { semaphore.release(); } -ReturnValue_t PlocSupvHelper::startEventbBufferRequest(std::string path) { +ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) { #ifdef XIPHOS_Q7S ReturnValue_t result = FilesystemHelper::checkPath(path); if (result != returnvalue::OK) { @@ -801,3 +806,58 @@ ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reade } void PlocSupvHelper::resetSpParams() { spParams.buf = commandBuffer; } + +ReturnValue_t PlocSupvHelper::initializeInterface(CookieIF* cookie) { + UartCookie* uartCookie = dynamic_cast(cookie); + if (uartCookie == nullptr) { + return FAILED; + } + std::string devname = uartCookie->getDeviceFile(); + /* Get file descriptor */ + serialPort = open(devname.c_str(), O_RDWR); + if (serialPort < 0) { + sif::warning << "ScexUartReader::initializeInterface: open call failed with error [" << errno + << ", " << strerror(errno) << std::endl; + return FAILED; + } + // Setting up UART parameters + tty.c_cflag &= ~PARENB; // Clear parity bit + uart::setParity(tty, uartCookie->getParity()); + uart::setStopbits(tty, uartCookie->getStopBits()); + uart::setBitsPerWord(tty, BitsPerWord::BITS_8); + tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control + uart::enableRead(tty); + uart::ignoreCtrlLines(tty); + + // Use non-canonical mode and clear echo flag + tty.c_lflag &= ~(ICANON | ECHO); + + // Non-blocking mode, 0.5 seconds timeout + tty.c_cc[VTIME] = 5; + tty.c_cc[VMIN] = 0; + + uart::setBaudrate(tty, uartCookie->getBaudrate()); + if (tcsetattr(serialPort, TCSANOW, &tty) != 0) { + sif::warning << "ScexUartReader::initializeInterface: tcsetattr call failed with error [" + << errno << ", " << strerror(errno) << std::endl; + } + // Flush received and unread data + tcflush(serialPort, TCIOFLUSH); + return OK; +} + +ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendData, + size_t sendLen) { + return returnvalue::OK; +} + +ReturnValue_t PlocSupvHelper::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } + +ReturnValue_t PlocSupvHelper::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { + return returnvalue::OK; +} + +ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, + size_t* size) { + return returnvalue::OK; +} diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index 30152e65..4e1c4c17 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -1,6 +1,9 @@ #ifndef BSP_Q7S_DEVICES_PLOCSUPVHELPER_H_ #define BSP_Q7S_DEVICES_PLOCSUPVHELPER_H_ +#include +#include + #include #include "OBSWConfig.h" @@ -21,7 +24,9 @@ * the supervisor and the OBC. * @author J. Meier */ -class PlocSupvHelper : public SystemObject, public ExecutableObjectIF { +class PlocSupvHelper : public DeviceCommunicationIF, + public SystemObject, + public ExecutableObjectIF { public: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_SUPV_HELPER; @@ -131,7 +136,7 @@ class PlocSupvHelper : public SystemObject, public ExecutableObjectIF { /** * @brief Calling this function will initiate the procedure to request the event buffer */ - ReturnValue_t startEventbBufferRequest(std::string path); + ReturnValue_t startEventBufferRequest(std::string path); /** * @brief Can be used to interrupt a running data transfer. @@ -181,6 +186,9 @@ class PlocSupvHelper : public SystemObject, public ExecutableObjectIF { struct Update update; + int serialPort = 0; + struct termios tty = {}; + struct EventBufferRequest { std::string path = ""; // Default name of file where event buffer data will be written to. Timestamp will be added to @@ -206,6 +214,7 @@ class PlocSupvHelper : public SystemObject, public ExecutableObjectIF { bool terminate = false; + SimpleRingBuffer ringBuf; /** * Communication interface responsible for data transactions between OBC and Supervisor. */ @@ -272,6 +281,58 @@ class PlocSupvHelper : public SystemObject, public ExecutableObjectIF { ReturnValue_t handleRemainingExeReport(ploc::SpTmReader& reader); void resetSpParams(); + + /** + * @brief Device specific initialization, using the cookie. + * @details + * The cookie is already prepared in the factory. If the communication + * interface needs to be set up in some way and requires cookie information, + * this can be performed in this function, which is called on device handler + * initialization. + * @param cookie + * @return + * - @c returnvalue::OK if initialization was successfull + * - Everything else triggers failure event with returnvalue as parameter 1 + */ + ReturnValue_t initializeInterface(CookieIF* cookie) override; + + /** + * Called by DHB in the SEND_WRITE doSendWrite(). + * This function is used to send data to the physical device + * by implementing and calling related drivers or wrapper functions. + * @param cookie + * @param data + * @param len If this is 0, nothing shall be sent. + * @return + * - @c returnvalue::OK for successfull send + * - Everything else triggers failure event with returnvalue as parameter 1 + */ + ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; + /** + * Called by DHB in the GET_WRITE doGetWrite(). + * Get send confirmation that the data in sendMessage() was sent successfully. + * @param cookie + * @return + * - @c returnvalue::OK if data was sent successfully but a reply is expected + * - NO_REPLY_EXPECTED if data was sent successfully and no reply is expected + * - Everything else to indicate failure + */ + ReturnValue_t getSendSuccess(CookieIF* cookie) override; + /** + * Called by DHB in the SEND_WRITE doSendRead(). + * It is assumed that it is always possible to request a reply + * from a device. If a requestLen of 0 is supplied, no reply was enabled + * and communication specific action should be taken (e.g. read nothing + * or read everything). + * + * @param cookie + * @param requestLen Size of data to read + * @return - @c returnvalue::OK to confirm the request for data has been sent. + * - Everything else triggers failure event with + * returnvalue as parameter 1 + */ + ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; + ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; }; #endif /* BSP_Q7S_DEVICES_PLOCSUPVHELPER_H_ */ From 826f4ce29c22a000da949d9a3efa3b6c5dd4f4d2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 11:16:22 +0100 Subject: [PATCH 084/244] add lock, use generic sem interface --- linux/devices/ploc/PlocSupvHelper.cpp | 16 ++++++++++------ linux/devices/ploc/PlocSupvHelper.h | 3 ++- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index 24f436aa..a7245cc1 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -3,6 +3,7 @@ #include #include // Contains file controls like O_RDWR #include +#include #include #include @@ -27,6 +28,9 @@ using namespace returnvalue; PlocSupvHelper::PlocSupvHelper(object_id_t objectId) : SystemObject(objectId), ringBuf(4096, true) { spParams.maxSize = sizeof(commandBuffer); resetSpParams(); + semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); + semaphore->acquire(); + lock = MutexFactory::instance()->createMutex(); } PlocSupvHelper::~PlocSupvHelper() {} @@ -44,11 +48,11 @@ ReturnValue_t PlocSupvHelper::initialize() { ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { ReturnValue_t result = returnvalue::OK; - semaphore.acquire(); + semaphore->acquire(); while (true) { switch (internalState) { case InternalState::IDLE: { - semaphore.acquire(); + semaphore->acquire(); break; } case InternalState::UPDATE: { @@ -164,7 +168,7 @@ ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { update.sequenceCount = params.seqCount; internalState = InternalState::UPDATE; uartComIF->flushUartTxAndRxBuf(comCookie); - semaphore.release(); + semaphore->release(); return result; } @@ -183,13 +187,13 @@ ReturnValue_t PlocSupvHelper::performMemCheck(uint8_t memoryId, uint32_t startAd update.crcShouldBeChecked = checkCrc; internalState = InternalState::CHECK_MEMORY; uartComIF->flushUartTxAndRxBuf(comCookie); - semaphore.release(); + semaphore->release(); return returnvalue::OK; } void PlocSupvHelper::initiateUpdateContinuation() { internalState = InternalState::CONTINUE_UPDATE; - semaphore.release(); + semaphore->release(); } ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) { @@ -205,7 +209,7 @@ ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) { eventBufferReq.path = path; internalState = InternalState::REQUEST_EVENT_BUFFER; uartComIF->flushUartTxAndRxBuf(comCookie); - semaphore.release(); + semaphore->release(); return returnvalue::OK; } diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index 4e1c4c17..809dfd5b 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -186,6 +186,8 @@ class PlocSupvHelper : public DeviceCommunicationIF, struct Update update; + SemaphoreIF* semaphore; + MutexIF* lock; int serialPort = 0; struct termios tty = {}; @@ -202,7 +204,6 @@ class PlocSupvHelper : public DeviceCommunicationIF, InternalState internalState = InternalState::IDLE; - BinarySemaphore semaphore; #ifdef XIPHOS_Q7S SdCardManager* sdcMan = nullptr; #endif From d254331b8e5aad89f9708c532ea7d4b7e7d443d5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 11:34:33 +0100 Subject: [PATCH 085/244] add basic manual UART reading --- linux/devices/ploc/PlocSupvHelper.cpp | 135 +++++++++++++++++--------- linux/devices/ploc/PlocSupvHelper.h | 12 ++- 2 files changed, 100 insertions(+), 47 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index a7245cc1..ac9a3cd3 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -33,7 +33,7 @@ PlocSupvHelper::PlocSupvHelper(object_id_t objectId) : SystemObject(objectId), r lock = MutexFactory::instance()->createMutex(); } -PlocSupvHelper::~PlocSupvHelper() {} +PlocSupvHelper::~PlocSupvHelper() = default; ReturnValue_t PlocSupvHelper::initialize() { #ifdef XIPHOS_Q7S @@ -47,59 +47,102 @@ ReturnValue_t PlocSupvHelper::initialize() { } ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { - ReturnValue_t result = returnvalue::OK; - semaphore->acquire(); + ReturnValue_t result; + lock->lockMutex(); + internalState = InternalState::IDLE; + lock->unlockMutex(); while (true) { - switch (internalState) { - case InternalState::IDLE: { - semaphore->acquire(); - break; - } - case InternalState::UPDATE: { - result = executeUpdate(); - if (result == returnvalue::OK) { - triggerEvent(SUPV_UPDATE_SUCCESSFUL, result); - } else if (result == PROCESS_TERMINATED) { - // Event already triggered - } else { - triggerEvent(SUPV_UPDATE_FAILED, result); + semaphore->acquire(); + int bytesRead = 0; + while (true) { + bytesRead = read(serialPort, reinterpret_cast(recBuf.data()), + static_cast(recBuf.size())); + if (bytesRead == 0) { + { + MutexGuard mg(lock); + if (internalState == InternalState::FINISH) { + // Flush received and unread data + tcflush(serialPort, TCIOFLUSH); + internalState = InternalState::IDLE; + break; + } } - internalState = InternalState::IDLE; + } else if (bytesRead < 0) { + sif::warning << "ScexUartReader::performOperation: read call failed with error [" << errno + << ", " << strerror(errno) << "]" << std::endl; break; - } - case InternalState::CHECK_MEMORY: { - executeFullCheckMemoryCommand(); - internalState = InternalState::IDLE; - break; - } - case InternalState::CONTINUE_UPDATE: { - result = continueUpdate(); - if (result == returnvalue::OK) { - triggerEvent(SUPV_CONTINUE_UPDATE_SUCCESSFUL, result); - } else if (result == PROCESS_TERMINATED) { - // Event already triggered - } else { - triggerEvent(SUPV_CONTINUE_UPDATE_FAILED, result); + } else if (bytesRead >= static_cast(recBuf.size())) { + sif::error << "ScexUartReader::performOperation: Receive buffer too small for " << bytesRead + << " bytes" << std::endl; + } else if (bytesRead > 0) { + if (debugMode) { + sif::info << "Received " << bytesRead + << " bytes from the Solar Cell Experiment:" << std::endl; } - internalState = InternalState::IDLE; - break; + // insert buffer into ring buffer here + // ReturnValue_t result = dleParser.passData(recBuf.data(), bytesRead); + // TODO: Parse ring buffer here } - case InternalState::REQUEST_EVENT_BUFFER: { - result = performEventBufferRequest(); - if (result == returnvalue::OK) { - triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result); - } else if (result == PROCESS_TERMINATED) { - // Event already triggered + lock->lockMutex(); + InternalState currentState = internalState; + lock->unlockMutex(); + switch (currentState) { + case InternalState::IDLE: { break; - } else { - triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result); } - internalState = InternalState::IDLE; - break; + case InternalState::UPDATE: { + result = executeUpdate(); + if (result == returnvalue::OK) { + triggerEvent(SUPV_UPDATE_SUCCESSFUL, result); + } else if (result == PROCESS_TERMINATED) { + // Event already triggered + } else { + triggerEvent(SUPV_UPDATE_FAILED, result); + } + MutexGuard mg(lock); + internalState = InternalState::IDLE; + break; + } + case InternalState::CHECK_MEMORY: { + executeFullCheckMemoryCommand(); + MutexGuard mg(lock); + internalState = InternalState::IDLE; + break; + } + case InternalState::CONTINUE_UPDATE: { + result = continueUpdate(); + if (result == returnvalue::OK) { + triggerEvent(SUPV_CONTINUE_UPDATE_SUCCESSFUL, result); + } else if (result == PROCESS_TERMINATED) { + // Event already triggered + } else { + triggerEvent(SUPV_CONTINUE_UPDATE_FAILED, result); + } + MutexGuard mg(lock); + internalState = InternalState::IDLE; + break; + } + case InternalState::REQUEST_EVENT_BUFFER: { + result = performEventBufferRequest(); + if (result == returnvalue::OK) { + triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result); + } else if (result == PROCESS_TERMINATED) { + // Event already triggered + break; + } else { + triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result); + } + MutexGuard mg(lock); + internalState = InternalState::IDLE; + break; + } + case InternalState::HANDLER_DRIVEN: { + continue; + } + default: + sif::debug << "PlocSupvHelper::performOperation: Invalid state" << std::endl; + break; } - default: - sif::debug << "PlocSupvHelper::performOperation: Invalid state" << std::endl; - break; } } } diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index 809dfd5b..a54b50a6 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -200,13 +200,22 @@ class PlocSupvHelper : public DeviceCommunicationIF, EventBufferRequest eventBufferReq; - enum class InternalState { IDLE, UPDATE, CONTINUE_UPDATE, REQUEST_EVENT_BUFFER, CHECK_MEMORY }; + enum class InternalState { + IDLE, + HANDLER_DRIVEN, + UPDATE, + CONTINUE_UPDATE, + REQUEST_EVENT_BUFFER, + CHECK_MEMORY, + FINISH + }; InternalState internalState = InternalState::IDLE; #ifdef XIPHOS_Q7S SdCardManager* sdcMan = nullptr; #endif + std::array recBuf = {}; uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]{}; SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); @@ -214,6 +223,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, std::array tmBuf{}; bool terminate = false; + bool debugMode = false; SimpleRingBuffer ringBuf; /** From 6350dbe0e9a428b7bab152599d936a39a0cd23bf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 11:38:21 +0100 Subject: [PATCH 086/244] now its getting tricky --- linux/devices/ploc/PlocSupvHelper.cpp | 4 +++- linux/devices/ploc/PlocSupvHelper.h | 3 +-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index ac9a3cd3..2f1b874d 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -25,7 +25,8 @@ using namespace returnvalue; -PlocSupvHelper::PlocSupvHelper(object_id_t objectId) : SystemObject(objectId), ringBuf(4096, true) { +PlocSupvHelper::PlocSupvHelper(object_id_t objectId) + : SystemObject(objectId), recRingBuf(4096, true) { spParams.maxSize = sizeof(commandBuffer); resetSpParams(); semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); @@ -79,6 +80,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { sif::info << "Received " << bytesRead << " bytes from the Solar Cell Experiment:" << std::endl; } + recRingBuf.writeData(recBuf.data(), bytesRead); // insert buffer into ring buffer here // ReturnValue_t result = dleParser.passData(recBuf.data(), bytesRead); // TODO: Parse ring buffer here diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index a54b50a6..c0edc4bb 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -216,6 +216,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, SdCardManager* sdcMan = nullptr; #endif std::array recBuf = {}; + SimpleRingBuffer recRingBuf; uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]{}; SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); @@ -224,8 +225,6 @@ class PlocSupvHelper : public DeviceCommunicationIF, bool terminate = false; bool debugMode = false; - - SimpleRingBuffer ringBuf; /** * Communication interface responsible for data transactions between OBC and Supervisor. */ From 45c95e559e2376ba9a3e41c71ab72e482bcdc728 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 11:53:07 +0100 Subject: [PATCH 087/244] started HDLC ring buffer decoder --- linux/devices/ploc/PlocSupvHelper.cpp | 152 +++++++++++++++++++------- linux/devices/ploc/PlocSupvHelper.h | 28 +++-- 2 files changed, 131 insertions(+), 49 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index 2f1b874d..33cf32fe 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -36,6 +36,45 @@ PlocSupvHelper::PlocSupvHelper(object_id_t objectId) PlocSupvHelper::~PlocSupvHelper() = default; +ReturnValue_t PlocSupvHelper::initializeInterface(CookieIF* cookie) { + UartCookie* uartCookie = dynamic_cast(cookie); + if (uartCookie == nullptr) { + return FAILED; + } + std::string devname = uartCookie->getDeviceFile(); + /* Get file descriptor */ + serialPort = open(devname.c_str(), O_RDWR); + if (serialPort < 0) { + sif::warning << "ScexUartReader::initializeInterface: open call failed with error [" << errno + << ", " << strerror(errno) << std::endl; + return FAILED; + } + // Setting up UART parameters + tty.c_cflag &= ~PARENB; // Clear parity bit + uart::setParity(tty, uartCookie->getParity()); + uart::setStopbits(tty, uartCookie->getStopBits()); + uart::setBitsPerWord(tty, BitsPerWord::BITS_8); + tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control + uart::enableRead(tty); + uart::ignoreCtrlLines(tty); + + // Use non-canonical mode and clear echo flag + tty.c_lflag &= ~(ICANON | ECHO); + + // Non-blocking mode, 0.5 seconds timeout + tty.c_cc[VTIME] = 5; + tty.c_cc[VMIN] = 0; + + uart::setBaudrate(tty, uartCookie->getBaudrate()); + if (tcsetattr(serialPort, TCSANOW, &tty) != 0) { + sif::warning << "ScexUartReader::initializeInterface: tcsetattr call failed with error [" + << errno << ", " << strerror(errno) << std::endl; + } + // Flush received and unread data + tcflush(serialPort, TCIOFLUSH); + return OK; +} + ReturnValue_t PlocSupvHelper::initialize() { #ifdef XIPHOS_Q7S sdcMan = SdCardManager::instance(); @@ -856,45 +895,6 @@ ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reade void PlocSupvHelper::resetSpParams() { spParams.buf = commandBuffer; } -ReturnValue_t PlocSupvHelper::initializeInterface(CookieIF* cookie) { - UartCookie* uartCookie = dynamic_cast(cookie); - if (uartCookie == nullptr) { - return FAILED; - } - std::string devname = uartCookie->getDeviceFile(); - /* Get file descriptor */ - serialPort = open(devname.c_str(), O_RDWR); - if (serialPort < 0) { - sif::warning << "ScexUartReader::initializeInterface: open call failed with error [" << errno - << ", " << strerror(errno) << std::endl; - return FAILED; - } - // Setting up UART parameters - tty.c_cflag &= ~PARENB; // Clear parity bit - uart::setParity(tty, uartCookie->getParity()); - uart::setStopbits(tty, uartCookie->getStopBits()); - uart::setBitsPerWord(tty, BitsPerWord::BITS_8); - tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control - uart::enableRead(tty); - uart::ignoreCtrlLines(tty); - - // Use non-canonical mode and clear echo flag - tty.c_lflag &= ~(ICANON | ECHO); - - // Non-blocking mode, 0.5 seconds timeout - tty.c_cc[VTIME] = 5; - tty.c_cc[VMIN] = 0; - - uart::setBaudrate(tty, uartCookie->getBaudrate()); - if (tcsetattr(serialPort, TCSANOW, &tty) != 0) { - sif::warning << "ScexUartReader::initializeInterface: tcsetattr call failed with error [" - << errno << ", " << strerror(errno) << std::endl; - } - // Flush received and unread data - tcflush(serialPort, TCIOFLUSH); - return OK; -} - ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { return returnvalue::OK; @@ -910,3 +910,77 @@ ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** bu size_t* size) { return returnvalue::OK; } + +ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc() { + size_t availableData = recRingBuf.getAvailableReadData(); + if (availableData == 0) { + return NO_PACKET_FOUND; + } + if(availableData > encodedBuf.size()) { + return DECODE_BUF_TOO_SMALL; + } + ReturnValue_t result = recRingBuf.readData(encodedBuf.data(), availableData); + if(result != returnvalue::OK) { + return result; + } + bool startMarkerFound = false; + size_t startIdx = 0; + return returnvalue::OK; + // if (result != returnvalue::OK) { + // ErrorInfo info; + // info.res = result; + // setErrorContext(ErrorTypes::RING_BUF_ERROR, info); + // return result; + // } + // bool stxFound = false; + // size_t stxIdx = 0; + // for (size_t vectorIdx = 0; vectorIdx < availableData; vectorIdx++) { + // // handle STX char + // if (encodedBuf.first[vectorIdx] == DleEncoder::STX_CHAR) { + // if (not stxFound) { + // stxFound = true; + // stxIdx = vectorIdx; + // } else { + // // might be lost packet, so we should advance the read pointer + // // without skipping the STX + // readSize = vectorIdx; + // ErrorInfo info; + // setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); + // return POSSIBLE_PACKET_LOSS; + // } + // } + // // handle ETX char + // if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) { + // if (stxFound) { + // // This is propably a packet, so we decode it. + // size_t decodedLen = 0; + // size_t dummy = 0; + // + // ReturnValue_t result = + // decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy, + // decodedBuf.first, decodedBuf.second, + //&decodedLen); if (result == returnvalue::OK) { ctx.setType(ContextType::PACKET_FOUND); + // ctx.decodedPacket.first = decodedBuf.first; + // ctx.decodedPacket.second = decodedLen; + // readSize = ++vectorIdx; + // return returnvalue::OK; + // } else { + // // invalid packet, skip. + // readSize = ++vectorIdx; + // ErrorInfo info; + // info.res = result; + // setErrorContext(ErrorTypes::DECODE_ERROR, info); + // return POSSIBLE_PACKET_LOSS; + // } + // } else { + // // might be lost packet, so we should advance the read pointer + // readSize = ++vectorIdx; + // ErrorInfo info; + // info.len = 0; + // setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); + // return POSSIBLE_PACKET_LOSS; + // } + // } + // } + // return NO_PACKET_FOUND; +} diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index c0edc4bb..8c4bb8e2 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -28,6 +28,17 @@ class PlocSupvHelper : public DeviceCommunicationIF, public SystemObject, public ExecutableObjectIF { public: + static const uint8_t INTERFACE_ID = CLASS_ID::PLOC_SUPV_HELPER; + + //! [EXPORT] : [COMMENT] File accidentally close + static const ReturnValue_t FILE_CLOSED_ACCIDENTALLY = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] Process has been terminated by command + static const ReturnValue_t PROCESS_TERMINATED = MAKE_RETURN_CODE(0xA1); + //! [EXPORT] : [COMMENT] Received command with invalid pathname + static const ReturnValue_t PATH_NOT_EXISTS = MAKE_RETURN_CODE(0xA2); + //! [EXPORT] : [COMMENT] Expected event buffer TM but received space packet with other APID + static const ReturnValue_t EVENT_BUFFER_REPLY_INVALID_APID = MAKE_RETURN_CODE(0xA3); + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_SUPV_HELPER; //! [EXPORT] : [COMMENT] update failed @@ -146,17 +157,9 @@ class PlocSupvHelper : public DeviceCommunicationIF, static uint32_t buildProgParams1(uint8_t percent, uint16_t seqCount); private: - static const uint8_t INTERFACE_ID = CLASS_ID::PLOC_SUPV_HELPER; - - //! [EXPORT] : [COMMENT] File accidentally close - static const ReturnValue_t FILE_CLOSED_ACCIDENTALLY = MAKE_RETURN_CODE(0xA0); - //! [EXPORT] : [COMMENT] Process has been terminated by command - static const ReturnValue_t PROCESS_TERMINATED = MAKE_RETURN_CODE(0xA1); - //! [EXPORT] : [COMMENT] Received command with invalid pathname - static const ReturnValue_t PATH_NOT_EXISTS = MAKE_RETURN_CODE(0xA2); - //! [EXPORT] : [COMMENT] Expected event buffer TM but received space packet with other APID - static const ReturnValue_t EVENT_BUFFER_REPLY_INVALID_APID = MAKE_RETURN_CODE(0xA3); + static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 0); + static constexpr ReturnValue_t DECODE_BUF_TOO_SMALL = returnvalue::makeCode(1, 1); static const uint16_t CRC16_INIT = 0xFFFF; // Event buffer reply will carry 24 space packets with 1016 bytes and one space packet with // 192 bytes @@ -166,6 +169,9 @@ class PlocSupvHelper : public DeviceCommunicationIF, static const uint32_t CRC_EXECUTION_TIMEOUT = 60000; static const uint32_t PREPARE_UPDATE_EXECUTION_REPORT = 2000; + static constexpr uint8_t HDLC_START_MARKER = 0x7C; + static constexpr uint8_t HDLC_END_MARKER = 0x7E; + struct Update { uint8_t memoryId; uint32_t startAddress; @@ -216,6 +222,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, SdCardManager* sdcMan = nullptr; #endif std::array recBuf = {}; + std::array encodedBuf = {}; SimpleRingBuffer recRingBuf; uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]{}; SpacePacketCreator creator; @@ -239,6 +246,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, void executeFullCheckMemoryCommand(); + ReturnValue_t parseRecRingBufForHdlc(); ReturnValue_t executeUpdate(); ReturnValue_t continueUpdate(); ReturnValue_t updateOperation(); From af0853a42b86d1cb7e3da5e1dc3d5404d17fc152 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 12:04:47 +0100 Subject: [PATCH 088/244] continue HDLC parsing --- linux/devices/ploc/PlocSupvHelper.cpp | 57 ++++++++++++--------------- linux/devices/ploc/PlocSupvHelper.h | 25 ++++++------ 2 files changed, 40 insertions(+), 42 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index 33cf32fe..5b0c9a7c 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -11,6 +11,7 @@ #include #include "OBSWConfig.h" +#include "tas/hdlc.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/FilesystemHelper.h" #include "bsp_q7s/fs/SdCardManager.h" @@ -911,44 +912,38 @@ ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** bu return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc() { +ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { size_t availableData = recRingBuf.getAvailableReadData(); if (availableData == 0) { - return NO_PACKET_FOUND; + return NO_PACKET_FOUND; } - if(availableData > encodedBuf.size()) { - return DECODE_BUF_TOO_SMALL; + if (availableData > encodedBuf.size()) { + return DECODE_BUF_TOO_SMALL; } ReturnValue_t result = recRingBuf.readData(encodedBuf.data(), availableData); - if(result != returnvalue::OK) { - return result; + if (result != returnvalue::OK) { + return result; } bool startMarkerFound = false; size_t startIdx = 0; return returnvalue::OK; - // if (result != returnvalue::OK) { - // ErrorInfo info; - // info.res = result; - // setErrorContext(ErrorTypes::RING_BUF_ERROR, info); - // return result; - // } - // bool stxFound = false; - // size_t stxIdx = 0; - // for (size_t vectorIdx = 0; vectorIdx < availableData; vectorIdx++) { - // // handle STX char - // if (encodedBuf.first[vectorIdx] == DleEncoder::STX_CHAR) { - // if (not stxFound) { - // stxFound = true; - // stxIdx = vectorIdx; - // } else { - // // might be lost packet, so we should advance the read pointer - // // without skipping the STX - // readSize = vectorIdx; - // ErrorInfo info; - // setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); - // return POSSIBLE_PACKET_LOSS; - // } - // } + for (size_t idx = 0; idx < availableData; idx++) { + // handle start marker + if (encodedBuf[idx] == HDLC_START_MARKER) { + if (not startMarkerFound) { + startMarkerFound = true; + startIdx = idx; + } else { + readSize = idx; + return POSSIBLE_PACKET_LOSS_CONSECUTIVE_START; + } + } + if (encodedBuf[idx] == HDLC_END_MARKER) { + if (startMarkerFound) { + // Probably a packet, so decode it + } + } + } // // handle ETX char // if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) { // if (stxFound) { @@ -959,8 +954,8 @@ ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc() { // ReturnValue_t result = // decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy, // decodedBuf.first, decodedBuf.second, - //&decodedLen); if (result == returnvalue::OK) { ctx.setType(ContextType::PACKET_FOUND); - // ctx.decodedPacket.first = decodedBuf.first; + //&decodedLen); if (result == returnvalue::OK) { + //ctx.setType(ContextType::PACKET_FOUND); ctx.decodedPacket.first = decodedBuf.first; // ctx.decodedPacket.second = decodedLen; // readSize = ++vectorIdx; // return returnvalue::OK; diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index 8c4bb8e2..38f7869b 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -28,16 +28,16 @@ class PlocSupvHelper : public DeviceCommunicationIF, public SystemObject, public ExecutableObjectIF { public: - static const uint8_t INTERFACE_ID = CLASS_ID::PLOC_SUPV_HELPER; + static const uint8_t INTERFACE_ID = CLASS_ID::PLOC_SUPV_HELPER; - //! [EXPORT] : [COMMENT] File accidentally close - static const ReturnValue_t FILE_CLOSED_ACCIDENTALLY = MAKE_RETURN_CODE(0xA0); - //! [EXPORT] : [COMMENT] Process has been terminated by command - static const ReturnValue_t PROCESS_TERMINATED = MAKE_RETURN_CODE(0xA1); - //! [EXPORT] : [COMMENT] Received command with invalid pathname - static const ReturnValue_t PATH_NOT_EXISTS = MAKE_RETURN_CODE(0xA2); - //! [EXPORT] : [COMMENT] Expected event buffer TM but received space packet with other APID - static const ReturnValue_t EVENT_BUFFER_REPLY_INVALID_APID = MAKE_RETURN_CODE(0xA3); + //! [EXPORT] : [COMMENT] File accidentally close + static const ReturnValue_t FILE_CLOSED_ACCIDENTALLY = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] Process has been terminated by command + static const ReturnValue_t PROCESS_TERMINATED = MAKE_RETURN_CODE(0xA1); + //! [EXPORT] : [COMMENT] Received command with invalid pathname + static const ReturnValue_t PATH_NOT_EXISTS = MAKE_RETURN_CODE(0xA2); + //! [EXPORT] : [COMMENT] Expected event buffer TM but received space packet with other APID + static const ReturnValue_t EVENT_BUFFER_REPLY_INVALID_APID = MAKE_RETURN_CODE(0xA3); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_SUPV_HELPER; @@ -157,9 +157,12 @@ class PlocSupvHelper : public DeviceCommunicationIF, static uint32_t buildProgParams1(uint8_t percent, uint16_t seqCount); private: - static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 0); static constexpr ReturnValue_t DECODE_BUF_TOO_SMALL = returnvalue::makeCode(1, 1); + static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS_CONSECUTIVE_START = + returnvalue::makeCode(1, 2); + static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS_CONSECUTIVE_END = returnvalue::makeCode(1, 3); + static const uint16_t CRC16_INIT = 0xFFFF; // Event buffer reply will carry 24 space packets with 1016 bytes and one space packet with // 192 bytes @@ -246,7 +249,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, void executeFullCheckMemoryCommand(); - ReturnValue_t parseRecRingBufForHdlc(); + ReturnValue_t parseRecRingBufForHdlc(size_t& readSize); ReturnValue_t executeUpdate(); ReturnValue_t continueUpdate(); ReturnValue_t updateOperation(); From b5cd873f6d97ee275429a3eebeebeba890f55e04 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 12:38:30 +0100 Subject: [PATCH 089/244] completed ring buffer parser --- linux/devices/ploc/PlocSupvHelper.cpp | 42 +++++---------------------- linux/devices/ploc/PlocSupvHelper.h | 1 + thirdparty/tas/crc.c | 2 +- thirdparty/tas/hdlc.c | 7 +++-- thirdparty/tas/tas/crc.h | 2 +- thirdparty/tas/tas/hdlc.h | 15 ++++++++-- 6 files changed, 28 insertions(+), 41 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index 5b0c9a7c..f4d25707 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -941,41 +941,15 @@ ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { if (encodedBuf[idx] == HDLC_END_MARKER) { if (startMarkerFound) { // Probably a packet, so decode it + size_t decodedLen = 0; + hdlc_remove_framing(encodedBuf.data() + startIdx, idx + 1, decodedBuf.data(), &decodedLen); + readSize = decodedLen; + return returnvalue::OK; + } else { + readSize = ++idx; + return POSSIBLE_PACKET_LOSS_CONSECUTIVE_END; } } } - // // handle ETX char - // if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) { - // if (stxFound) { - // // This is propably a packet, so we decode it. - // size_t decodedLen = 0; - // size_t dummy = 0; - // - // ReturnValue_t result = - // decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy, - // decodedBuf.first, decodedBuf.second, - //&decodedLen); if (result == returnvalue::OK) { - //ctx.setType(ContextType::PACKET_FOUND); ctx.decodedPacket.first = decodedBuf.first; - // ctx.decodedPacket.second = decodedLen; - // readSize = ++vectorIdx; - // return returnvalue::OK; - // } else { - // // invalid packet, skip. - // readSize = ++vectorIdx; - // ErrorInfo info; - // info.res = result; - // setErrorContext(ErrorTypes::DECODE_ERROR, info); - // return POSSIBLE_PACKET_LOSS; - // } - // } else { - // // might be lost packet, so we should advance the read pointer - // readSize = ++vectorIdx; - // ErrorInfo info; - // info.len = 0; - // setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); - // return POSSIBLE_PACKET_LOSS; - // } - // } - // } - // return NO_PACKET_FOUND; + return NO_PACKET_FOUND; } diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index 38f7869b..b07e120c 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -226,6 +226,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, #endif std::array recBuf = {}; std::array encodedBuf = {}; + std::array decodedBuf = {}; SimpleRingBuffer recRingBuf; uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]{}; SpacePacketCreator creator; diff --git a/thirdparty/tas/crc.c b/thirdparty/tas/crc.c index 15d152b9..5a50a3a8 100644 --- a/thirdparty/tas/crc.c +++ b/thirdparty/tas/crc.c @@ -181,7 +181,7 @@ void calc_crc16_byte_reflected(uint16_t *crc16, uint8_t bt) } // initial: 0xFFFF, xorOut: 0xFFFF, RefIn: true, RefOut: true, polynomial: 0x1021 -uint16_t calc_crc16_buff_reflected(uint8_t *data, uint16_t len) +uint16_t calc_crc16_buff_reflected(const uint8_t *data, uint16_t len) { uint16_t crc16 = 0xFFFF; diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 6debec9c..4734c0b9 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -9,10 +9,11 @@ ************************************************************************************** */ -#include #include "tas/hdlc.h" #include "tas/crc.h" +#include + static void hdlc_add_byte(uint8_t ch, uint8_t *buff, uint16_t *pos) { uint16_t templen = *pos; @@ -29,7 +30,7 @@ static void hdlc_add_byte(uint8_t ch, uint8_t *buff, uint16_t *pos) *pos = templen; } -void hdlc_add_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen) +void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) { uint16_t tlen = 0; uint16_t ii; @@ -54,7 +55,7 @@ void hdlc_add_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen) *dlen = tlen; } -void hdlc_remove_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen) +void hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) { uint16_t tlen = 0; uint16_t ii; diff --git a/thirdparty/tas/tas/crc.h b/thirdparty/tas/tas/crc.h index f92664b2..a9735402 100644 --- a/thirdparty/tas/tas/crc.h +++ b/thirdparty/tas/tas/crc.h @@ -103,5 +103,5 @@ void calc_crc16_byte_reflected(uint16_t *crc16, uint8_t bt); * \param final_xor The value that the final result will be xored * \return CRC result */ -uint16_t calc_crc16_buff_reflected(uint8_t *data, uint16_t len); +uint16_t calc_crc16_buff_reflected(const uint8_t *data, uint16_t len); #endif diff --git a/thirdparty/tas/tas/hdlc.h b/thirdparty/tas/tas/hdlc.h index a728c5c5..97e69500 100644 --- a/thirdparty/tas/tas/hdlc.h +++ b/thirdparty/tas/tas/hdlc.h @@ -12,6 +12,13 @@ #ifndef LIB_HDLC_H_ #define LIB_HDLC_H_ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + #define HDLC_ENABLE #define HDLC_START_BYTE (0x7Eu) @@ -19,8 +26,12 @@ #define HDLC_END_BYTE (0x7Cu) #define HDLC_ESCAPE_CHAR (0x20u) -void hdlc_add_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen); +void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); -void hdlc_remove_framing(uint8_t *src, uint16_t slen, uint8_t *dst, uint16_t *dlen); +void hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); + +#ifdef __cplusplus +} +#endif #endif /* LIB_HDLC_H_ */ From a4735defaaeba3a827cb69c245bf5b0f21f21b7b Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 4 Nov 2022 12:41:35 +0100 Subject: [PATCH 090/244] amended gyr sensor fusion --- mission/controller/acs/AcsParameters.h | 2 + mission/controller/acs/SensorProcessing.cpp | 65 +++++++-------------- 2 files changed, 23 insertions(+), 44 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f5b72252..813851dc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -762,6 +762,8 @@ 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; } gyrHandlingParameters; struct RwHandlingParameters { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index bb445485..483b94c2 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -480,80 +480,57 @@ void SensorProcessing::processGyr( // Transforming Values to the Body Frame (actually it is the geometry frame atm) double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0}, gyr3ValueBody[3] = {0, 0, 0}; + float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; if (gyr0valid) { const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; + } } if (gyr1valid) { const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; + } } if (gyr2valid) { const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; + } } if (gyr3valid) { const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; + } } /* -------- SatRateEst: Middle Value ------- */ // 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) { + if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) { double gyr02ValuesSum[3]; VectorOperations::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); VectorOperations::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); - } else if ((gyr0valid || gyr2valid) && !(gyr1valid || gyr3valid)) { - if (gyr0valid) { - std::memcpy(gyrVecTot, gyr0ValueBody, 3 * sizeof(double)); - } else if (gyr2valid) { - std::memcpy(gyrVecTot, gyr2ValueBody, 3 * sizeof(double)); + } else { + for (uint8_t i = 0; i < 3; i++) { + gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } - } else if (gyr1valid && gyr3valid) { - double gyr13ValuesSum[3]; - double gyr13ValuesMean[3]; - VectorOperations::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); - VectorOperations::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); - if (gyr0valid) { - gyrVecTot[0] = - ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) + - (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / - ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); - gyrVecTot[1] = - ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) + - (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / - ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); - gyrVecTot[2] = - ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) + - (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / - ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); - } else if (gyr2valid) { - gyrVecTot[0] = - ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) + - (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / - ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); - gyrVecTot[1] = - ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) + - (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / - ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); - gyrVecTot[2] = - ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) + - (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / - ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); - } else { - std::memcpy(gyrVecTot, gyr13ValuesMean, 3 * sizeof(double)); - } - } else if (gyr1valid) { - std::memcpy(gyrVecTot, gyr1ValueBody, 3 * sizeof(double)); - } else if (gyr3valid) { - std::memcpy(gyrVecTot, gyr3ValueBody, 3 * sizeof(double)); } { PoolReadGuard pg(gyrDataProcessed); From 74837753bfe05beee3e90a005730a958a9ad7699 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 12:45:20 +0100 Subject: [PATCH 091/244] add IPC ring buf and buffer --- linux/devices/ploc/PlocSupvHelper.cpp | 4 +++- linux/devices/ploc/PlocSupvHelper.h | 8 +++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index f4d25707..371da37d 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -27,7 +27,9 @@ using namespace returnvalue; PlocSupvHelper::PlocSupvHelper(object_id_t objectId) - : SystemObject(objectId), recRingBuf(4096, true) { + : SystemObject(objectId), + recRingBuf(4096, true), + ipcRingBuf(1200 * MAX_STORED_DECODED_PACKETS, true) { spParams.maxSize = sizeof(commandBuffer); resetSpParams(); semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index b07e120c..8a693460 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -7,6 +7,7 @@ #include #include "OBSWConfig.h" +#include "fsfw/container/FIFO.h" #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/osal/linux/BinarySemaphore.h" @@ -172,6 +173,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, static const uint32_t CRC_EXECUTION_TIMEOUT = 60000; static const uint32_t PREPARE_UPDATE_EXECUTION_REPORT = 2000; + static constexpr uint8_t MAX_STORED_DECODED_PACKETS = 4; static constexpr uint8_t HDLC_START_MARKER = 0x7C; static constexpr uint8_t HDLC_END_MARKER = 0x7E; @@ -224,10 +226,14 @@ class PlocSupvHelper : public DeviceCommunicationIF, #ifdef XIPHOS_Q7S SdCardManager* sdcMan = nullptr; #endif + SimpleRingBuffer recRingBuf; std::array recBuf = {}; std::array encodedBuf = {}; std::array decodedBuf = {}; - SimpleRingBuffer recRingBuf; + std::array ipcBuffer = {}; + SimpleRingBuffer ipcRingBuf; + FIFO ipcQueue; + uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]{}; SpacePacketCreator creator; ploc::SpTcParams spParams = ploc::SpTcParams(creator); From 5df45f9fa79802b63f52ba847575dd4566f13bcd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 12:51:01 +0100 Subject: [PATCH 092/244] start com IF function impl --- linux/devices/ploc/PlocSupvHelper.cpp | 60 ++++++++++++++++++++------- linux/devices/ploc/PlocSupvHelper.h | 10 ++--- 2 files changed, 49 insertions(+), 21 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index 371da37d..9dd53535 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -92,7 +92,7 @@ ReturnValue_t PlocSupvHelper::initialize() { ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { ReturnValue_t result; lock->lockMutex(); - internalState = InternalState::IDLE; + state = InternalState::IDLE; lock->unlockMutex(); while (true) { semaphore->acquire(); @@ -103,10 +103,10 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { if (bytesRead == 0) { { MutexGuard mg(lock); - if (internalState == InternalState::FINISH) { + if (state == InternalState::FINISH) { // Flush received and unread data tcflush(serialPort, TCIOFLUSH); - internalState = InternalState::IDLE; + state = InternalState::IDLE; break; } } @@ -128,7 +128,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { // TODO: Parse ring buffer here } lock->lockMutex(); - InternalState currentState = internalState; + InternalState currentState = state; lock->unlockMutex(); switch (currentState) { case InternalState::IDLE: { @@ -144,13 +144,13 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { triggerEvent(SUPV_UPDATE_FAILED, result); } MutexGuard mg(lock); - internalState = InternalState::IDLE; + state = InternalState::IDLE; break; } case InternalState::CHECK_MEMORY: { executeFullCheckMemoryCommand(); MutexGuard mg(lock); - internalState = InternalState::IDLE; + state = InternalState::IDLE; break; } case InternalState::CONTINUE_UPDATE: { @@ -163,7 +163,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { triggerEvent(SUPV_CONTINUE_UPDATE_FAILED, result); } MutexGuard mg(lock); - internalState = InternalState::IDLE; + state = InternalState::IDLE; break; } case InternalState::REQUEST_EVENT_BUFFER: { @@ -177,7 +177,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result); } MutexGuard mg(lock); - internalState = InternalState::IDLE; + state = InternalState::IDLE; break; } case InternalState::HANDLER_DRIVEN: { @@ -253,7 +253,7 @@ ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { update.packetNum = 1; update.deleteMemory = params.deleteMemory; update.sequenceCount = params.seqCount; - internalState = InternalState::UPDATE; + state = InternalState::UPDATE; uartComIF->flushUartTxAndRxBuf(comCookie); semaphore->release(); return result; @@ -272,14 +272,14 @@ ReturnValue_t PlocSupvHelper::performMemCheck(uint8_t memoryId, uint32_t startAd update.startAddress = startAddress; update.length = sizeToCheck; update.crcShouldBeChecked = checkCrc; - internalState = InternalState::CHECK_MEMORY; + state = InternalState::CHECK_MEMORY; uartComIF->flushUartTxAndRxBuf(comCookie); semaphore->release(); return returnvalue::OK; } void PlocSupvHelper::initiateUpdateContinuation() { - internalState = InternalState::CONTINUE_UPDATE; + state = InternalState::CONTINUE_UPDATE; semaphore->release(); } @@ -294,7 +294,7 @@ ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) { return PATH_NOT_EXISTS; } eventBufferReq.path = path; - internalState = InternalState::REQUEST_EVENT_BUFFER; + state = InternalState::REQUEST_EVENT_BUFFER; uartComIF->flushUartTxAndRxBuf(comCookie); semaphore->release(); return returnvalue::OK; @@ -584,7 +584,7 @@ ReturnValue_t PlocSupvHelper::sendCommand(ploc::SpTcBase& packet) { result = uartComIF->sendMessage(comCookie, packet.getFullPacket(), packet.getFullPacketLen()); if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::sendCommand: Failed to send command" << std::endl; - triggerEvent(SUPV_SENDING_COMMAND_FAILED, result, static_cast(internalState)); + triggerEvent(SUPV_SENDING_COMMAND_FAILED, result, static_cast(state)); return result; } return result; @@ -699,13 +699,13 @@ ReturnValue_t PlocSupvHelper::receive(uint8_t* data, size_t* readBytes, size_t r if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::receive: Failed to request reply" << std::endl; triggerEvent(SUPV_HELPER_REQUESTING_REPLY_FAILED, result, - static_cast(static_cast(internalState))); + static_cast(static_cast(state))); return returnvalue::FAILED; } result = uartComIF->readReceivedMessage(comCookie, &buffer, readBytes); if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::receive: Failed to read received message" << std::endl; - triggerEvent(SUPV_HELPER_READING_REPLY_FAILED, result, static_cast(internalState)); + triggerEvent(SUPV_HELPER_READING_REPLY_FAILED, result, static_cast(state)); return returnvalue::FAILED; } if (*readBytes > 0) { @@ -900,6 +900,23 @@ void PlocSupvHelper::resetSpParams() { spParams.buf = commandBuffer; } ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { + ReturnValue_t result; + if (sendData == nullptr or sendLen == 0) { + return FAILED; + } + lock->lockMutex(); + if (state != InternalState::IDLE) { + lock->unlockMutex(); + return FAILED; + } + tcflush(serialPort, TCIFLUSH); + state = InternalState::RUNNING; + lock->unlockMutex(); + + result = semaphore->release(); + if (result != OK) { + std::cout << "PlocSupvHelper::sendMessage: Releasing semaphore failed" << std::endl; + } return returnvalue::OK; } @@ -911,7 +928,18 @@ ReturnValue_t PlocSupvHelper::requestReceiveMessage(CookieIF* cookie, size_t req ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { - return returnvalue::OK; + MutexGuard mg(lock); + if (ipcQueue.empty()) { + *size = 0; + return OK; + } + ipcQueue.retrieve(size); + *buffer = ipcBuffer.data(); + ReturnValue_t result = ipcRingBuf.readData(ipcBuffer.data(), *size, true); + if (result != OK) { + sif::warning << "ScexUartReader::readReceivedMessage: Reading RingBuffer failed" << std::endl; + } + return OK; } ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index 8a693460..2e6db05b 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -211,17 +211,17 @@ class PlocSupvHelper : public DeviceCommunicationIF, EventBufferRequest eventBufferReq; - enum class InternalState { - IDLE, + enum class InternalState { IDLE, RUNNING, FINISH }; + + enum class Request { HANDLER_DRIVEN, UPDATE, CONTINUE_UPDATE, REQUEST_EVENT_BUFFER, CHECK_MEMORY, - FINISH }; - - InternalState internalState = InternalState::IDLE; + InternalState state = InternalState::IDLE; + Request request = Request::HANDLER_DRIVEN; #ifdef XIPHOS_Q7S SdCardManager* sdcMan = nullptr; From dc024e5385c2ee29a26db4f43c42899230791f5e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 13:40:42 +0100 Subject: [PATCH 093/244] low level impl complete --- linux/devices/ploc/PlocSupvHelper.cpp | 218 ++++++++++++++++---------- linux/devices/ploc/PlocSupvHelper.h | 6 + 2 files changed, 142 insertions(+), 82 deletions(-) diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvHelper.cpp index 9dd53535..51b6397e 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvHelper.cpp @@ -90,107 +90,76 @@ ReturnValue_t PlocSupvHelper::initialize() { } ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { - ReturnValue_t result; lock->lockMutex(); state = InternalState::IDLE; lock->unlockMutex(); + bool putTaskToSleep = false; while (true) { semaphore->acquire(); - int bytesRead = 0; while (true) { - bytesRead = read(serialPort, reinterpret_cast(recBuf.data()), - static_cast(recBuf.size())); - if (bytesRead == 0) { - { - MutexGuard mg(lock); - if (state == InternalState::FINISH) { - // Flush received and unread data - tcflush(serialPort, TCIOFLUSH); - state = InternalState::IDLE; - break; - } - } - } else if (bytesRead < 0) { - sif::warning << "ScexUartReader::performOperation: read call failed with error [" << errno - << ", " << strerror(errno) << "]" << std::endl; + putTaskToSleep = handleUartReception(); + if (putTaskToSleep) { break; - } else if (bytesRead >= static_cast(recBuf.size())) { - sif::error << "ScexUartReader::performOperation: Receive buffer too small for " << bytesRead - << " bytes" << std::endl; - } else if (bytesRead > 0) { - if (debugMode) { - sif::info << "Received " << bytesRead - << " bytes from the Solar Cell Experiment:" << std::endl; - } - recRingBuf.writeData(recBuf.data(), bytesRead); - // insert buffer into ring buffer here - // ReturnValue_t result = dleParser.passData(recBuf.data(), bytesRead); - // TODO: Parse ring buffer here } lock->lockMutex(); InternalState currentState = state; lock->unlockMutex(); switch (currentState) { case InternalState::IDLE: { + putTaskToSleep = true; break; } - case InternalState::UPDATE: { - result = executeUpdate(); - if (result == returnvalue::OK) { - triggerEvent(SUPV_UPDATE_SUCCESSFUL, result); - } else if (result == PROCESS_TERMINATED) { - // Event already triggered - } else { - triggerEvent(SUPV_UPDATE_FAILED, result); - } - MutexGuard mg(lock); + case InternalState::FINISH: { state = InternalState::IDLE; + putTaskToSleep = true; break; } - case InternalState::CHECK_MEMORY: { - executeFullCheckMemoryCommand(); - MutexGuard mg(lock); - state = InternalState::IDLE; + case InternalState::RUNNING: { + putTaskToSleep = handleRunningRequest(); break; } - case InternalState::CONTINUE_UPDATE: { - result = continueUpdate(); - if (result == returnvalue::OK) { - triggerEvent(SUPV_CONTINUE_UPDATE_SUCCESSFUL, result); - } else if (result == PROCESS_TERMINATED) { - // Event already triggered - } else { - triggerEvent(SUPV_CONTINUE_UPDATE_FAILED, result); - } - MutexGuard mg(lock); - state = InternalState::IDLE; - break; - } - case InternalState::REQUEST_EVENT_BUFFER: { - result = performEventBufferRequest(); - if (result == returnvalue::OK) { - triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result); - } else if (result == PROCESS_TERMINATED) { - // Event already triggered - break; - } else { - triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result); - } - MutexGuard mg(lock); - state = InternalState::IDLE; - break; - } - case InternalState::HANDLER_DRIVEN: { - continue; - } - default: - sif::debug << "PlocSupvHelper::performOperation: Invalid state" << std::endl; - break; + } + if (putTaskToSleep) { + break; } } } } +bool PlocSupvHelper::handleUartReception() { + ReturnValue_t result = OK; + ssize_t bytesRead = read(serialPort, reinterpret_cast(recBuf.data()), + static_cast(recBuf.size())); + if (bytesRead == 0) { + { + MutexGuard mg(lock); + if (state == InternalState::FINISH) { + // Flush received and unread data + tcflush(serialPort, TCIOFLUSH); + state = InternalState::IDLE; + return true; + } + } + while (result != NO_PACKET_FOUND) { + result = tryHdlcParsing(); + } + } else if (bytesRead < 0) { + sif::warning << "PlocSupvHelper::performOperation: read call failed with error [" << errno + << ", " << strerror(errno) << "]" << std::endl; + return true; + } else if (bytesRead >= static_cast(recBuf.size())) { + sif::error << "PlocSupvHelper::performOperation: Receive buffer too small for " << bytesRead + << " bytes" << std::endl; + } else if (bytesRead > 0) { + if (debugMode) { + sif::info << "Received " << bytesRead << " bytes from the PLOC Supervisor:" << std::endl; + } + recRingBuf.writeData(recBuf.data(), bytesRead); + tryHdlcParsing(); + } + return false; +} + ReturnValue_t PlocSupvHelper::setComIF(UartComIF* uartComIF_) { if (uartComIF_ == nullptr) { sif::warning << "PlocSupvHelper::initialize: Provided invalid uart com if" << std::endl; @@ -253,7 +222,7 @@ ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { update.packetNum = 1; update.deleteMemory = params.deleteMemory; update.sequenceCount = params.seqCount; - state = InternalState::UPDATE; + request = Request::UPDATE; uartComIF->flushUartTxAndRxBuf(comCookie); semaphore->release(); return result; @@ -272,14 +241,14 @@ ReturnValue_t PlocSupvHelper::performMemCheck(uint8_t memoryId, uint32_t startAd update.startAddress = startAddress; update.length = sizeToCheck; update.crcShouldBeChecked = checkCrc; - state = InternalState::CHECK_MEMORY; + request = Request::CHECK_MEMORY; uartComIF->flushUartTxAndRxBuf(comCookie); semaphore->release(); return returnvalue::OK; } void PlocSupvHelper::initiateUpdateContinuation() { - state = InternalState::CONTINUE_UPDATE; + request = Request::CONTINUE_UPDATE; semaphore->release(); } @@ -294,7 +263,7 @@ ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) { return PATH_NOT_EXISTS; } eventBufferReq.path = path; - state = InternalState::REQUEST_EVENT_BUFFER; + request = Request::REQUEST_EVENT_BUFFER; uartComIF->flushUartTxAndRxBuf(comCookie); semaphore->release(); return returnvalue::OK; @@ -917,7 +886,7 @@ ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendD if (result != OK) { std::cout << "PlocSupvHelper::sendMessage: Releasing semaphore failed" << std::endl; } - return returnvalue::OK; + return encodeAndSendPacket(sendData, sendLen); } ReturnValue_t PlocSupvHelper::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } @@ -926,6 +895,74 @@ ReturnValue_t PlocSupvHelper::requestReceiveMessage(CookieIF* cookie, size_t req return returnvalue::OK; } +bool PlocSupvHelper::handleRunningRequest() { + ReturnValue_t result = OK; + switch (request) { + case Request::UPDATE: { + result = executeUpdate(); + if (result == returnvalue::OK) { + triggerEvent(SUPV_UPDATE_SUCCESSFUL, result); + } else if (result == PROCESS_TERMINATED) { + // Event already triggered + } else { + triggerEvent(SUPV_UPDATE_FAILED, result); + } + MutexGuard mg(lock); + state = InternalState::IDLE; + return true; + } + case Request::CHECK_MEMORY: { + executeFullCheckMemoryCommand(); + MutexGuard mg(lock); + state = InternalState::IDLE; + return true; + } + case Request::CONTINUE_UPDATE: { + result = continueUpdate(); + if (result == returnvalue::OK) { + triggerEvent(SUPV_CONTINUE_UPDATE_SUCCESSFUL, result); + } else if (result == PROCESS_TERMINATED) { + // Event already triggered + } else { + triggerEvent(SUPV_CONTINUE_UPDATE_FAILED, result); + } + MutexGuard mg(lock); + state = InternalState::IDLE; + return true; + } + case Request::REQUEST_EVENT_BUFFER: { + result = performEventBufferRequest(); + if (result == returnvalue::OK) { + triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result); + } else if (result == PROCESS_TERMINATED) { + // Event already triggered + break; + } else { + triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result); + } + MutexGuard mg(lock); + state = InternalState::IDLE; + return true; + } + case Request::HANDLER_DRIVEN: { + break; + } + } + return false; +} + +ReturnValue_t PlocSupvHelper::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) { + size_t encodedLen = 0; + hdlc_add_framing(sendData, sendLen, sendBuf.data(), &encodedLen); + size_t bytesWritten = write(serialPort, sendBuf.data(), encodedLen); + if (bytesWritten != encodedLen) { + sif::warning << "ScexUartReader::sendMessage: Sending ping command to solar experiment failed" + << std::endl; + return FAILED; + } + return returnvalue::OK; +} + ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { MutexGuard mg(lock); @@ -937,11 +974,28 @@ ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** bu *buffer = ipcBuffer.data(); ReturnValue_t result = ipcRingBuf.readData(ipcBuffer.data(), *size, true); if (result != OK) { - sif::warning << "ScexUartReader::readReceivedMessage: Reading RingBuffer failed" << std::endl; + sif::warning << "PlocSupvHelper::readReceivedMessage: Reading RingBuffer failed" << std::endl; } return OK; } +ReturnValue_t PlocSupvHelper::tryHdlcParsing() { + size_t bytesRead = 0; + ReturnValue_t result = parseRecRingBufForHdlc(bytesRead); + if (result == returnvalue::OK) { + // Packet found, advance read pointer. + ipcRingBuf.writeData(decodedBuf.data(), bytesRead); + recRingBuf.deleteData(bytesRead); + } else if (result != NO_PACKET_FOUND) { + sif::warning << "ScexUartReader::performOperation: Possible packet loss" << std::endl; + // Markers found at wrong place + // which might be a hint for a possibly lost packet. + // Maybe trigger event? + recRingBuf.deleteData(bytesRead); + } + return result; +} + ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { size_t availableData = recRingBuf.getAvailableReadData(); if (availableData == 0) { diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvHelper.h index 2e6db05b..d2a3a3ec 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvHelper.h @@ -227,6 +227,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, SdCardManager* sdcMan = nullptr; #endif SimpleRingBuffer recRingBuf; + std::array sendBuf = {}; std::array recBuf = {}; std::array encodedBuf = {}; std::array decodedBuf = {}; @@ -254,8 +255,13 @@ class PlocSupvHelper : public DeviceCommunicationIF, // Remembers APID to know at which command a procedure failed uint16_t rememberApid = 0; + bool handleRunningRequest(); + bool handleUartReception(); + + ReturnValue_t encodeAndSendPacket(const uint8_t* sendData, size_t sendLen); void executeFullCheckMemoryCommand(); + ReturnValue_t tryHdlcParsing(); ReturnValue_t parseRecRingBufForHdlc(size_t& readSize); ReturnValue_t executeUpdate(); ReturnValue_t continueUpdate(); From 2b9afbedff1783500afda35e37c1e3c4bd7d62cb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 14:19:58 +0100 Subject: [PATCH 094/244] important fix and clarification --- .../devicedefinitions/PlocMPSoCDefinitions.h | 23 +++--- .../PlocSupervisorDefinitions.h | 75 ++++++++++++------- linux/devices/ploc/PlocSupervisorHandler.cpp | 9 +-- mission/devices/devicedefinitions/SpBase.h | 10 +-- 4 files changed, 68 insertions(+), 49 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h index a5ed0672..a373d96b 100644 --- a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h +++ b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h @@ -153,7 +153,7 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { */ TcBase(ploc::SpTcParams params, uint16_t apid, uint16_t sequenceCount) : ploc::SpTcBase(params, apid, sequenceCount) { - spParams.setDataFieldLen(INIT_LENGTH); + spParams.setFullPacketLen(INIT_LENGTH); } ReturnValue_t buildPacket() { return buildPacket(nullptr, 0); } @@ -206,7 +206,7 @@ class TcMemRead : public TcBase { */ TcMemRead(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_MEM_READ, sequenceCount) { - spParams.setPayloadLen(COMMAND_LENGTH); + spParams.setFullPacketLen(COMMAND_LENGTH + CRC_SIZE); } uint16_t getMemLen() const { return memLen; } @@ -267,7 +267,7 @@ class TcMemWrite : public TcBase { } uint16_t memLen = *(commandData + MEM_ADDRESS_SIZE) << 8 | *(commandData + MEM_ADDRESS_SIZE + 1); - spParams.setPayloadLen(MIN_FIXED_PAYLOAD_LENGTH + memLen * 4); + spParams.setFullPacketLen(MIN_FIXED_PAYLOAD_LENGTH + memLen * 4 + CRC_SIZE); result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -313,7 +313,7 @@ class FlashFopen : public ploc::SpTcBase { ReturnValue_t createPacket(std::string filename, char accessMode_) { accessMode = accessMode_; size_t nameSize = filename.size(); - spParams.setPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + sizeof(accessMode)); + spParams.setFullPacketLen(nameSize + sizeof(NULL_TERMINATOR) + sizeof(accessMode) + CRC_SIZE); ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -339,7 +339,7 @@ class FlashFclose : public ploc::SpTcBase { ReturnValue_t createPacket(std::string filename) { size_t nameSize = filename.size(); - spParams.setPayloadLen(nameSize + sizeof(NULL_TERMINATOR)); + spParams.setFullPacketLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -365,7 +365,7 @@ class TcFlashWrite : public ploc::SpTcBase { sif::debug << "FlashWrite::createPacket: Command data too big" << std::endl; return returnvalue::FAILED; } - spParams.setPayloadLen(static_cast(writeLen) + 4); + spParams.setFullPacketLen(static_cast(writeLen) + 4 + CRC_SIZE); result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -399,7 +399,7 @@ class TcFlashDelete : public ploc::SpTcBase { ReturnValue_t buildPacket(std::string filename) { size_t nameSize = filename.size(); - spParams.setPayloadLen(nameSize + sizeof(NULL_TERMINATOR)); + spParams.setFullPacketLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); auto res = checkPayloadLen(); if (res != returnvalue::OK) { return res; @@ -439,7 +439,7 @@ class TcReplayStart : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { ReturnValue_t result = returnvalue::OK; - spParams.setPayloadLen(commandDataLen); + spParams.setFullPacketLen(commandDataLen + CRC_SIZE); result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { return result; @@ -500,7 +500,7 @@ class TcDownlinkPwrOn : public TcBase { if (result != returnvalue::OK) { return result; } - spParams.setPayloadLen(commandDataLen + sizeof(MAX_AMPLITUDE)); + spParams.setFullPacketLen(commandDataLen + sizeof(MAX_AMPLITUDE) + CRC_SIZE); result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -571,7 +571,7 @@ class TcReplayWriteSeq : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { ReturnValue_t result = returnvalue::OK; - spParams.setPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR)); + spParams.setFullPacketLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { return result; @@ -657,7 +657,8 @@ class TcCamcmdSend : public TcBase { return INVALID_LENGTH; } uint16_t dataLen = static_cast(commandDataLen + sizeof(CARRIAGE_RETURN)); - spParams.setPayloadLen(sizeof(dataLen) + commandDataLen + sizeof(CARRIAGE_RETURN)); + spParams.setFullPacketLen(sizeof(dataLen) + commandDataLen + sizeof(CARRIAGE_RETURN) + + CRC_SIZE); auto res = checkPayloadLen(); if (res != returnvalue::OK) { return res; diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 19b224cc..0e4d30c2 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -161,8 +161,6 @@ static const size_t MAX_DATA_CAPACITY = 1016; /** This is the maximum size of a space packet for the supervisor */ static const size_t MAX_PACKET_SIZE = 1024; -static const uint8_t SPACE_PACKET_HEADER_LENGTH = 6; - struct UpdateParams { std::string file; uint8_t memId; @@ -284,6 +282,29 @@ static const uint32_t ERASE_MEMORY = 60000; static const uint32_t UPDATE_STATUS_REPORT = 70000; } // namespace recv_timeout +static constexpr size_t TIMESTAMP_LEN = 7; +static constexpr size_t SECONDARY_HEADER_LEN = TIMESTAMP_LEN + 1; +static constexpr size_t CRC_LEN = 2; + +struct SupvTcParams : public ploc::SpTcParams { + public: + SupvTcParams(SpacePacketCreator& creator) : ploc::SpTcParams(creator) {} + + SupvTcParams(SpacePacketCreator& creator, uint8_t* buf, size_t maxSize) + : ploc::SpTcParams(creator, buf, maxSize) {} + + void setLenFromPayloadLen(size_t payloadLen) { + setFullPacketLen(ccsds::HEADER_LEN + SECONDARY_HEADER_LEN + payloadLen + CRC_LEN); + } +}; + +class SupvTcBase : public ploc::SpTcBase { + public: + SupvTcBase(SupvTcParams params) : ploc::SpTcBase(params) {} + + private: +}; + /** * @brief This class creates a space packet containing only the header data and the CRC. */ @@ -297,7 +318,7 @@ class ApidOnlyPacket : public ploc::SpTcBase { * @note Sequence count of empty packet is always 1. */ ApidOnlyPacket(ploc::SpTcParams params, uint16_t apid) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(LENGTH_EMPTY_TC); + spParams.setFullPacketLen(LENGTH_EMPTY_TC); spParams.creator.setApid(apid); } @@ -332,7 +353,7 @@ class MPSoCBootSelect : public ploc::SpTcBase { * @note Selection of partitions is currently not supported. */ MPSoCBootSelect(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SEL_MPSOC_BOOT_IMAGE); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -378,7 +399,7 @@ class EnableNvms : public ploc::SpTcBase { * @param bp2 Partition pin 2 */ EnableNvms(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_ENABLE_NVMS); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -408,7 +429,7 @@ class EnableNvms : public ploc::SpTcBase { class SetTimeRef : public ploc::SpTcBase { public: SetTimeRef(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_TIME_REF); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -487,7 +508,7 @@ class SetBootTimeout : public ploc::SpTcBase { * @param timeout The boot timeout in milliseconds. */ SetBootTimeout(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_BOOT_TIMEOUT); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -524,7 +545,7 @@ class SetRestartTries : public ploc::SpTcBase { * @param restartTries Maximum restart tries to set. */ SetRestartTries(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_MAX_RESTART_TRIES); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -558,7 +579,7 @@ class DisablePeriodicHkTransmission : public ploc::SpTcBase { * @brief Constructor */ DisablePeriodicHkTransmission(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_DISABLE_HK); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -594,7 +615,7 @@ class LatchupAlert : public ploc::SpTcBase { * 3 - 3.3V, 4 - NVM_4XO, 5 - MISSION, 6 - SAFECOTS) */ LatchupAlert(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -632,7 +653,7 @@ class SetAlertlimit : public ploc::SpTcBase { * @param dutycycle */ SetAlertlimit(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_ALERT_LIMIT); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -675,7 +696,7 @@ class SetAdcEnabledChannels : public ploc::SpTcBase { * @param ch Defines channels to be enabled or disabled. */ SetAdcEnabledChannels(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_ADC_ENABLED_CHANNELS); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -714,7 +735,7 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { * @param stridingStepSize */ SetAdcWindowAndStride(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_ADC_WINDOW_AND_STRIDE); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -754,7 +775,7 @@ class SetAdcThreshold : public ploc::SpTcBase { * @param threshold */ SetAdcThreshold(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_ADC_THRESHOLD); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -792,7 +813,7 @@ class RunAutoEmTests : public ploc::SpTcBase { * @param test 1 - complete EM test, 2 - Short test (only memory readback NVM0,1,3) */ RunAutoEmTests(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_RUN_AUTO_EM_TESTS); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -834,7 +855,7 @@ class MramCmd : public ploc::SpTcBase { * @note The content at the stop address is excluded from the dump or wipe operation. */ MramCmd(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -888,7 +909,7 @@ class SetGpio : public ploc::SpTcBase { * @param val */ SetGpio(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_GPIO); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -932,7 +953,7 @@ class ReadGpio : public ploc::SpTcBase { * @param pin */ ReadGpio(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setDataFieldLen(DATA_FIELD_LENGTH); + spParams.setFullPacketLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_READ_GPIO); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1009,14 +1030,14 @@ class FactoryReset : public ploc::SpTcBase { default: break; } - spParams.setDataFieldLen(packetDataLen); + spParams.setFullPacketLen(packetDataLen); } }; class SetShutdownTimeout : public ploc::SpTcBase { public: SetShutdownTimeout(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setPayloadLen(PAYLOAD_LEN); + spParams.setFullPacketLen(PAYLOAD_LEN + 2); spParams.creator.setApid(APID_SET_SHUTDOWN_TIMEOUT); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1055,7 +1076,7 @@ class CheckMemory : public ploc::SpTcBase { * @param length Length in bytes of memory region */ CheckMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setPayloadLen(PAYLOAD_LENGTH); + spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_CHECK_MEMORY); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1134,9 +1155,9 @@ class WriteMemory : public ploc::SpTcBase { uint8_t* updateData) { uint8_t* data = payloadStart; if (updateDataLen % 2 != 0) { - spParams.setPayloadLen(META_DATA_LENGTH + updateDataLen + 1); + spParams.setFullPacketLen(META_DATA_LENGTH + updateDataLen + 1 + 2); } else { - spParams.setPayloadLen(META_DATA_LENGTH + updateDataLen); + spParams.setFullPacketLen(META_DATA_LENGTH + updateDataLen + 2); } // To avoid crashes in this unexpected case ReturnValue_t result = checkPayloadLen(); @@ -1168,7 +1189,7 @@ class WriteMemory : public ploc::SpTcBase { class EraseMemory : public ploc::SpTcBase { public: EraseMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setPayloadLen(PAYLOAD_LENGTH); + spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_ERASE_MEMORY); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1210,7 +1231,7 @@ class EraseMemory : public ploc::SpTcBase { class EnableAutoTm : public ploc::SpTcBase { public: EnableAutoTm(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setPayloadLen(PAYLOAD_LENGTH); + spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_AUTO_TM); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1235,7 +1256,7 @@ class EnableAutoTm : public ploc::SpTcBase { class DisableAutoTm : public ploc::SpTcBase { public: DisableAutoTm(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setPayloadLen(PAYLOAD_LENGTH); + spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_AUTO_TM); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1270,7 +1291,7 @@ class RequestLoggingData : public ploc::SpTcBase { }; RequestLoggingData(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setPayloadLen(PAYLOAD_LENGTH); + spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_REQUEST_LOGGING_DATA); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 5b4339e9..95f64510 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -1808,11 +1808,11 @@ ReturnValue_t PlocSupervisorHandler::parseMramPackets(const uint8_t* packet, siz std::memcpy(spacePacketBuffer + bufferTop, packet + idx, 1); bufferTop += 1; *foundLen += 1; - if (bufferTop >= supv::SPACE_PACKET_HEADER_LENGTH) { + if (bufferTop >= ccsds::HEADER_LEN) { packetLen = readSpacePacketLength(spacePacketBuffer); } - if (bufferTop == supv::SPACE_PACKET_HEADER_LENGTH + packetLen + 1) { + if (bufferTop == ccsds::HEADER_LEN + packetLen + 1) { packetInBuffer = true; bufferTop = 0; return checkMramPacketApid(); @@ -1838,7 +1838,7 @@ ReturnValue_t PlocSupervisorHandler::handleMramDumpPacket(DeviceCommandId_t id) // Prepare packet for downlink if (packetInBuffer) { uint16_t packetLen = readSpacePacketLength(spacePacketBuffer); - result = verifyPacket(spacePacketBuffer, supv::SPACE_PACKET_HEADER_LENGTH + packetLen + 1); + result = verifyPacket(spacePacketBuffer, ccsds::HEADER_LEN + packetLen + 1); if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::handleMramDumpPacket: CRC failure" << std::endl; return result; @@ -1940,8 +1940,7 @@ ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) { return SupvReturnValuesIF::MRAM_FILE_NOT_EXISTS; } std::ofstream file(activeMramFile, std::ios_base::app | std::ios_base::out); - file.write(reinterpret_cast(spacePacketBuffer + supv::SPACE_PACKET_HEADER_LENGTH), - packetLen - 1); + file.write(reinterpret_cast(spacePacketBuffer + ccsds::HEADER_LEN), packetLen - 1); file.close(); return returnvalue::OK; } diff --git a/mission/devices/devicedefinitions/SpBase.h b/mission/devices/devicedefinitions/SpBase.h index caa14d5f..edc91334 100644 --- a/mission/devices/devicedefinitions/SpBase.h +++ b/mission/devices/devicedefinitions/SpBase.h @@ -14,14 +14,12 @@ struct SpTcParams { SpTcParams(SpacePacketCreator& creator, uint8_t* buf, size_t maxSize) : creator(creator), buf(buf), maxSize(maxSize) {} - void setPayloadLen(size_t payloadLen_) { dataFieldLen = payloadLen_ + 2; } - - void setDataFieldLen(size_t dataFieldLen_) { dataFieldLen = dataFieldLen_; } + void setFullPacketLen(size_t fullPacketLen_) { fullPacketLen = fullPacketLen_; } SpacePacketCreator& creator; uint8_t* buf = nullptr; size_t maxSize = 0; - size_t dataFieldLen = 0; + size_t fullPacketLen = 0; }; class SpTcBase { @@ -39,7 +37,7 @@ class SpTcBase { } void updateSpFields() { - spParams.creator.setDataLenField(spParams.dataFieldLen - 1); + spParams.creator.setDataLenField(spParams.fullPacketLen - 1); spParams.creator.setPacketType(ccsds::PacketType::TC); } @@ -50,7 +48,7 @@ class SpTcBase { uint16_t getApid() const { return spParams.creator.getApid(); } ReturnValue_t checkPayloadLen() { - if (ccsds::HEADER_LEN + spParams.dataFieldLen > spParams.maxSize) { + if (ccsds::HEADER_LEN + spParams.fullPacketLen > spParams.maxSize) { return SerializeIF::BUFFER_TOO_SHORT; } From d184f7487e6cead6705c31d0190f2c2e8eb86379 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 4 Nov 2022 14:54:23 +0100 Subject: [PATCH 095/244] post merge cleanup --- bsp_q7s/core/InitMission.cpp | 46 ++++++++++++------------ fsfw | 2 +- mission/controller/AcsController.cpp | 1 - mission/controller/AcsController.h | 1 - mission/controller/acs/SensorValues.h | 4 +-- mission/controller/acs/config/classIds.h | 9 +---- 6 files changed, 26 insertions(+), 37 deletions(-) diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index 4938b991..b922ae88 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -163,29 +163,29 @@ void initmission::initTasks() { } #endif #if OBSW_Q7S_EM == 1 - acsTask->addComponent(objects::MGM_0_LIS3_HANDLER); - acsTask->addComponent(objects::MGM_1_RM3100_HANDLER); - acsTask->addComponent(objects::MGM_2_LIS3_HANDLER); - acsTask->addComponent(objects::MGM_3_RM3100_HANDLER); - acsTask->addComponent(objects::IMTQ_HANDLER); - acsTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF); - acsTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF); - acsTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB); - acsTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB); - acsTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB); - acsTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB); - acsTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF); - acsTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF); - acsTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF); - acsTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); - acsTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); - acsTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); - acsTask->addComponent(objects::GYRO_0_ADIS_HANDLER); - acsTask->addComponent(objects::GYRO_1_L3G_HANDLER); - acsTask->addComponent(objects::GYRO_2_ADIS_HANDLER); - acsTask->addComponent(objects::GYRO_3_L3G_HANDLER); - acsTask->addComponent(objects::GPS_CONTROLLER); - acsTask->addComponent(objects::STAR_TRACKER); + acsCtrlTask->addComponent(objects::MGM_0_LIS3_HANDLER); + acsCtrlTask->addComponent(objects::MGM_1_RM3100_HANDLER); + acsCtrlTask->addComponent(objects::MGM_2_LIS3_HANDLER); + acsCtrlTask->addComponent(objects::MGM_3_RM3100_HANDLER); + acsCtrlTask->addComponent(objects::IMTQ_HANDLER); + acsCtrlTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF); + acsCtrlTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF); + acsCtrlTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB); + acsCtrlTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB); + acsCtrlTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB); + acsCtrlTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB); + acsCtrlTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF); + acsCtrlTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF); + acsCtrlTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF); + acsCtrlTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF); + acsCtrlTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB); + acsCtrlTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB); + acsCtrlTask->addComponent(objects::GYRO_0_ADIS_HANDLER); + acsCtrlTask->addComponent(objects::GYRO_1_L3G_HANDLER); + acsCtrlTask->addComponent(objects::GYRO_2_ADIS_HANDLER); + acsCtrlTask->addComponent(objects::GYRO_3_L3G_HANDLER); + acsCtrlTask->addComponent(objects::GPS_CONTROLLER); + acsCtrlTask->addComponent(objects::STAR_TRACKER); #endif PeriodicTaskIF* acsSysTask = factory->createPeriodicTask( diff --git a/fsfw b/fsfw index 7e0a5d5a..672fca51 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 7e0a5d5a9e4f38c6d818bbdd5b44d34d8007eb1e +Subproject commit 672fca5169b017387e58e2ff864913d932c59aa1 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 4134a38d..0c2b58a9 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -637,4 +637,3 @@ void AcsController::copyGyrData() { } } } - diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 1fd04aa2..cfcc200d 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -1,7 +1,6 @@ #ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_ #define MISSION_CONTROLLER_ACSCONTROLLER_H_ -#include #include #include diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 1a260635..ef897207 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,17 +1,15 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ -#include - #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/IMTQHandlerDefinitions.h" #include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" +#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" namespace ACS { diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h index 7db1ea72..e714ff33 100644 --- a/mission/controller/acs/config/classIds.h +++ b/mission/controller/acs/config/classIds.h @@ -1,14 +1,7 @@ -/* - * classIds.h - * - * Created on: 1 Mar 2022 - * Author: rooob - */ - #ifndef ACS_CONFIG_CLASSIDS_H_ #define ACS_CONFIG_CLASSIDS_H_ -#include +#include #include namespace CLASS_ID { From bb2f8edc7773ac081027067d0bd33e8624eece42 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 7 Nov 2022 09:26:36 +0100 Subject: [PATCH 096/244] add new APID defs --- .../PlocSupervisorDefinitions.h | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 0e4d30c2..3dc2931e 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -15,6 +15,23 @@ namespace supv { +typedef struct +{ +// The most significant bit of msec value is set to 0x80 to indicate that full +// time and data information is transmitted, when the time has been synced with +// the reference. If the time has not been synced with reference, then the most +// significant bit is set to 0x00. Only the most significant bit is used for + // this purpose (bit 15 of the field tm_msec) + uint16_t tm_msec; // miliseconds 0-999; + uint8_t tm_sec; // seconds after the minute, 0 to 60 + // (0 - 60 allows for the occasional leap second) + uint8_t tm_min; // minutes after the hour, 0 to 59 + uint8_t tm_hour; // hours since midnight, 0 to 23 + uint8_t tm_mday; // day of the month, 1 to 31 + uint8_t tm_mon; // months 1 to 12 + uint8_t tm_year; // years since 1900 +} tas_time_t; + /** Command IDs */ static const DeviceCommandId_t NONE = 0; static const DeviceCommandId_t GET_HK_REPORT = 1; @@ -104,6 +121,64 @@ static const uint16_t APID_DATA_LOGGER_DATA = 0x20D; // 2 bits APID SRC, 00 for OBC, 2 bits APID DEST, 01 for SUPV, 7 bits CMD ID -> Mask 0x080 static constexpr uint16_t APID_TC_SUPV_MASK = 0x080; +static constexpr uint16_t APID_TMTC_MAN = 0x00; +static constexpr uint16_t APID_HK = 0x01; +static constexpr uint16_t APID_BOOT_MAN = 0x02; +static constexpr uint16_t APID_LATCHUP_MON = 0x03; +static constexpr uint16_t APID_ADC_MON = 0x04; +static constexpr uint16_t APID_MEM_MAN = 0x05; +static constexpr uint16_t APID_DATA_LOGGER = 0x06; +static constexpr uint16_t APID_WDOG_MAN = 0x07; + +enum class HkServiceIds: uint8_t { + ENABLE = 0x01, + SET_PERIOD = 0x02, + GET_REPORT = 0x03, + GET_HARDFAULTS_REPORT = 0x04, +}; + +enum class TmtcServiceIds: uint8_t { + TIME_REF = 0x03, + GET_SUPV_VERSION = 0x05, + RUN_AUTO_EM_TEST = 0x08, + SET_GPIO = 0x0E, + READ_GPIO = 0x0F, + GET_MPSOC_POWER_INFO = 0x10 +}; + +enum class BootManServiceIds: uint8_t { + START_MPSOC = 0x01, + SHUTDOWN_MPSOC = 0x02, + SELECT_IMAGE = 0x03, + SET_BOOT_TIMEOUT = 0x04, + SET_MAX_REBOOT_TRIES = 0x05, + RESET_MPSOC = 0x06, + RESET_PL = 0x07, + GET_BOOT_STATUS_REPORT = 0x08, + PREPARE_UPDATE = 0x09, + SHUTDOWN_TIMEOUT = 0x0B, + FACTORY_FLASH = 0x0C +}; + +enum class LatchupMonServiceIds: uint8_t { + ENABLE = 0x01, + DISABLE = 0x02, + SET_ALERT_LIMIT = 0x04, + GET_STATUS_REPORT = 0x06 +}; + +// Right now, none of the commands seem to be implemented, but still +// keep the enum here in case some are added +enum class AdcMonServiceIds: uint8_t {}; + +enum class DataLoggerServiceIds: uint8_t { + FACTORY_RESET = 0x07 +}; + +// Right now, none of the commands seem to be implemented, but still +// keep the enum here in case some are added +enum class WdogManServiceIds: uint8_t {}; + static const uint16_t APID_START_MPSOC = 0xA1; static const uint16_t APID_SHUTWOWN_MPSOC = 0xA2; static const uint16_t APID_SEL_MPSOC_BOOT_IMAGE = 0xA3; @@ -302,6 +377,11 @@ class SupvTcBase : public ploc::SpTcBase { public: SupvTcBase(SupvTcParams params) : ploc::SpTcBase(params) {} + SupvTcBase(SupvTcParams params, uint16_t apid, uint16_t seqCount) + : ploc::SpTcBase(params, apid, seqCount) { + + } + private: }; From 32865d183494019f68a7c8bd8f5778917539cf5c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 7 Nov 2022 10:00:54 +0100 Subject: [PATCH 097/244] use on/off semantics and rename handler --- .../PlocSupervisorDefinitions.h | 59 ++++++++---------- linux/devices/ploc/CMakeLists.txt | 2 +- linux/devices/ploc/PlocSupervisorHandler.h | 3 +- ...PlocSupvHelper.cpp => PlocSupvUartMan.cpp} | 62 +++++++------------ .../{PlocSupvHelper.h => PlocSupvUartMan.h} | 21 ++++--- 5 files changed, 66 insertions(+), 81 deletions(-) rename linux/devices/ploc/{PlocSupvHelper.cpp => PlocSupvUartMan.cpp} (96%) rename linux/devices/ploc/{PlocSupvHelper.h => PlocSupvUartMan.h} (97%) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 3dc2931e..75bbdb28 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -15,21 +15,20 @@ namespace supv { -typedef struct -{ -// The most significant bit of msec value is set to 0x80 to indicate that full -// time and data information is transmitted, when the time has been synced with -// the reference. If the time has not been synced with reference, then the most -// significant bit is set to 0x00. Only the most significant bit is used for - // this purpose (bit 15 of the field tm_msec) - uint16_t tm_msec; // miliseconds 0-999; - uint8_t tm_sec; // seconds after the minute, 0 to 60 +typedef struct { + // The most significant bit of msec value is set to 0x80 to indicate that full + // time and data information is transmitted, when the time has been synced with + // the reference. If the time has not been synced with reference, then the most + // significant bit is set to 0x00. Only the most significant bit is used for + // this purpose (bit 15 of the field tm_msec) + uint16_t tm_msec; // miliseconds 0-999; + uint8_t tm_sec; // seconds after the minute, 0 to 60 // (0 - 60 allows for the occasional leap second) - uint8_t tm_min; // minutes after the hour, 0 to 59 - uint8_t tm_hour; // hours since midnight, 0 to 23 - uint8_t tm_mday; // day of the month, 1 to 31 - uint8_t tm_mon; // months 1 to 12 - uint8_t tm_year; // years since 1900 + uint8_t tm_min; // minutes after the hour, 0 to 59 + uint8_t tm_hour; // hours since midnight, 0 to 23 + uint8_t tm_mday; // day of the month, 1 to 31 + uint8_t tm_mon; // months 1 to 12 + uint8_t tm_year; // years since 1900 } tas_time_t; /** Command IDs */ @@ -130,23 +129,23 @@ static constexpr uint16_t APID_MEM_MAN = 0x05; static constexpr uint16_t APID_DATA_LOGGER = 0x06; static constexpr uint16_t APID_WDOG_MAN = 0x07; -enum class HkServiceIds: uint8_t { +enum class HkServiceIds : uint8_t { ENABLE = 0x01, SET_PERIOD = 0x02, GET_REPORT = 0x03, GET_HARDFAULTS_REPORT = 0x04, }; -enum class TmtcServiceIds: uint8_t { - TIME_REF = 0x03, - GET_SUPV_VERSION = 0x05, - RUN_AUTO_EM_TEST = 0x08, - SET_GPIO = 0x0E, - READ_GPIO = 0x0F, - GET_MPSOC_POWER_INFO = 0x10 +enum class TmtcServiceIds : uint8_t { + TIME_REF = 0x03, + GET_SUPV_VERSION = 0x05, + RUN_AUTO_EM_TEST = 0x08, + SET_GPIO = 0x0E, + READ_GPIO = 0x0F, + GET_MPSOC_POWER_INFO = 0x10 }; -enum class BootManServiceIds: uint8_t { +enum class BootManServiceIds : uint8_t { START_MPSOC = 0x01, SHUTDOWN_MPSOC = 0x02, SELECT_IMAGE = 0x03, @@ -160,7 +159,7 @@ enum class BootManServiceIds: uint8_t { FACTORY_FLASH = 0x0C }; -enum class LatchupMonServiceIds: uint8_t { +enum class LatchupMonServiceIds : uint8_t { ENABLE = 0x01, DISABLE = 0x02, SET_ALERT_LIMIT = 0x04, @@ -169,15 +168,13 @@ enum class LatchupMonServiceIds: uint8_t { // Right now, none of the commands seem to be implemented, but still // keep the enum here in case some are added -enum class AdcMonServiceIds: uint8_t {}; +enum class AdcMonServiceIds : uint8_t {}; -enum class DataLoggerServiceIds: uint8_t { - FACTORY_RESET = 0x07 -}; +enum class DataLoggerServiceIds : uint8_t { FACTORY_RESET = 0x07 }; // Right now, none of the commands seem to be implemented, but still // keep the enum here in case some are added -enum class WdogManServiceIds: uint8_t {}; +enum class WdogManServiceIds : uint8_t {}; static const uint16_t APID_START_MPSOC = 0xA1; static const uint16_t APID_SHUTWOWN_MPSOC = 0xA2; @@ -378,9 +375,7 @@ class SupvTcBase : public ploc::SpTcBase { SupvTcBase(SupvTcParams params) : ploc::SpTcBase(params) {} SupvTcBase(SupvTcParams params, uint16_t apid, uint16_t seqCount) - : ploc::SpTcBase(params, apid, seqCount) { - - } + : ploc::SpTcBase(params, apid, seqCount) {} private: }; diff --git a/linux/devices/ploc/CMakeLists.txt b/linux/devices/ploc/CMakeLists.txt index f085eab3..67a0637b 100644 --- a/linux/devices/ploc/CMakeLists.txt +++ b/linux/devices/ploc/CMakeLists.txt @@ -1,4 +1,4 @@ target_sources( ${OBSW_NAME} PRIVATE PlocSupervisorHandler.cpp PlocMemoryDumper.cpp PlocMPSoCHandler.cpp - PlocMPSoCHelper.cpp PlocSupvHelper.cpp) + PlocMPSoCHelper.cpp PlocSupvUartMan.cpp) diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 4bdf2efa..d9776c36 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -1,8 +1,9 @@ #ifndef MISSION_DEVICES_PLOCSUPERVISORHANDLER_H_ #define MISSION_DEVICES_PLOCSUPERVISORHANDLER_H_ +#include + #include "OBSWConfig.h" -#include "PlocSupvHelper.h" #include "bsp_q7s/fs/SdCardManager.h" #include "devices/powerSwitcherList.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" diff --git a/linux/devices/ploc/PlocSupvHelper.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp similarity index 96% rename from linux/devices/ploc/PlocSupvHelper.cpp rename to linux/devices/ploc/PlocSupvUartMan.cpp index 51b6397e..5bab8d13 100644 --- a/linux/devices/ploc/PlocSupvHelper.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1,9 +1,8 @@ -#include "PlocSupvHelper.h" - #include #include // Contains file controls like O_RDWR #include #include +#include #include #include @@ -90,36 +89,37 @@ ReturnValue_t PlocSupvHelper::initialize() { } ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { - lock->lockMutex(); - state = InternalState::IDLE; - lock->unlockMutex(); bool putTaskToSleep = false; while (true) { semaphore->acquire(); while (true) { putTaskToSleep = handleUartReception(); if (putTaskToSleep) { + performUartShutdown(); break; } lock->lockMutex(); InternalState currentState = state; lock->unlockMutex(); switch (currentState) { - case InternalState::IDLE: { + case InternalState::SLEEPING: + case InternalState::GO_TO_SLEEP: { putTaskToSleep = true; break; } - case InternalState::FINISH: { - state = InternalState::IDLE; - putTaskToSleep = true; + case InternalState::LONGER_REQUEST: { + if (handleRunningLongerRequest() == REQUEST_DONE) { + MutexGuard mg(lock); + state = InternalState::DEFAULT; + } break; } - case InternalState::RUNNING: { - putTaskToSleep = handleRunningRequest(); + case InternalState::DEFAULT: { break; } } if (putTaskToSleep) { + performUartShutdown(); break; } } @@ -133,10 +133,7 @@ bool PlocSupvHelper::handleUartReception() { if (bytesRead == 0) { { MutexGuard mg(lock); - if (state == InternalState::FINISH) { - // Flush received and unread data - tcflush(serialPort, TCIOFLUSH); - state = InternalState::IDLE; + if (state == InternalState::GO_TO_SLEEP) { return true; } } @@ -869,23 +866,15 @@ void PlocSupvHelper::resetSpParams() { spParams.buf = commandBuffer; } ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { - ReturnValue_t result; if (sendData == nullptr or sendLen == 0) { return FAILED; } lock->lockMutex(); - if (state != InternalState::IDLE) { + if (state == InternalState::SLEEPING or state == InternalState::LONGER_REQUEST) { lock->unlockMutex(); return FAILED; } - tcflush(serialPort, TCIFLUSH); - state = InternalState::RUNNING; lock->unlockMutex(); - - result = semaphore->release(); - if (result != OK) { - std::cout << "PlocSupvHelper::sendMessage: Releasing semaphore failed" << std::endl; - } return encodeAndSendPacket(sendData, sendLen); } @@ -895,7 +884,7 @@ ReturnValue_t PlocSupvHelper::requestReceiveMessage(CookieIF* cookie, size_t req return returnvalue::OK; } -bool PlocSupvHelper::handleRunningRequest() { +ReturnValue_t PlocSupvHelper::handleRunningLongerRequest() { ReturnValue_t result = OK; switch (request) { case Request::UPDATE: { @@ -907,15 +896,11 @@ bool PlocSupvHelper::handleRunningRequest() { } else { triggerEvent(SUPV_UPDATE_FAILED, result); } - MutexGuard mg(lock); - state = InternalState::IDLE; - return true; + break; } case Request::CHECK_MEMORY: { executeFullCheckMemoryCommand(); - MutexGuard mg(lock); - state = InternalState::IDLE; - return true; + break; } case Request::CONTINUE_UPDATE: { result = continueUpdate(); @@ -926,9 +911,7 @@ bool PlocSupvHelper::handleRunningRequest() { } else { triggerEvent(SUPV_CONTINUE_UPDATE_FAILED, result); } - MutexGuard mg(lock); - state = InternalState::IDLE; - return true; + break; } case Request::REQUEST_EVENT_BUFFER: { result = performEventBufferRequest(); @@ -940,11 +923,9 @@ bool PlocSupvHelper::handleRunningRequest() { } else { triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result); } - MutexGuard mg(lock); - state = InternalState::IDLE; - return true; + break; } - case Request::HANDLER_DRIVEN: { + case Request::DEFAULT: { break; } } @@ -1037,3 +1018,8 @@ ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { } return NO_PACKET_FOUND; } + +void PlocSupvHelper::performUartShutdown() { + tcflush(serialPort, TCIOFLUSH); + state = InternalState::GO_TO_SLEEP; +} diff --git a/linux/devices/ploc/PlocSupvHelper.h b/linux/devices/ploc/PlocSupvUartMan.h similarity index 97% rename from linux/devices/ploc/PlocSupvHelper.h rename to linux/devices/ploc/PlocSupvUartMan.h index d2a3a3ec..6781ec67 100644 --- a/linux/devices/ploc/PlocSupvHelper.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -158,11 +158,12 @@ class PlocSupvHelper : public DeviceCommunicationIF, static uint32_t buildProgParams1(uint8_t percent, uint16_t seqCount); private: - static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 0); - static constexpr ReturnValue_t DECODE_BUF_TOO_SMALL = returnvalue::makeCode(1, 1); + static constexpr ReturnValue_t REQUEST_DONE = returnvalue::makeCode(1, 0); + static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 1); + static constexpr ReturnValue_t DECODE_BUF_TOO_SMALL = returnvalue::makeCode(1, 2); static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS_CONSECUTIVE_START = - returnvalue::makeCode(1, 2); - static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS_CONSECUTIVE_END = returnvalue::makeCode(1, 3); + returnvalue::makeCode(1, 3); + static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS_CONSECUTIVE_END = returnvalue::makeCode(1, 4); static const uint16_t CRC16_INIT = 0xFFFF; // Event buffer reply will carry 24 space packets with 1016 bytes and one space packet with @@ -211,17 +212,17 @@ class PlocSupvHelper : public DeviceCommunicationIF, EventBufferRequest eventBufferReq; - enum class InternalState { IDLE, RUNNING, FINISH }; + enum class InternalState { SLEEPING, DEFAULT, LONGER_REQUEST, GO_TO_SLEEP }; enum class Request { - HANDLER_DRIVEN, + DEFAULT, UPDATE, CONTINUE_UPDATE, REQUEST_EVENT_BUFFER, CHECK_MEMORY, }; - InternalState state = InternalState::IDLE; - Request request = Request::HANDLER_DRIVEN; + InternalState state = InternalState::SLEEPING; + Request request = Request::DEFAULT; #ifdef XIPHOS_Q7S SdCardManager* sdcMan = nullptr; @@ -255,7 +256,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, // Remembers APID to know at which command a procedure failed uint16_t rememberApid = 0; - bool handleRunningRequest(); + ReturnValue_t handleRunningLongerRequest(); bool handleUartReception(); ReturnValue_t encodeAndSendPacket(const uint8_t* sendData, size_t sendLen); @@ -367,6 +368,8 @@ class PlocSupvHelper : public DeviceCommunicationIF, */ ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; + + void performUartShutdown(); }; #endif /* BSP_Q7S_DEVICES_PLOCSUPVHELPER_H_ */ From 23025eec7a692dbd4a04d878c64f3196c1d81a9e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 7 Nov 2022 10:30:09 +0100 Subject: [PATCH 098/244] no payload packet impl --- .../devicedefinitions/PlocMPSoCDefinitions.h | 34 +-- .../PlocSupervisorDefinitions.h | 194 ++++++++++-------- linux/devices/ploc/PlocSupervisorHandler.cpp | 10 +- mission/devices/devicedefinitions/SpBase.h | 10 +- 4 files changed, 137 insertions(+), 111 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h index a373d96b..8a88a716 100644 --- a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h +++ b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h @@ -153,7 +153,7 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { */ TcBase(ploc::SpTcParams params, uint16_t apid, uint16_t sequenceCount) : ploc::SpTcBase(params, apid, sequenceCount) { - spParams.setFullPacketLen(INIT_LENGTH); + spParams.setFullPayloadLen(INIT_LENGTH); } ReturnValue_t buildPacket() { return buildPacket(nullptr, 0); } @@ -181,7 +181,7 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF { if (res != returnvalue::OK) { return res; } - return calcCrc(); + return calcAndSetCrc(); } protected: @@ -206,7 +206,7 @@ class TcMemRead : public TcBase { */ TcMemRead(ploc::SpTcParams params, uint16_t sequenceCount) : TcBase(params, apid::TC_MEM_READ, sequenceCount) { - spParams.setFullPacketLen(COMMAND_LENGTH + CRC_SIZE); + spParams.setFullPayloadLen(COMMAND_LENGTH + CRC_SIZE); } uint16_t getMemLen() const { return memLen; } @@ -267,7 +267,7 @@ class TcMemWrite : public TcBase { } uint16_t memLen = *(commandData + MEM_ADDRESS_SIZE) << 8 | *(commandData + MEM_ADDRESS_SIZE + 1); - spParams.setFullPacketLen(MIN_FIXED_PAYLOAD_LENGTH + memLen * 4 + CRC_SIZE); + spParams.setFullPayloadLen(MIN_FIXED_PAYLOAD_LENGTH + memLen * 4 + CRC_SIZE); result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -313,7 +313,7 @@ class FlashFopen : public ploc::SpTcBase { ReturnValue_t createPacket(std::string filename, char accessMode_) { accessMode = accessMode_; size_t nameSize = filename.size(); - spParams.setFullPacketLen(nameSize + sizeof(NULL_TERMINATOR) + sizeof(accessMode) + CRC_SIZE); + spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + sizeof(accessMode) + CRC_SIZE); ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -322,7 +322,7 @@ class FlashFopen : public ploc::SpTcBase { *(spParams.buf + nameSize) = NULL_TERMINATOR; std::memcpy(payloadStart + nameSize + sizeof(NULL_TERMINATOR), &accessMode, sizeof(accessMode)); updateSpFields(); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -339,14 +339,14 @@ class FlashFclose : public ploc::SpTcBase { ReturnValue_t createPacket(std::string filename) { size_t nameSize = filename.size(); - spParams.setFullPacketLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); + spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); ReturnValue_t result = checkPayloadLen(); if (result != returnvalue::OK) { return result; } std::memcpy(payloadStart, filename.c_str(), nameSize); *(payloadStart + nameSize) = NULL_TERMINATOR; - return calcCrc(); + return calcAndSetCrc(); } }; @@ -365,7 +365,7 @@ class TcFlashWrite : public ploc::SpTcBase { sif::debug << "FlashWrite::createPacket: Command data too big" << std::endl; return returnvalue::FAILED; } - spParams.setFullPacketLen(static_cast(writeLen) + 4 + CRC_SIZE); + spParams.setFullPayloadLen(static_cast(writeLen) + 4 + CRC_SIZE); result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -382,7 +382,7 @@ class TcFlashWrite : public ploc::SpTcBase { if (res != returnvalue::OK) { return res; } - return calcCrc(); + return calcAndSetCrc(); } private: @@ -399,7 +399,7 @@ class TcFlashDelete : public ploc::SpTcBase { ReturnValue_t buildPacket(std::string filename) { size_t nameSize = filename.size(); - spParams.setFullPacketLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); + spParams.setFullPayloadLen(nameSize + sizeof(NULL_TERMINATOR) + CRC_SIZE); auto res = checkPayloadLen(); if (res != returnvalue::OK) { return res; @@ -412,7 +412,7 @@ class TcFlashDelete : public ploc::SpTcBase { if (res != returnvalue::OK) { return res; } - return calcCrc(); + return calcAndSetCrc(); } }; @@ -439,7 +439,7 @@ class TcReplayStart : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { ReturnValue_t result = returnvalue::OK; - spParams.setFullPacketLen(commandDataLen + CRC_SIZE); + spParams.setFullPayloadLen(commandDataLen + CRC_SIZE); result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { return result; @@ -500,7 +500,7 @@ class TcDownlinkPwrOn : public TcBase { if (result != returnvalue::OK) { return result; } - spParams.setFullPacketLen(commandDataLen + sizeof(MAX_AMPLITUDE) + CRC_SIZE); + spParams.setFullPayloadLen(commandDataLen + sizeof(MAX_AMPLITUDE) + CRC_SIZE); result = checkPayloadLen(); if (result != returnvalue::OK) { return result; @@ -571,7 +571,7 @@ class TcReplayWriteSeq : public TcBase { protected: ReturnValue_t initPacket(const uint8_t* commandData, size_t commandDataLen) override { ReturnValue_t result = returnvalue::OK; - spParams.setFullPacketLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); + spParams.setFullPayloadLen(commandDataLen + sizeof(NULL_TERMINATOR) + CRC_SIZE); result = lengthCheck(commandDataLen); if (result != returnvalue::OK) { return result; @@ -657,8 +657,8 @@ class TcCamcmdSend : public TcBase { return INVALID_LENGTH; } uint16_t dataLen = static_cast(commandDataLen + sizeof(CARRIAGE_RETURN)); - spParams.setFullPacketLen(sizeof(dataLen) + commandDataLen + sizeof(CARRIAGE_RETURN) + - CRC_SIZE); + spParams.setFullPayloadLen(sizeof(dataLen) + commandDataLen + sizeof(CARRIAGE_RETURN) + + CRC_SIZE); auto res = checkPayloadLen(); if (res != returnvalue::OK) { return res; diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 75bbdb28..cdc5f24a 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -13,6 +13,8 @@ #include "linux/devices/devicedefinitions/SupvReturnValuesIF.h" #include "mission/devices/devicedefinitions/SpBase.h" +using namespace returnvalue; + namespace supv { typedef struct { @@ -218,14 +220,28 @@ static const uint16_t APID_GET_HK_REPORT = 0xC6; static const uint16_t APID_MASK = 0x3FF; static const uint16_t SEQUENCE_COUNT_MASK = 0xFFF; -/** Offset from first byte in Space packet to first byte of data field */ -static const uint8_t DATA_FIELD_OFFSET = 6; +static constexpr uint16_t DEFAULT_SEQUENCE_COUNT = 1; +static const uint8_t HK_SET_ENTRIES = 13; +static const uint8_t BOOT_REPORT_SET_ENTRIES = 10; +static const uint8_t LATCHUP_RPT_SET_ENTRIES = 16; +static const uint8_t LOGGING_RPT_SET_ENTRIES = 16; +static const uint8_t ADC_RPT_SET_ENTRIES = 32; -/** - * Space packet length for fixed size packets. This is the size of the whole packet data - * field. For the length field in the space packet this size will be substracted by one. - */ -static const uint16_t LENGTH_EMPTY_TC = 2; // Only CRC will be transported with the data field +static const uint32_t HK_SET_ID = HK_REPORT; +static const uint32_t BOOT_REPORT_SET_ID = BOOT_STATUS_REPORT; +static const uint32_t LATCHUP_RPT_ID = LATCHUP_REPORT; +static const uint32_t LOGGING_RPT_ID = LOGGING_REPORT; +static const uint32_t ADC_REPORT_SET_ID = ADC_REPORT; + +namespace recv_timeout { +// Erase memory can require up to 60 seconds for execution +static const uint32_t ERASE_MEMORY = 60000; +static const uint32_t UPDATE_STATUS_REPORT = 70000; +} // namespace recv_timeout + +static constexpr size_t TIMESTAMP_LEN = 7; +static constexpr size_t SECONDARY_HEADER_LEN = TIMESTAMP_LEN + 1; +static constexpr size_t CRC_LEN = 2; /** This is the maximum length of a space packet as defined by the TAS ICD */ static const size_t MAX_COMMAND_SIZE = 1024; @@ -233,6 +249,10 @@ static const size_t MAX_DATA_CAPACITY = 1016; /** This is the maximum size of a space packet for the supervisor */ static const size_t MAX_PACKET_SIZE = 1024; +static constexpr size_t MIN_PAYLOAD_LEN = SECONDARY_HEADER_LEN + CRC_LEN; +static constexpr size_t MIN_TC_LEN = ccsds::HEADER_LEN + MIN_PAYLOAD_LEN; +static constexpr size_t PAYLOAD_OFFSET = ccsds::HEADER_LEN + SECONDARY_HEADER_LEN; + struct UpdateParams { std::string file; uint8_t memId; @@ -335,29 +355,6 @@ enum PoolIds : lp_id_t { ADC_ENG_15 }; -static constexpr uint16_t DEFAULT_SEQUENCE_COUNT = 1; -static const uint8_t HK_SET_ENTRIES = 13; -static const uint8_t BOOT_REPORT_SET_ENTRIES = 10; -static const uint8_t LATCHUP_RPT_SET_ENTRIES = 16; -static const uint8_t LOGGING_RPT_SET_ENTRIES = 16; -static const uint8_t ADC_RPT_SET_ENTRIES = 32; - -static const uint32_t HK_SET_ID = HK_REPORT; -static const uint32_t BOOT_REPORT_SET_ID = BOOT_STATUS_REPORT; -static const uint32_t LATCHUP_RPT_ID = LATCHUP_REPORT; -static const uint32_t LOGGING_RPT_ID = LOGGING_REPORT; -static const uint32_t ADC_REPORT_SET_ID = ADC_REPORT; - -namespace recv_timeout { -// Erase memory can require up to 60 seconds for execution -static const uint32_t ERASE_MEMORY = 60000; -static const uint32_t UPDATE_STATUS_REPORT = 70000; -} // namespace recv_timeout - -static constexpr size_t TIMESTAMP_LEN = 7; -static constexpr size_t SECONDARY_HEADER_LEN = TIMESTAMP_LEN + 1; -static constexpr size_t CRC_LEN = 2; - struct SupvTcParams : public ploc::SpTcParams { public: SupvTcParams(SpacePacketCreator& creator) : ploc::SpTcParams(creator) {} @@ -366,20 +363,49 @@ struct SupvTcParams : public ploc::SpTcParams { : ploc::SpTcParams(creator, buf, maxSize) {} void setLenFromPayloadLen(size_t payloadLen) { - setFullPacketLen(ccsds::HEADER_LEN + SECONDARY_HEADER_LEN + payloadLen + CRC_LEN); + setFullPayloadLen(ccsds::HEADER_LEN + SECONDARY_HEADER_LEN + payloadLen + CRC_LEN); } }; class SupvTcBase : public ploc::SpTcBase { public: - SupvTcBase(SupvTcParams params) : ploc::SpTcBase(params) {} + SupvTcBase(SupvTcParams params) : ploc::SpTcBase(params) { setup(); } - SupvTcBase(SupvTcParams params, uint16_t apid, uint16_t seqCount) - : ploc::SpTcBase(params, apid, seqCount) {} + SupvTcBase(SupvTcParams params, uint16_t apid, uint8_t serviceId, uint16_t seqCount) + : ploc::SpTcBase(params, apid, seqCount), serviceId(serviceId) { + if (setup() == OK) { + params.buf + supv::PAYLOAD_OFFSET = serviceId; + } + } + + private: + uint8_t serviceId = 0; + + ReturnValue_t setup() { + if (spParams.maxSize < MIN_PAYLOAD_LEN) { + sif::error << "SupvTcBase::SupvTcBase: Passed buffer is too small" << std::endl; + return returnvalue::FAILED; + } + std::memset(spParams.buf + ccsds::HEADER_LEN, 0, TIMESTAMP_LEN); + return returnvalue::OK; + } +}; + +class NoPayloadPacket : public SupvTcBase { + public: + NoPayloadPacket(SupvTcParams params, uint16_t apid, uint8_t serviceId, uint16_t seqCount) + : SupvTcBase(params, apid, serviceId, seqCount) {} + + ReturnValue_t buildPacket() { + ReturnValue_t result = checkSizeAndSerializeHeader(); + if (result != OK) { + return result; + } + return calcAndSetCrc(); + } private: }; - /** * @brief This class creates a space packet containing only the header data and the CRC. */ @@ -393,7 +419,7 @@ class ApidOnlyPacket : public ploc::SpTcBase { * @note Sequence count of empty packet is always 1. */ ApidOnlyPacket(ploc::SpTcParams params, uint16_t apid) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(LENGTH_EMPTY_TC); + spParams.setFullPayloadLen(MIN_TC_LEN); spParams.creator.setApid(apid); } @@ -402,7 +428,7 @@ class ApidOnlyPacket : public ploc::SpTcBase { if (res != returnvalue::OK) { return res; } - return calcCrc(); + return calcAndSetCrc(); } private: @@ -428,7 +454,7 @@ class MPSoCBootSelect : public ploc::SpTcBase { * @note Selection of partitions is currently not supported. */ MPSoCBootSelect(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SEL_MPSOC_BOOT_IMAGE); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -439,7 +465,7 @@ class MPSoCBootSelect : public ploc::SpTcBase { return res; } initPacket(mem, bp0, bp1, bp2); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -474,7 +500,7 @@ class EnableNvms : public ploc::SpTcBase { * @param bp2 Partition pin 2 */ EnableNvms(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_ENABLE_NVMS); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -485,7 +511,7 @@ class EnableNvms : public ploc::SpTcBase { return res; } initPacket(nvm01, nvm3); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -504,7 +530,7 @@ class EnableNvms : public ploc::SpTcBase { class SetTimeRef : public ploc::SpTcBase { public: SetTimeRef(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_TIME_REF); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -518,7 +544,7 @@ class SetTimeRef : public ploc::SpTcBase { if (res != returnvalue::OK) { return res; } - return calcCrc(); + return calcAndSetCrc(); } private: @@ -583,7 +609,7 @@ class SetBootTimeout : public ploc::SpTcBase { * @param timeout The boot timeout in milliseconds. */ SetBootTimeout(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_BOOT_TIMEOUT); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -594,7 +620,7 @@ class SetBootTimeout : public ploc::SpTcBase { return res; } initPacket(timeout); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -620,7 +646,7 @@ class SetRestartTries : public ploc::SpTcBase { * @param restartTries Maximum restart tries to set. */ SetRestartTries(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_MAX_RESTART_TRIES); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -631,7 +657,7 @@ class SetRestartTries : public ploc::SpTcBase { return res; } initPacket(restartTries); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -654,7 +680,7 @@ class DisablePeriodicHkTransmission : public ploc::SpTcBase { * @brief Constructor */ DisablePeriodicHkTransmission(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_DISABLE_HK); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -665,7 +691,7 @@ class DisablePeriodicHkTransmission : public ploc::SpTcBase { return res; } initPacket(); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -690,7 +716,7 @@ class LatchupAlert : public ploc::SpTcBase { * 3 - 3.3V, 4 - NVM_4XO, 5 - MISSION, 6 - SAFECOTS) */ LatchupAlert(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -705,7 +731,7 @@ class LatchupAlert : public ploc::SpTcBase { return res; } initPacket(latchupId); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -728,7 +754,7 @@ class SetAlertlimit : public ploc::SpTcBase { * @param dutycycle */ SetAlertlimit(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_ALERT_LIMIT); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -742,7 +768,7 @@ class SetAlertlimit : public ploc::SpTcBase { if (res != returnvalue::OK) { return res; } - return calcCrc(); + return calcAndSetCrc(); } private: @@ -771,7 +797,7 @@ class SetAdcEnabledChannels : public ploc::SpTcBase { * @param ch Defines channels to be enabled or disabled. */ SetAdcEnabledChannels(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_ADC_ENABLED_CHANNELS); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -782,7 +808,7 @@ class SetAdcEnabledChannels : public ploc::SpTcBase { return res; } initPacket(ch); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -810,7 +836,7 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { * @param stridingStepSize */ SetAdcWindowAndStride(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_ADC_WINDOW_AND_STRIDE); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -821,7 +847,7 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { return res; } initPacket(windowSize, stridingStepSize); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -850,7 +876,7 @@ class SetAdcThreshold : public ploc::SpTcBase { * @param threshold */ SetAdcThreshold(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_ADC_THRESHOLD); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -861,7 +887,7 @@ class SetAdcThreshold : public ploc::SpTcBase { return res; } initPacket(threshold); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -888,7 +914,7 @@ class RunAutoEmTests : public ploc::SpTcBase { * @param test 1 - complete EM test, 2 - Short test (only memory readback NVM0,1,3) */ RunAutoEmTests(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_RUN_AUTO_EM_TESTS); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -899,7 +925,7 @@ class RunAutoEmTests : public ploc::SpTcBase { return res; } initPacket(test); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -930,7 +956,7 @@ class MramCmd : public ploc::SpTcBase { * @note The content at the stop address is excluded from the dump or wipe operation. */ MramCmd(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -947,7 +973,7 @@ class MramCmd : public ploc::SpTcBase { return res; } initPacket(start, stop); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -984,7 +1010,7 @@ class SetGpio : public ploc::SpTcBase { * @param val */ SetGpio(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_SET_GPIO); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -995,7 +1021,7 @@ class SetGpio : public ploc::SpTcBase { return res; } initPacket(port, pin, val); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -1028,7 +1054,7 @@ class ReadGpio : public ploc::SpTcBase { * @param pin */ ReadGpio(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(DATA_FIELD_LENGTH); + spParams.setFullPayloadLen(DATA_FIELD_LENGTH); spParams.creator.setApid(APID_READ_GPIO); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1039,7 +1065,7 @@ class ReadGpio : public ploc::SpTcBase { return res; } initPacket(port, pin); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -1085,7 +1111,7 @@ class FactoryReset : public ploc::SpTcBase { return res; } initPacket(op); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -1105,14 +1131,14 @@ class FactoryReset : public ploc::SpTcBase { default: break; } - spParams.setFullPacketLen(packetDataLen); + spParams.setFullPayloadLen(packetDataLen); } }; class SetShutdownTimeout : public ploc::SpTcBase { public: SetShutdownTimeout(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(PAYLOAD_LEN + 2); + spParams.setFullPayloadLen(PAYLOAD_LEN + 2); spParams.creator.setApid(APID_SET_SHUTDOWN_TIMEOUT); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1123,7 +1149,7 @@ class SetShutdownTimeout : public ploc::SpTcBase { return res; } initPacket(timeout); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -1151,7 +1177,7 @@ class CheckMemory : public ploc::SpTcBase { * @param length Length in bytes of memory region */ CheckMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); + spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_CHECK_MEMORY); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1162,7 +1188,7 @@ class CheckMemory : public ploc::SpTcBase { return res; } initPacket(memoryId, startAddress, length); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -1216,7 +1242,7 @@ class WriteMemory : public ploc::SpTcBase { if (res != returnvalue::OK) { return res; } - return calcCrc(); + return calcAndSetCrc(); } // Although the space packet has space left for 1010 bytes of data to supervisor can only process // update packets with a maximum of 512 bytes. @@ -1230,9 +1256,9 @@ class WriteMemory : public ploc::SpTcBase { uint8_t* updateData) { uint8_t* data = payloadStart; if (updateDataLen % 2 != 0) { - spParams.setFullPacketLen(META_DATA_LENGTH + updateDataLen + 1 + 2); + spParams.setFullPayloadLen(META_DATA_LENGTH + updateDataLen + 1 + 2); } else { - spParams.setFullPacketLen(META_DATA_LENGTH + updateDataLen + 2); + spParams.setFullPayloadLen(META_DATA_LENGTH + updateDataLen + 2); } // To avoid crashes in this unexpected case ReturnValue_t result = checkPayloadLen(); @@ -1264,7 +1290,7 @@ class WriteMemory : public ploc::SpTcBase { class EraseMemory : public ploc::SpTcBase { public: EraseMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); + spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_ERASE_MEMORY); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1275,7 +1301,7 @@ class EraseMemory : public ploc::SpTcBase { return res; } initPacket(memoryId, startAddress, length); - return calcCrc(); + return calcAndSetCrc(); } private: @@ -1306,7 +1332,7 @@ class EraseMemory : public ploc::SpTcBase { class EnableAutoTm : public ploc::SpTcBase { public: EnableAutoTm(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); + spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_AUTO_TM); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1317,7 +1343,7 @@ class EnableAutoTm : public ploc::SpTcBase { return res; } payloadStart[0] = ENABLE; - return calcCrc(); + return calcAndSetCrc(); } private: @@ -1331,7 +1357,7 @@ class EnableAutoTm : public ploc::SpTcBase { class DisableAutoTm : public ploc::SpTcBase { public: DisableAutoTm(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); + spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_AUTO_TM); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1342,7 +1368,7 @@ class DisableAutoTm : public ploc::SpTcBase { return res; } payloadStart[0] = DISABLE; - return calcCrc(); + return calcAndSetCrc(); } private: @@ -1366,7 +1392,7 @@ class RequestLoggingData : public ploc::SpTcBase { }; RequestLoggingData(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPacketLen(PAYLOAD_LENGTH + 2); + spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); spParams.creator.setApid(APID_REQUEST_LOGGING_DATA); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1383,7 +1409,7 @@ class RequestLoggingData : public ploc::SpTcBase { } payloadStart[0] = static_cast(sa); payloadStart[1] = tpc; - return calcCrc(); + return calcAndSetCrc(); } private: diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 95f64510..c2797494 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -1028,7 +1028,7 @@ ReturnValue_t PlocSupervisorHandler::handleHkReport(const uint8_t* data) { return result; } - uint16_t offset = supv::DATA_FIELD_OFFSET; + uint16_t offset = supv::PAYLOAD_OFFSET; hkset.tempPs = *(data + offset) << 24 | *(data + offset + 1) << 16 | *(data + offset + 2) << 8 | *(data + offset + 3); offset += 4; @@ -1106,7 +1106,7 @@ ReturnValue_t PlocSupervisorHandler::handleBootStatusReport(const uint8_t* data) return result; } - uint16_t offset = supv::DATA_FIELD_OFFSET; + uint16_t offset = supv::PAYLOAD_OFFSET; bootStatusReport.socState = *(data + offset); offset += 1; bootStatusReport.powerCycles = *(data + offset); @@ -1171,7 +1171,7 @@ ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* da return result; } - uint16_t offset = supv::DATA_FIELD_OFFSET; + uint16_t offset = supv::PAYLOAD_OFFSET; latchupStatusReport.id = *(data + offset); offset += 1; latchupStatusReport.cnt0 = *(data + offset) << 8 | *(data + offset + 1); @@ -1255,7 +1255,7 @@ ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) { return result; } - const uint8_t* dataField = data + supv::DATA_FIELD_OFFSET + sizeof(supv::RequestLoggingData::Sa); + const uint8_t* dataField = data + supv::PAYLOAD_OFFSET + sizeof(supv::RequestLoggingData::Sa); result = loggingReport.read(); if (result != returnvalue::OK) { return result; @@ -1291,7 +1291,7 @@ ReturnValue_t PlocSupervisorHandler::handleAdcReport(const uint8_t* data) { return result; } - const uint8_t* dataField = data + supv::DATA_FIELD_OFFSET; + const uint8_t* dataField = data + supv::PAYLOAD_OFFSET; result = adcReport.read(); if (result != returnvalue::OK) { return result; diff --git a/mission/devices/devicedefinitions/SpBase.h b/mission/devices/devicedefinitions/SpBase.h index edc91334..8a5dd079 100644 --- a/mission/devices/devicedefinitions/SpBase.h +++ b/mission/devices/devicedefinitions/SpBase.h @@ -14,12 +14,12 @@ struct SpTcParams { SpTcParams(SpacePacketCreator& creator, uint8_t* buf, size_t maxSize) : creator(creator), buf(buf), maxSize(maxSize) {} - void setFullPacketLen(size_t fullPacketLen_) { fullPacketLen = fullPacketLen_; } + void setFullPayloadLen(size_t fullPayloadLen_) { fullPayloadLen = fullPayloadLen_; } SpacePacketCreator& creator; uint8_t* buf = nullptr; size_t maxSize = 0; - size_t fullPacketLen = 0; + size_t fullPayloadLen = 0; }; class SpTcBase { @@ -37,7 +37,7 @@ class SpTcBase { } void updateSpFields() { - spParams.creator.setDataLenField(spParams.fullPacketLen - 1); + spParams.creator.setDataLenField(spParams.fullPayloadLen - 1); spParams.creator.setPacketType(ccsds::PacketType::TC); } @@ -48,7 +48,7 @@ class SpTcBase { uint16_t getApid() const { return spParams.creator.getApid(); } ReturnValue_t checkPayloadLen() { - if (ccsds::HEADER_LEN + spParams.fullPacketLen > spParams.maxSize) { + if (ccsds::HEADER_LEN + spParams.fullPayloadLen > spParams.maxSize) { return SerializeIF::BUFFER_TOO_SHORT; } @@ -69,7 +69,7 @@ class SpTcBase { return serializeHeader(); } - ReturnValue_t calcCrc() { + ReturnValue_t calcAndSetCrc() { /* Calculate crc */ uint16_t crc = CRC::crc16ccitt(spParams.buf, getFullPacketLen() - 2); From 95a3ea1d23f586d7a416f1ce56e29e662a818c18 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 7 Nov 2022 11:19:10 +0100 Subject: [PATCH 099/244] start using new packet format --- .../PlocSupervisorDefinitions.h | 51 ++++----------- linux/devices/ploc/PlocSupervisorHandler.cpp | 64 ++++++++++--------- linux/devices/ploc/PlocSupervisorHandler.h | 4 +- linux/devices/ploc/PlocSupvUartMan.cpp | 4 +- linux/devices/ploc/PlocSupvUartMan.h | 2 +- 5 files changed, 53 insertions(+), 72 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index cdc5f24a..257a8c53 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -355,11 +355,11 @@ enum PoolIds : lp_id_t { ADC_ENG_15 }; -struct SupvTcParams : public ploc::SpTcParams { +struct TcParams : public ploc::SpTcParams { public: - SupvTcParams(SpacePacketCreator& creator) : ploc::SpTcParams(creator) {} + TcParams(SpacePacketCreator& creator) : ploc::SpTcParams(creator) {} - SupvTcParams(SpacePacketCreator& creator, uint8_t* buf, size_t maxSize) + TcParams(SpacePacketCreator& creator, uint8_t* buf, size_t maxSize) : ploc::SpTcParams(creator, buf, maxSize) {} void setLenFromPayloadLen(size_t payloadLen) { @@ -367,14 +367,14 @@ struct SupvTcParams : public ploc::SpTcParams { } }; -class SupvTcBase : public ploc::SpTcBase { +class TcBase : public ploc::SpTcBase { public: - SupvTcBase(SupvTcParams params) : ploc::SpTcBase(params) { setup(); } + TcBase(TcParams params) : ploc::SpTcBase(params) { setup(); } - SupvTcBase(SupvTcParams params, uint16_t apid, uint8_t serviceId, uint16_t seqCount) + TcBase(TcParams params, uint16_t apid, uint8_t serviceId, uint16_t seqCount) : ploc::SpTcBase(params, apid, seqCount), serviceId(serviceId) { - if (setup() == OK) { - params.buf + supv::PAYLOAD_OFFSET = serviceId; + if (setup() == OK and params.buf != nullptr) { + params.buf[supv::PAYLOAD_OFFSET] = serviceId; } } @@ -391,11 +391,13 @@ class SupvTcBase : public ploc::SpTcBase { } }; -class NoPayloadPacket : public SupvTcBase { +class NoPayloadPacket : public TcBase { public: - NoPayloadPacket(SupvTcParams params, uint16_t apid, uint8_t serviceId, uint16_t seqCount) - : SupvTcBase(params, apid, serviceId, seqCount) {} + NoPayloadPacket(TcParams params, uint16_t apid, uint8_t serviceId) + : NoPayloadPacket(params, apid, serviceId, 0) {} + NoPayloadPacket(TcParams params, uint16_t apid, uint8_t serviceId, uint16_t seqId) + : TcBase(params, apid, serviceId, seqId) {} ReturnValue_t buildPacket() { ReturnValue_t result = checkSizeAndSerializeHeader(); if (result != OK) { @@ -406,33 +408,6 @@ class NoPayloadPacket : public SupvTcBase { private: }; -/** - * @brief This class creates a space packet containing only the header data and the CRC. - */ -class ApidOnlyPacket : public ploc::SpTcBase { - public: - /** - * @brief Constructor - * - * @param apid The APID to set in the space packet. - * - * @note Sequence count of empty packet is always 1. - */ - ApidOnlyPacket(ploc::SpTcParams params, uint16_t apid) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(MIN_TC_LEN); - spParams.creator.setApid(apid); - } - - ReturnValue_t buildPacket() { - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - return calcAndSetCrc(); - } - - private: -}; /** * @brief This class can be used to generate the space packet selecting the boot image of diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index c2797494..853741cf 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -221,17 +221,17 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d spParams.buf = commandBuffer; switch (deviceCommand) { case GET_HK_REPORT: { - prepareEmptyCmd(APID_GET_HK_REPORT); + prepareEmptyCmd(APID_HK, static_cast(HkServiceIds::GET_REPORT)); result = returnvalue::OK; break; } case START_MPSOC: { - prepareEmptyCmd(APID_START_MPSOC); + prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::START_MPSOC)); result = returnvalue::OK; break; } case SHUTDOWN_MPSOC: { - prepareEmptyCmd(APID_SHUTWOWN_MPSOC); + prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::SHUTDOWN_MPSOC)); result = returnvalue::OK; break; } @@ -241,7 +241,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case RESET_MPSOC: { - prepareEmptyCmd(APID_RESET_MPSOC); + prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::RESET_MPSOC)); result = returnvalue::OK; break; } @@ -265,7 +265,8 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case GET_BOOT_STATUS_REPORT: { - prepareEmptyCmd(APID_GET_BOOT_STATUS_RPT); + prepareEmptyCmd(APID_BOOT_MAN, + static_cast(BootManServiceIds::GET_BOOT_STATUS_REPORT)); result = returnvalue::OK; break; } @@ -297,20 +298,22 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case GET_LATCHUP_STATUS_REPORT: { - prepareEmptyCmd(APID_GET_LATCHUP_STATUS_REPORT); - result = returnvalue::OK; - break; - } - case COPY_ADC_DATA_TO_MRAM: { - prepareEmptyCmd(APID_COPY_ADC_DATA_TO_MRAM); - result = returnvalue::OK; - break; - } - case REQUEST_ADC_REPORT: { - prepareEmptyCmd(APID_REQUEST_ADC_REPORT); + prepareEmptyCmd(APID_LATCHUP_MON, + static_cast(LatchupMonServiceIds::GET_STATUS_REPORT)); result = returnvalue::OK; break; } + // I think this is disabled right now according to the TC excel table + // case COPY_ADC_DATA_TO_MRAM: { + // prepareEmptyCmd(APID_COPY_ADC_DATA_TO_MRAM); + // result = returnvalue::OK; + // break; + // } + // case REQUEST_ADC_REPORT: { + // prepareEmptyCmd(APID_REQUEST_ADC_REPORT); + // result = returnvalue::OK; + // break; + // } case RUN_AUTO_EM_TESTS: { result = prepareRunAutoEmTest(commandData); break; @@ -333,11 +336,11 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d result = returnvalue::OK; break; } - case RESTART_SUPERVISOR: { - prepareEmptyCmd(APID_RESTART_SUPERVISOR); - result = returnvalue::OK; - break; - } + // case RESTART_SUPERVISOR: { + // prepareEmptyCmd(APID_RESTART_SUPERVISOR); + // result = returnvalue::OK; + // break; + // } case FACTORY_RESET_CLEAR_ALL: { FactoryReset packet(spParams); result = packet.buildPacket(FactoryReset::Op::CLEAR_ALL); @@ -365,18 +368,19 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d finishTcPrep(packet.getFullPacketLen()); break; } - case START_MPSOC_QUIET: { - prepareEmptyCmd(APID_START_MPSOC_QUIET); - result = returnvalue::OK; - break; - } + // Removed command + // case START_MPSOC_QUIET: { + // prepareEmptyCmd(APID_START_MPSOC_QUIET); + // result = returnvalue::OK; + // break; + // } case SET_SHUTDOWN_TIMEOUT: { prepareSetShutdownTimeoutCmd(commandData); result = returnvalue::OK; break; } case FACTORY_FLASH: { - prepareEmptyCmd(APID_FACTORY_FLASH); + prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::FACTORY_FLASH)); result = returnvalue::OK; break; } @@ -430,7 +434,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case RESET_PL: { - prepareEmptyCmd(APID_RESET_PL); + prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::RESET_PL)); result = returnvalue::OK; break; } @@ -1414,8 +1418,8 @@ void PlocSupervisorHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, } } -ReturnValue_t PlocSupervisorHandler::prepareEmptyCmd(uint16_t apid) { - supv::ApidOnlyPacket packet(spParams, apid); +ReturnValue_t PlocSupervisorHandler::prepareEmptyCmd(uint16_t apid, uint8_t serviceId) { + supv::NoPayloadPacket packet(spParams, apid, serviceId); ReturnValue_t result = packet.buildPacket(); if (result != returnvalue::OK) { return result; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index d9776c36..d731a314 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -102,7 +102,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]; SpacePacketCreator creator; - ploc::SpTcParams spParams = ploc::SpTcParams(creator); + supv::TcParams spParams = supv::TcParams(creator); /** * This variable is used to store the id of the next reply to receive. This is necessary @@ -236,7 +236,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { * @brief This function prepares a space packet which does not transport any data in the * packet data field apart from the crc. */ - ReturnValue_t prepareEmptyCmd(uint16_t apid); + ReturnValue_t prepareEmptyCmd(uint16_t apid, uint8_t serviceId); /** * @brief This function initializes the space packet to select the boot image of the MPSoC. diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 5bab8d13..ce197cf4 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -24,6 +24,7 @@ #include "mission/utility/Timestamp.h" using namespace returnvalue; +using namespace supv; PlocSupvHelper::PlocSupvHelper(object_id_t objectId) : SystemObject(objectId), @@ -498,7 +499,8 @@ ReturnValue_t PlocSupvHelper::selectMemory() { ReturnValue_t PlocSupvHelper::prepareUpdate() { ReturnValue_t result = returnvalue::OK; resetSpParams(); - supv::ApidOnlyPacket packet(spParams, supv::APID_PREPARE_UPDATE); + supv::NoPayloadPacket packet(spParams, supv::APID_BOOT_MAN, + static_cast(BootManServiceIds::PREPARE_UPDATE)); result = packet.buildPacket(); if (result != returnvalue::OK) { return result; diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 6781ec67..c0f23f22 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -238,7 +238,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]{}; SpacePacketCreator creator; - ploc::SpTcParams spParams = ploc::SpTcParams(creator); + supv::TcParams spParams = supv::TcParams(creator); std::array tmBuf{}; From f32f68d0e00fd1c789e7ca9d9bfb3417a9d4bb2c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 7 Nov 2022 11:26:36 +0100 Subject: [PATCH 100/244] continue switch to new packet format --- .../PlocSupervisorDefinitions.h | 16 +++++++++------- linux/devices/ploc/PlocSupervisorHandler.cpp | 2 +- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 257a8c53..e0c4f960 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -378,6 +378,10 @@ class TcBase : public ploc::SpTcBase { } } + void setLenFromPayloadLen(size_t payloadLen) { + spParams.setFullPayloadLen(ccsds::HEADER_LEN + SECONDARY_HEADER_LEN + payloadLen + CRC_LEN); + } + private: uint8_t serviceId = 0; @@ -387,6 +391,7 @@ class TcBase : public ploc::SpTcBase { return returnvalue::FAILED; } std::memset(spParams.buf + ccsds::HEADER_LEN, 0, TIMESTAMP_LEN); + payloadStart = spParams.buf + SECONDARY_HEADER_LEN; return returnvalue::OK; } }; @@ -413,7 +418,7 @@ class NoPayloadPacket : public TcBase { * @brief This class can be used to generate the space packet selecting the boot image of * of the MPSoC. */ -class MPSoCBootSelect : public ploc::SpTcBase { +class MPSoCBootSelect : public TcBase { public: static const uint8_t NVM0 = 0; static const uint8_t NVM1 = 1; @@ -428,8 +433,8 @@ class MPSoCBootSelect : public ploc::SpTcBase { * * @note Selection of partitions is currently not supported. */ - MPSoCBootSelect(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); + MPSoCBootSelect(TcParams params) : TcBase(params) { + params.setLenFromPayloadLen(4); spParams.creator.setApid(APID_SEL_MPSOC_BOOT_IMAGE); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -444,16 +449,13 @@ class MPSoCBootSelect : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 6; - static const uint8_t MEM_OFFSET = 0; static const uint8_t BP0_OFFSET = 1; static const uint8_t BP1_OFFSET = 2; static const uint8_t BP2_OFFSET = 3; - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; void initPacket(uint8_t mem = 0, uint8_t bp0 = 0, uint8_t bp1 = 0, uint8_t bp2 = 0) { - std::memcpy(payloadStart + MEM_OFFSET, &mem, sizeof(mem)); + payloadStart[0] = mem; std::memcpy(payloadStart + BP0_OFFSET, &bp0, sizeof(bp0)); std::memcpy(payloadStart + BP1_OFFSET, &bp1, sizeof(bp1)); std::memcpy(payloadStart + BP2_OFFSET, &bp2, sizeof(bp2)); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 853741cf..65eda209 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -1431,7 +1431,7 @@ ReturnValue_t PlocSupervisorHandler::prepareEmptyCmd(uint16_t apid, uint8_t serv ReturnValue_t PlocSupervisorHandler::prepareSelBootImageCmd(const uint8_t* commandData) { supv::MPSoCBootSelect packet(spParams); ReturnValue_t result = - packet.buildPacket(*commandData, *(commandData + 1), *(commandData + 2), *(commandData + 3)); + packet.buildPacket(commandData[0], commandData[1], commandData[2], commandData[3]); if (result != returnvalue::OK) { return result; } From e46bb42266c4d9d282c58cea774a6ea7d8ba4f4a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 7 Nov 2022 13:37:51 +0100 Subject: [PATCH 101/244] continue switch to new packet format --- .../PlocSupervisorDefinitions.h | 332 +++++++----------- linux/devices/ploc/PlocSupervisorHandler.cpp | 48 +-- linux/devices/ploc/PlocSupervisorHandler.h | 2 +- linux/devices/ploc/PlocSupvUartMan.cpp | 2 +- mission/devices/devicedefinitions/SpBase.h | 15 +- 5 files changed, 171 insertions(+), 228 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index e0c4f960..e34f6ab0 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -33,6 +33,8 @@ typedef struct { uint8_t tm_year; // years since 1900 } tas_time_t; +static constexpr uint16_t DEFAULT_SEQ_COUNT = 0; + /** Command IDs */ static const DeviceCommandId_t NONE = 0; static const DeviceCommandId_t GET_HK_REPORT = 1; @@ -122,14 +124,16 @@ static const uint16_t APID_DATA_LOGGER_DATA = 0x20D; // 2 bits APID SRC, 00 for OBC, 2 bits APID DEST, 01 for SUPV, 7 bits CMD ID -> Mask 0x080 static constexpr uint16_t APID_TC_SUPV_MASK = 0x080; -static constexpr uint16_t APID_TMTC_MAN = 0x00; -static constexpr uint16_t APID_HK = 0x01; -static constexpr uint16_t APID_BOOT_MAN = 0x02; -static constexpr uint16_t APID_LATCHUP_MON = 0x03; -static constexpr uint16_t APID_ADC_MON = 0x04; -static constexpr uint16_t APID_MEM_MAN = 0x05; -static constexpr uint16_t APID_DATA_LOGGER = 0x06; -static constexpr uint16_t APID_WDOG_MAN = 0x07; +enum Apids { + TMTC_MAN = 0x00, + HK = 0x01, + BOOT_MAN = 0x02, + LATCHUP_MON = 0x03, + ADC_MON = 0x04, + MEM_MAN = 0x05, + DATA_LOGGER = 0x06, + WDOG_MAN = 0x07 +}; enum class HkServiceIds : uint8_t { ENABLE = 0x01, @@ -170,7 +174,13 @@ enum class LatchupMonServiceIds : uint8_t { // Right now, none of the commands seem to be implemented, but still // keep the enum here in case some are added -enum class AdcMonServiceIds : uint8_t {}; +enum class AdcMonServiceIds : uint8_t { + SET_SWEEP_PERIOD = 0x01, + SET_ENABLED_CHANNELS = 0x02, + SET_WINDOW_STRIDE = 0x03, + SET_ADC_THRESHOLD = 0x04, + COPY_ADC_DATA_TO_MRAM = 0x05 +}; enum class DataLoggerServiceIds : uint8_t { FACTORY_RESET = 0x07 }; @@ -178,49 +188,9 @@ enum class DataLoggerServiceIds : uint8_t { FACTORY_RESET = 0x07 }; // keep the enum here in case some are added enum class WdogManServiceIds : uint8_t {}; -static const uint16_t APID_START_MPSOC = 0xA1; -static const uint16_t APID_SHUTWOWN_MPSOC = 0xA2; -static const uint16_t APID_SEL_MPSOC_BOOT_IMAGE = 0xA3; -static const uint16_t APID_SET_BOOT_TIMEOUT = 0xA4; -static const uint16_t APID_SET_MAX_RESTART_TRIES = 0xA5; -static const uint16_t APID_RESET_MPSOC = 0xA6; -static const uint16_t APID_RESET_PL = 0xA7; -static const uint16_t APID_GET_BOOT_STATUS_RPT = 0xA8; -static const uint16_t APID_PREPARE_UPDATE = 0xA9; -static const uint16_t APID_START_MPSOC_QUIET = 0xAA; -static const uint16_t APID_SET_SHUTDOWN_TIMEOUT = 0xAB; -static const uint16_t APID_FACTORY_FLASH = 0xAC; -static const uint16_t APID_ERASE_MEMORY = 0xB0; -static const uint16_t APID_WRITE_MEMORY = 0xB1; -static const uint16_t APID_CHECK_MEMORY = 0xB2; -static const uint16_t APID_SET_TIME_REF = 0xC2; -static const uint16_t APID_DISABLE_HK = 0xC3; -static const uint16_t APID_AUTO_TM = 0xC5; -static const uint16_t APID_ENABLE_LATCHUP_ALERT = 0xD0; -static const uint16_t APID_DISABLE_LATCHUP_ALERT = 0xD1; -static const uint16_t APID_SET_ALERT_LIMIT = 0xD3; -static const uint16_t APID_SET_ADC_ENABLED_CHANNELS = 0xE1; -static const uint16_t APID_SET_ADC_WINDOW_AND_STRIDE = 0xE2; -static const uint16_t APID_SET_ADC_THRESHOLD = 0xE3; -static const uint16_t APID_GET_LATCHUP_STATUS_REPORT = 0xD9; -static const uint16_t APID_COPY_ADC_DATA_TO_MRAM = 0xDA; -static const uint16_t APID_REQUEST_ADC_REPORT = 0xDB; -static const uint16_t APID_ENABLE_NVMS = 0xF0; -static const uint16_t APID_RUN_AUTO_EM_TESTS = 0xF2; -static const uint16_t APID_WIPE_MRAM = 0xF3; -static const uint16_t APID_DUMP_MRAM = 0xF4; -static const uint16_t APID_SET_GPIO = 0xF9; -static const uint16_t APID_READ_GPIO = 0xFA; -static const uint16_t APID_RESTART_SUPERVISOR = 0xFB; -static const uint16_t APID_FACTORY_RESET = 0xFC; -static const uint16_t APID_REQUEST_LOGGING_DATA = 0xFD; - -static const uint16_t APID_GET_HK_REPORT = 0xC6; - static const uint16_t APID_MASK = 0x3FF; static const uint16_t SEQUENCE_COUNT_MASK = 0xFFF; -static constexpr uint16_t DEFAULT_SEQUENCE_COUNT = 1; static const uint8_t HK_SET_ENTRIES = 13; static const uint8_t BOOT_REPORT_SET_ENTRIES = 10; static const uint8_t LATCHUP_RPT_SET_ENTRIES = 16; @@ -369,29 +339,43 @@ struct TcParams : public ploc::SpTcParams { class TcBase : public ploc::SpTcBase { public: - TcBase(TcParams params) : ploc::SpTcBase(params) { setup(); } + TcBase(TcParams params) : TcBase(params, 0x00, 0x00, MIN_PAYLOAD_LEN) {} - TcBase(TcParams params, uint16_t apid, uint8_t serviceId, uint16_t seqCount) - : ploc::SpTcBase(params, apid, seqCount), serviceId(serviceId) { - if (setup() == OK and params.buf != nullptr) { - params.buf[supv::PAYLOAD_OFFSET] = serviceId; + TcBase(TcParams params, uint16_t apid) : TcBase(params, apid, 0x00, MIN_PAYLOAD_LEN) {} + + TcBase(TcParams params, uint16_t apid, uint8_t service, size_t payloadLen) + : TcBase(params, apid, service, payloadLen, DEFAULT_SEQ_COUNT) {} + + TcBase(TcParams params, uint16_t apid, uint8_t serviceId, size_t payloadLen, uint16_t seqCount) + : ploc::SpTcBase(params, apid, fullSpDataLenFromPayloadLen(payloadLen), seqCount) { + setup(serviceId); + } + + void setServiceId(uint8_t id) { + if (spParams.maxSize < MIN_PAYLOAD_LEN) { + return; } + payloadStart[supv::PAYLOAD_OFFSET] = id; + } + + static size_t fullSpDataLenFromPayloadLen(size_t payloadLen) { + return SECONDARY_HEADER_LEN + payloadLen + CRC_LEN; } void setLenFromPayloadLen(size_t payloadLen) { - spParams.setFullPayloadLen(ccsds::HEADER_LEN + SECONDARY_HEADER_LEN + payloadLen + CRC_LEN); + spParams.setFullPayloadLen(fullSpDataLenFromPayloadLen(payloadLen)); + updateLenFromParams(); } private: - uint8_t serviceId = 0; - - ReturnValue_t setup() { + ReturnValue_t setup(uint8_t serviceId) { if (spParams.maxSize < MIN_PAYLOAD_LEN) { sif::error << "SupvTcBase::SupvTcBase: Passed buffer is too small" << std::endl; return returnvalue::FAILED; } std::memset(spParams.buf + ccsds::HEADER_LEN, 0, TIMESTAMP_LEN); - payloadStart = spParams.buf + SECONDARY_HEADER_LEN; + payloadStart = spParams.buf + ccsds::HEADER_LEN + SECONDARY_HEADER_LEN; + payloadStart[supv::PAYLOAD_OFFSET] = serviceId; return returnvalue::OK; } }; @@ -402,7 +386,8 @@ class NoPayloadPacket : public TcBase { : NoPayloadPacket(params, apid, serviceId, 0) {} NoPayloadPacket(TcParams params, uint16_t apid, uint8_t serviceId, uint16_t seqId) - : TcBase(params, apid, serviceId, seqId) {} + : TcBase(params, apid, serviceId, MIN_PAYLOAD_LEN, seqId) {} + ReturnValue_t buildPacket() { ReturnValue_t result = checkSizeAndSerializeHeader(); if (result != OK) { @@ -433,11 +418,8 @@ class MPSoCBootSelect : public TcBase { * * @note Selection of partitions is currently not supported. */ - MPSoCBootSelect(TcParams params) : TcBase(params) { - params.setLenFromPayloadLen(4); - spParams.creator.setApid(APID_SEL_MPSOC_BOOT_IMAGE); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + MPSoCBootSelect(TcParams params) + : TcBase(params, Apids::BOOT_MAN, static_cast(BootManServiceIds::SELECT_IMAGE), 4) {} ReturnValue_t buildPacket(uint8_t mem = 0, uint8_t bp0 = 0, uint8_t bp1 = 0, uint8_t bp2 = 0) { auto res = checkSizeAndSerializeHeader(); @@ -449,16 +431,11 @@ class MPSoCBootSelect : public TcBase { } private: - static const uint8_t MEM_OFFSET = 0; - static const uint8_t BP0_OFFSET = 1; - static const uint8_t BP1_OFFSET = 2; - static const uint8_t BP2_OFFSET = 3; - void initPacket(uint8_t mem = 0, uint8_t bp0 = 0, uint8_t bp1 = 0, uint8_t bp2 = 0) { payloadStart[0] = mem; - std::memcpy(payloadStart + BP0_OFFSET, &bp0, sizeof(bp0)); - std::memcpy(payloadStart + BP1_OFFSET, &bp1, sizeof(bp1)); - std::memcpy(payloadStart + BP2_OFFSET, &bp2, sizeof(bp2)); + payloadStart[1] = bp0; + payloadStart[2] = bp1; + payloadStart[3] = bp2; } }; @@ -466,51 +443,49 @@ class MPSoCBootSelect : public TcBase { * @brief This class creates the command to enable or disable the NVMs connected to the * supervisor. */ -class EnableNvms : public ploc::SpTcBase { - public: - /** - * @brief Constructor - * - * @param mem The memory to boot from: NVM0 (0), NVM1 (1) - * @param bp0 Partition pin 0 - * @param bp1 Partition pin 1 - * @param bp2 Partition pin 2 - */ - EnableNvms(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_ENABLE_NVMS); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } - - ReturnValue_t buildPacket(uint8_t nvm01, uint8_t nvm3) { - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - initPacket(nvm01, nvm3); - return calcAndSetCrc(); - } - - private: - static const uint8_t DATA_FIELD_LENGTH = 4; - static const uint8_t CRC_OFFSET = 2; - - void initPacket(uint8_t nvm01, uint8_t nvm3) { - payloadStart[0] = nvm01; - payloadStart[1] = nvm3; - } -}; +// class EnableNvms : public ploc::SpTcBase { +// public: +// /** +// * @brief Constructor +// * +// * @param mem The memory to boot from: NVM0 (0), NVM1 (1) +// * @param bp0 Partition pin 0 +// * @param bp1 Partition pin 1 +// * @param bp2 Partition pin 2 +// */ +// EnableNvms(ploc::SpTcParams params) : ploc::SpTcBase(params) { +// spParams.setFullPayloadLen(DATA_FIELD_LENGTH); +// spParams.creator.setApid(APID_ENABLE_NVMS); +// spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); +// } +// +// ReturnValue_t buildPacket(uint8_t nvm01, uint8_t nvm3) { +// auto res = checkSizeAndSerializeHeader(); +// if (res != returnvalue::OK) { +// return res; +// } +// initPacket(nvm01, nvm3); +// return calcAndSetCrc(); +// } +// +// private: +// static const uint8_t DATA_FIELD_LENGTH = 4; +// static const uint8_t CRC_OFFSET = 2; +// +// void initPacket(uint8_t nvm01, uint8_t nvm3) { +// payloadStart[0] = nvm01; +// payloadStart[1] = nvm3; +// } +// }; /** * @brief This class generates the space packet to update the time of the PLOC supervisor. */ -class SetTimeRef : public ploc::SpTcBase { +class SetTimeRef : public TcBase { public: - SetTimeRef(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_SET_TIME_REF); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + static constexpr size_t PAYLOAD_LEN = 8; + SetTimeRef(TcParams params) + : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::TIME_REF), 8) {} ReturnValue_t buildPacket(Clock::TimeOfDay_t* time) { auto res = checkSizeAndSerializeHeader(); @@ -525,8 +500,6 @@ class SetTimeRef : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 10; - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; static const uint16_t SYNC = 0x8000; ReturnValue_t initPacket(Clock::TimeOfDay_t* time) { @@ -578,18 +551,17 @@ class SetTimeRef : public ploc::SpTcBase { /** * @brief This class can be used to generate the set boot timout command. */ -class SetBootTimeout : public ploc::SpTcBase { +class SetBootTimeout : public TcBase { public: + static constexpr size_t PAYLOAD_LEN = 4; /** * @brief Constructor * * @param timeout The boot timeout in milliseconds. */ - SetBootTimeout(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_SET_BOOT_TIMEOUT); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + SetBootTimeout(TcParams params) + : TcBase(params, Apids::BOOT_MAN, static_cast(BootManServiceIds::SET_BOOT_TIMEOUT), + PAYLOAD_LEN) {} ReturnValue_t buildPacket(uint32_t timeout) { auto res = checkSizeAndSerializeHeader(); @@ -601,9 +573,6 @@ class SetBootTimeout : public ploc::SpTcBase { } private: - /** boot timeout value (uint32_t) and crc (uint16_t) */ - static const uint16_t DATA_FIELD_LENGTH = 6; - void initPacket(uint32_t timeout) { size_t serializedSize = 0; uint8_t* dataFieldPtr = payloadStart; @@ -615,18 +584,16 @@ class SetBootTimeout : public ploc::SpTcBase { /** * @brief This class can be used to generate the space packet to set the maximum boot tries. */ -class SetRestartTries : public ploc::SpTcBase { +class SetRestartTries : public TcBase { public: /** * @brief Constructor * * @param restartTries Maximum restart tries to set. */ - SetRestartTries(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_SET_MAX_RESTART_TRIES); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + SetRestartTries(TcParams params) + : TcBase(params, Apids::BOOT_MAN, + static_cast(BootManServiceIds::SET_MAX_REBOOT_TRIES), 1) {} ReturnValue_t buildPacket(uint8_t restartTries) { auto res = checkSizeAndSerializeHeader(); @@ -640,9 +607,6 @@ class SetRestartTries : public ploc::SpTcBase { private: uint8_t restartTries = 0; - /** Restart tries value (uint8_t) and crc (uint16_t) */ - static const uint16_t DATA_FIELD_LENGTH = 3; - void initPacket(uint8_t restartTries) { payloadStart[0] = restartTries; } }; @@ -651,16 +615,13 @@ class SetRestartTries : public ploc::SpTcBase { * of housekeeping data. Normally, this will be disabled by default. However, adding this * command can be useful for debugging. */ -class DisablePeriodicHkTransmission : public ploc::SpTcBase { +class DisablePeriodicHkTransmission : public TcBase { public: /** * @brief Constructor */ - DisablePeriodicHkTransmission(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_DISABLE_HK); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + DisablePeriodicHkTransmission(TcParams params) + : TcBase(params, Apids::HK, static_cast(HkServiceIds::ENABLE), 1) {} ReturnValue_t buildPacket() { auto res = checkSizeAndSerializeHeader(); @@ -672,9 +633,6 @@ class DisablePeriodicHkTransmission : public ploc::SpTcBase { } private: - /** Restart tries value (uint8_t) and crc (uint16_t) */ - static const uint16_t DATA_FIELD_LENGTH = 3; - void initPacket() { payloadStart[0] = false; } }; @@ -683,7 +641,7 @@ class DisablePeriodicHkTransmission : public ploc::SpTcBase { * * @details There are 7 different latchup alerts. */ -class LatchupAlert : public ploc::SpTcBase { +class LatchupAlert : public TcBase { public: /** * @brief Constructor @@ -692,16 +650,13 @@ class LatchupAlert : public ploc::SpTcBase { * @param latchupId Identifies the latchup to enable/disable (0 - 0.85V, 1 - 1.8V, 2 - MISC, * 3 - 3.3V, 4 - NVM_4XO, 5 - MISSION, 6 - SAFECOTS) */ - LatchupAlert(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + LatchupAlert(TcParams params) : TcBase(params, Apids::LATCHUP_MON) { setLenFromPayloadLen(1); } ReturnValue_t buildPacket(bool state, uint8_t latchupId) { if (state) { - spParams.creator.setApid(APID_ENABLE_LATCHUP_ALERT); + setServiceId(static_cast(LatchupMonServiceIds::ENABLE)); } else { - spParams.creator.setApid(APID_DISABLE_LATCHUP_ALERT); + setServiceId(static_cast(LatchupMonServiceIds::DISABLE)); } auto res = checkSizeAndSerializeHeader(); if (res != returnvalue::OK) { @@ -712,16 +667,12 @@ class LatchupAlert : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 3; - - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - uint8_t latchupId = 0; void initPacket(uint8_t latchupId) { payloadStart[0] = latchupId; } }; -class SetAlertlimit : public ploc::SpTcBase { +class SetAlertlimit : public TcBase { public: /** * @brief Constructor @@ -730,11 +681,9 @@ class SetAlertlimit : public ploc::SpTcBase { * 3 - 3.3V, 4 - NVM_4XO, 5 - MISSION, 6 - SAFECOTS) * @param dutycycle */ - SetAlertlimit(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_SET_ALERT_LIMIT); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + SetAlertlimit(TcParams params) + : TcBase(params, Apids::LATCHUP_MON, + static_cast(LatchupMonServiceIds::SET_ALERT_LIMIT), 5) {} ReturnValue_t buildPacket(uint8_t latchupId, uint32_t dutycycle) { auto res = checkSizeAndSerializeHeader(); @@ -749,9 +698,6 @@ class SetAlertlimit : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 7; - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - uint8_t latchupId = 0; uint32_t dutycycle = 0; @@ -766,18 +712,16 @@ class SetAlertlimit : public ploc::SpTcBase { /** * @brief This class packages the space packet to enable or disable ADC channels. */ -class SetAdcEnabledChannels : public ploc::SpTcBase { +class SetAdcEnabledChannels : public TcBase { public: /** * @brief Constructor * * @param ch Defines channels to be enabled or disabled. */ - SetAdcEnabledChannels(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_SET_ADC_ENABLED_CHANNELS); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + SetAdcEnabledChannels(TcParams params) + : TcBase(params, Apids::ADC_MON, static_cast(AdcMonServiceIds::SET_ENABLED_CHANNELS), + 2) {} ReturnValue_t buildPacket(uint16_t ch) { auto res = checkSizeAndSerializeHeader(); @@ -789,10 +733,6 @@ class SetAdcEnabledChannels : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 4; - - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - void initPacket(uint16_t ch) { size_t serializedSize = 0; SerializeAdapter::serialize(&ch, &payloadStart, &serializedSize, sizeof(ch), @@ -814,8 +754,8 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { */ SetAdcWindowAndStride(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_SET_ADC_WINDOW_AND_STRIDE); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setApid(APID_SET_ADC_WINDOW_AND_STRIDE); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } ReturnValue_t buildPacket(uint16_t windowSize, uint16_t stridingStepSize) { @@ -854,7 +794,7 @@ class SetAdcThreshold : public ploc::SpTcBase { */ SetAdcThreshold(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_SET_ADC_THRESHOLD); + // spParams.creator.setApid(APID_SET_ADC_THRESHOLD); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -892,7 +832,7 @@ class RunAutoEmTests : public ploc::SpTcBase { */ RunAutoEmTests(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_RUN_AUTO_EM_TESTS); + // spParams.creator.setApid(APID_RUN_AUTO_EM_TESTS); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -934,14 +874,14 @@ class MramCmd : public ploc::SpTcBase { */ MramCmd(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { if (action == MramAction::WIPE) { - spParams.creator.setApid(APID_WIPE_MRAM); + // spParams.creator.setApid(APID_WIPE_MRAM); } else if (action == MramAction::DUMP) { - spParams.creator.setApid(APID_DUMP_MRAM); + // spParams.creator.setApid(APID_DUMP_MRAM); } else { sif::debug << "WipeMram: Invalid action specified"; } @@ -988,7 +928,7 @@ class SetGpio : public ploc::SpTcBase { */ SetGpio(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_SET_GPIO); + // spParams.creator.setApid(APID_SET_GPIO); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1032,7 +972,7 @@ class ReadGpio : public ploc::SpTcBase { */ ReadGpio(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - spParams.creator.setApid(APID_READ_GPIO); + // spParams.creator.setApid(APID_READ_GPIO); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1078,7 +1018,7 @@ class FactoryReset : public ploc::SpTcBase { * @param op */ FactoryReset(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.creator.setApid(APID_FACTORY_RESET); + // spParams.creator.setApid(APID_FACTORY_RESET); spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } @@ -1116,8 +1056,8 @@ class SetShutdownTimeout : public ploc::SpTcBase { public: SetShutdownTimeout(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LEN + 2); - spParams.creator.setApid(APID_SET_SHUTDOWN_TIMEOUT); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setApid(APID_SET_SHUTDOWN_TIMEOUT); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } ReturnValue_t buildPacket(uint32_t timeout) { @@ -1155,8 +1095,8 @@ class CheckMemory : public ploc::SpTcBase { */ CheckMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - spParams.creator.setApid(APID_CHECK_MEMORY); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setApid(APID_CHECK_MEMORY); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { @@ -1202,8 +1142,8 @@ class WriteMemory : public ploc::SpTcBase { * @param updateData Pointer to buffer containing update data */ WriteMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.creator.setApid(APID_WRITE_MEMORY); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setApid(APID_WRITE_MEMORY); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } ReturnValue_t buildPacket(ccsds::SequenceFlags seqFlags, uint16_t sequenceCount, uint8_t memoryId, @@ -1268,8 +1208,8 @@ class EraseMemory : public ploc::SpTcBase { public: EraseMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - spParams.creator.setApid(APID_ERASE_MEMORY); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setApid(APID_ERASE_MEMORY); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { @@ -1310,8 +1250,8 @@ class EnableAutoTm : public ploc::SpTcBase { public: EnableAutoTm(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - spParams.creator.setApid(APID_AUTO_TM); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setApid(APID_AUTO_TM); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } ReturnValue_t buildPacket() { @@ -1335,8 +1275,8 @@ class DisableAutoTm : public ploc::SpTcBase { public: DisableAutoTm(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - spParams.creator.setApid(APID_AUTO_TM); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setApid(APID_AUTO_TM); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } ReturnValue_t buildPacket() { @@ -1370,8 +1310,8 @@ class RequestLoggingData : public ploc::SpTcBase { RequestLoggingData(ploc::SpTcParams params) : ploc::SpTcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - spParams.creator.setApid(APID_REQUEST_LOGGING_DATA); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + // spParams.creator.setApid(APID_REQUEST_LOGGING_DATA); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); } /** diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 65eda209..a5a3b5ef 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -221,17 +221,17 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d spParams.buf = commandBuffer; switch (deviceCommand) { case GET_HK_REPORT: { - prepareEmptyCmd(APID_HK, static_cast(HkServiceIds::GET_REPORT)); + prepareEmptyCmd(Apids::HK, static_cast(HkServiceIds::GET_REPORT)); result = returnvalue::OK; break; } case START_MPSOC: { - prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::START_MPSOC)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::START_MPSOC)); result = returnvalue::OK; break; } case SHUTDOWN_MPSOC: { - prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::SHUTDOWN_MPSOC)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::SHUTDOWN_MPSOC)); result = returnvalue::OK; break; } @@ -241,7 +241,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case RESET_MPSOC: { - prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::RESET_MPSOC)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::RESET_MPSOC)); result = returnvalue::OK; break; } @@ -265,7 +265,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case GET_BOOT_STATUS_REPORT: { - prepareEmptyCmd(APID_BOOT_MAN, + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::GET_BOOT_STATUS_REPORT)); result = returnvalue::OK; break; @@ -298,7 +298,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case GET_LATCHUP_STATUS_REPORT: { - prepareEmptyCmd(APID_LATCHUP_MON, + prepareEmptyCmd(Apids::LATCHUP_MON, static_cast(LatchupMonServiceIds::GET_STATUS_REPORT)); result = returnvalue::OK; break; @@ -380,7 +380,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case FACTORY_FLASH: { - prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::FACTORY_FLASH)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::FACTORY_FLASH)); result = returnvalue::OK; break; } @@ -434,14 +434,14 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case RESET_PL: { - prepareEmptyCmd(APID_BOOT_MAN, static_cast(BootManServiceIds::RESET_PL)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::RESET_PL)); result = returnvalue::OK; break; } - case ENABLE_NVMS: { - result = prepareEnableNvmsCommand(commandData); - break; - } + // case ENABLE_NVMS: { + // result = prepareEnableNvmsCommand(commandData); + // break; + // } default: sif::debug << "PlocSupervisorHandler::buildCommandFromCommand: Command not implemented" << std::endl; @@ -1701,18 +1701,18 @@ ReturnValue_t PlocSupervisorHandler::prepareLoggingRequest(const uint8_t* comman return returnvalue::OK; } -ReturnValue_t PlocSupervisorHandler::prepareEnableNvmsCommand(const uint8_t* commandData) { - using namespace supv; - uint8_t nvm01 = *(commandData); - uint8_t nvm3 = *(commandData + 1); - EnableNvms packet(spParams); - ReturnValue_t result = packet.buildPacket(nvm01, nvm3); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(packet.getFullPacketLen()); - return returnvalue::OK; -} +// ReturnValue_t PlocSupervisorHandler::prepareEnableNvmsCommand(const uint8_t* commandData) { +// using namespace supv; +// uint8_t nvm01 = *(commandData); +// uint8_t nvm3 = *(commandData + 1); +// EnableNvms packet(spParams); +// ReturnValue_t result = packet.buildPacket(nvm01, nvm3); +// if (result != returnvalue::OK) { +// return result; +// } +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } void PlocSupervisorHandler::disableAllReplies() { using namespace supv; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index d731a314..98cd22e9 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -283,7 +283,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t prepareSetGpioCmd(const uint8_t* commandData); ReturnValue_t prepareReadGpioCmd(const uint8_t* commandData); ReturnValue_t prepareLoggingRequest(const uint8_t* commandData, size_t commandDataLen); - ReturnValue_t prepareEnableNvmsCommand(const uint8_t* commandData); + // ReturnValue_t prepareEnableNvmsCommand(const uint8_t* commandData); /** * @brief Copies the content of a space packet to the command buffer. diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index ce197cf4..22484f31 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -499,7 +499,7 @@ ReturnValue_t PlocSupvHelper::selectMemory() { ReturnValue_t PlocSupvHelper::prepareUpdate() { ReturnValue_t result = returnvalue::OK; resetSpParams(); - supv::NoPayloadPacket packet(spParams, supv::APID_BOOT_MAN, + supv::NoPayloadPacket packet(spParams, Apids::BOOT_MAN, static_cast(BootManServiceIds::PREPARE_UPDATE)); result = packet.buildPacket(); if (result != returnvalue::OK) { diff --git a/mission/devices/devicedefinitions/SpBase.h b/mission/devices/devicedefinitions/SpBase.h index 8a5dd079..da2f4cd4 100644 --- a/mission/devices/devicedefinitions/SpBase.h +++ b/mission/devices/devicedefinitions/SpBase.h @@ -24,23 +24,26 @@ struct SpTcParams { class SpTcBase { public: - SpTcBase(SpTcParams params) : spParams(params) { - payloadStart = spParams.buf + ccsds::HEADER_LEN; - updateSpFields(); - } + SpTcBase(SpTcParams params) : SpTcBase(params, 0x00, 1, 0) {} - SpTcBase(SpTcParams params, uint16_t apid, uint16_t seqCount) : spParams(params) { + SpTcBase(SpTcParams params, uint16_t apid, size_t payloadLen) + : SpTcBase(params, apid, payloadLen, 0) {} + + SpTcBase(SpTcParams params, uint16_t apid, size_t payloadLen, uint16_t seqCount) + : spParams(params) { spParams.creator.setApid(apid); spParams.creator.setSeqCount(seqCount); payloadStart = spParams.buf + ccsds::HEADER_LEN; + spParams.fullPayloadLen = payloadLen; updateSpFields(); } void updateSpFields() { - spParams.creator.setDataLenField(spParams.fullPayloadLen - 1); + updateLenFromParams(); spParams.creator.setPacketType(ccsds::PacketType::TC); } + void updateLenFromParams() { spParams.creator.setDataLenField(spParams.fullPayloadLen - 1); } const uint8_t* getFullPacket() const { return spParams.buf; } size_t getFullPacketLen() const { return spParams.creator.getFullPacketLen(); } From 8dbb3dcd9b9fa08ee2222e0ab8629ae583415f40 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 7 Nov 2022 17:31:16 +0100 Subject: [PATCH 102/244] continue refactoring --- .../PlocSupervisorDefinitions.h | 329 ++++++++---------- linux/devices/ploc/PlocSupervisorHandler.cpp | 150 ++++---- linux/devices/ploc/PlocSupervisorHandler.h | 4 +- 3 files changed, 218 insertions(+), 265 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index e34f6ab0..128d988c 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -182,7 +182,13 @@ enum class AdcMonServiceIds : uint8_t { COPY_ADC_DATA_TO_MRAM = 0x05 }; -enum class DataLoggerServiceIds : uint8_t { FACTORY_RESET = 0x07 }; +enum class MemManServiceIds : uint8_t { ERASE = 0x01, WRITE = 0x02, CHECK = 0x03 }; + +enum class DataLoggerServiceIds : uint8_t { + WIPE_MRAM = 0x05, + DUMP_MRAM = 0x06, + FACTORY_RESET = 0x07 +}; // Right now, none of the commands seem to be implemented, but still // keep the enum here in case some are added @@ -744,7 +750,7 @@ class SetAdcEnabledChannels : public TcBase { * @brief This class packages the space packet to configures the window size and striding step of * the moving average filter applied to the ADC readings. */ -class SetAdcWindowAndStride : public ploc::SpTcBase { +class SetAdcWindowAndStride : public TcBase { public: /** * @brief Constructor @@ -752,11 +758,9 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { * @param windowSize * @param stridingStepSize */ - SetAdcWindowAndStride(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - // spParams.creator.setApid(APID_SET_ADC_WINDOW_AND_STRIDE); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + SetAdcWindowAndStride(TcParams params) + : TcBase(params, Apids::ADC_MON, static_cast(AdcMonServiceIds::SET_WINDOW_STRIDE), + 4) {} ReturnValue_t buildPacket(uint16_t windowSize, uint16_t stridingStepSize) { auto res = checkSizeAndSerializeHeader(); @@ -768,10 +772,6 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 6; - - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - void initPacket(uint16_t windowSize, uint16_t stridingStepSize) { size_t serializedSize = 6; uint8_t* data = payloadStart; @@ -785,18 +785,16 @@ class SetAdcWindowAndStride : public ploc::SpTcBase { /** * @brief This class packages the space packet to set the ADC trigger threshold. */ -class SetAdcThreshold : public ploc::SpTcBase { +class SetAdcThreshold : public TcBase { public: /** * @brief Constructor * * @param threshold */ - SetAdcThreshold(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - // spParams.creator.setApid(APID_SET_ADC_THRESHOLD); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + SetAdcThreshold(TcParams params) + : TcBase(params, Apids::ADC_MON, static_cast(AdcMonServiceIds::SET_ADC_THRESHOLD), + 4) {} ReturnValue_t buildPacket(uint32_t threshold) { auto res = checkSizeAndSerializeHeader(); @@ -808,11 +806,6 @@ class SetAdcThreshold : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 6; - static const uint16_t DEFAULT_SEQUENCE_COUNT = 1; - - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - void initPacket(uint32_t threshold) { size_t serializedSize = 0; SerializeAdapter::serialize(&threshold, payloadStart, &serializedSize, sizeof(threshold), @@ -823,17 +816,15 @@ class SetAdcThreshold : public ploc::SpTcBase { /** * @brief This class packages the space packet to run auto EM tests. */ -class RunAutoEmTests : public ploc::SpTcBase { +class RunAutoEmTests : public TcBase { public: /** * @brief Constructor * * @param test 1 - complete EM test, 2 - Short test (only memory readback NVM0,1,3) */ - RunAutoEmTests(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - // spParams.creator.setApid(APID_RUN_AUTO_EM_TESTS); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + RunAutoEmTests(TcParams params) + : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::RUN_AUTO_EM_TEST), 1) { } ReturnValue_t buildPacket(uint8_t test) { @@ -846,11 +837,6 @@ class RunAutoEmTests : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 3; - static const uint16_t DEFAULT_SEQUENCE_COUNT = 1; - - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - uint8_t test = 0; void initPacket(uint8_t test) { payloadStart[0] = test; } @@ -859,65 +845,62 @@ class RunAutoEmTests : public ploc::SpTcBase { /** * @brief This class packages the space packet to wipe or dump parts of the MRAM. */ -class MramCmd : public ploc::SpTcBase { - public: - enum class MramAction { WIPE, DUMP }; - - /** - * @brief Constructor - * - * @param start Start address of the MRAM section to wipe or dump - * @param stop End address of the MRAM section to wipe or dump - * @param action Dump or wipe MRAM - * - * @note The content at the stop address is excluded from the dump or wipe operation. - */ - MramCmd(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } - - ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { - if (action == MramAction::WIPE) { - // spParams.creator.setApid(APID_WIPE_MRAM); - } else if (action == MramAction::DUMP) { - // spParams.creator.setApid(APID_DUMP_MRAM); - } else { - sif::debug << "WipeMram: Invalid action specified"; - } - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - initPacket(start, stop); - return calcAndSetCrc(); - } - - private: - static const uint16_t DATA_FIELD_LENGTH = 8; - - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - - uint32_t start = 0; - uint32_t stop = 0; - - void initPacket(uint32_t start, uint32_t stop) { - uint8_t concatBuffer[6]; - concatBuffer[0] = static_cast(start >> 16); - concatBuffer[1] = static_cast(start >> 8); - concatBuffer[2] = static_cast(start); - concatBuffer[3] = static_cast(stop >> 16); - concatBuffer[4] = static_cast(stop >> 8); - concatBuffer[5] = static_cast(stop); - std::memcpy(payloadStart, concatBuffer, sizeof(concatBuffer)); - } -}; +// class MramCmd : public TcBase { +// public: +// enum class MramAction { WIPE, DUMP }; +// +// /** +// * @brief Constructor +// * +// * @param start Start address of the MRAM section to wipe or dump +// * @param stop End address of the MRAM section to wipe or dump +// * @param action Dump or wipe MRAM +// * +// * @note The content at the stop address is excluded from the dump or wipe operation. +// */ +// MramCmd(TcParams params) +// : TcBase(params, Apids::DATA_LOGGER) { +// setLenFromPayloadLen(6); +// } +// +// ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { +// if (action == MramAction::WIPE) { +// setServiceId(static_cast(DataLoggerServiceIds::WIPE_MRAM)); +// } else if (action == MramAction::DUMP) { +// setServiceId(static_cast(DataLoggerServiceIds::DUMP_MRAM)); +// } else { +// sif::debug << "WipeMram: Invalid action specified"; +// } +// auto res = checkSizeAndSerializeHeader(); +// if (res != returnvalue::OK) { +// return res; +// } +// initPacket(start, stop); +// return calcAndSetCrc(); +// } +// +// private: +// +// uint32_t start = 0; +// uint32_t stop = 0; +// +// void initPacket(uint32_t start, uint32_t stop) { +// uint8_t concatBuffer[6]; +// concatBuffer[0] = static_cast(start >> 16); +// concatBuffer[1] = static_cast(start >> 8); +// concatBuffer[2] = static_cast(start); +// concatBuffer[3] = static_cast(stop >> 16); +// concatBuffer[4] = static_cast(stop >> 8); +// concatBuffer[5] = static_cast(stop); +// std::memcpy(payloadStart, concatBuffer, sizeof(concatBuffer)); +// } +// }; /** * @brief This class packages the space packet change the state of a GPIO. This command is only * required for ground testing. */ -class SetGpio : public ploc::SpTcBase { +class SetGpio : public TcBase { public: /** * @brief Constructor @@ -926,11 +909,8 @@ class SetGpio : public ploc::SpTcBase { * @param pin * @param val */ - SetGpio(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - // spParams.creator.setApid(APID_SET_GPIO); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + SetGpio(TcParams params) + : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::SET_GPIO), 3) {} ReturnValue_t buildPacket(uint8_t port, uint8_t pin, uint8_t val) { auto res = checkSizeAndSerializeHeader(); @@ -942,11 +922,6 @@ class SetGpio : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 5; - static const uint16_t DEFAULT_SEQUENCE_COUNT = 1; - - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - uint8_t port = 0; uint8_t pin = 0; uint8_t val = 0; @@ -962,7 +937,7 @@ class SetGpio : public ploc::SpTcBase { * @brief This class packages the space packet causing the supervisor print the state of a GPIO * to the debug output. */ -class ReadGpio : public ploc::SpTcBase { +class ReadGpio : public TcBase { public: /** * @brief Constructor @@ -970,11 +945,8 @@ class ReadGpio : public ploc::SpTcBase { * @param port * @param pin */ - ReadGpio(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(DATA_FIELD_LENGTH); - // spParams.creator.setApid(APID_READ_GPIO); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + ReadGpio(TcParams params) + : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::READ_GPIO), 2) {} ReturnValue_t buildPacket(uint8_t port, uint8_t pin) { auto res = checkSizeAndSerializeHeader(); @@ -986,11 +958,6 @@ class ReadGpio : public ploc::SpTcBase { } private: - static const uint16_t DATA_FIELD_LENGTH = 4; - static const uint16_t DEFAULT_SEQUENCE_COUNT = 1; - - static const uint16_t CRC_OFFSET = DATA_FIELD_LENGTH - 2; - uint8_t port = 0; uint8_t pin = 0; @@ -1008,57 +975,56 @@ class ReadGpio : public ploc::SpTcBase { * OP = 0x01: Only the mirror entries will be wiped. * OP = 0x02: Only the circular entries will be wiped. */ -class FactoryReset : public ploc::SpTcBase { +// class FactoryReset : public TcBase { +// public: +// enum class Op { CLEAR_ALL, MIRROR_ENTRIES, CIRCULAR_ENTRIES }; +// +// /** +// * @brief Constructor +// * +// * @param op +// */ +// FactoryReset(TcParams params) +// : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::)) { +// // spParams.creator.setApid(APID_FACTORY_RESET); +// spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); +// } +// +// ReturnValue_t buildPacket(Op op) { +// auto res = checkSizeAndSerializeHeader(); +// if (res != returnvalue::OK) { +// return res; +// } +// initPacket(op); +// return calcAndSetCrc(); +// } +// +// private: +// static const uint16_t DEFAULT_SEQUENCE_COUNT = 1; +// +// void initPacket(Op op) { +// size_t packetDataLen = 2; +// switch (op) { +// case Op::MIRROR_ENTRIES: +// payloadStart[0] = 1; +// packetDataLen = 3; +// break; +// case Op::CIRCULAR_ENTRIES: +// payloadStart[0] = 2; +// packetDataLen = 3; +// break; +// default: +// break; +// } +// spParams.setFullPayloadLen(packetDataLen); +// } +// }; + +class SetShutdownTimeout : public TcBase { public: - enum class Op { CLEAR_ALL, MIRROR_ENTRIES, CIRCULAR_ENTRIES }; - - /** - * @brief Constructor - * - * @param op - */ - FactoryReset(ploc::SpTcParams params) : ploc::SpTcBase(params) { - // spParams.creator.setApid(APID_FACTORY_RESET); - spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } - - ReturnValue_t buildPacket(Op op) { - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - initPacket(op); - return calcAndSetCrc(); - } - - private: - static const uint16_t DEFAULT_SEQUENCE_COUNT = 1; - - void initPacket(Op op) { - size_t packetDataLen = 2; - switch (op) { - case Op::MIRROR_ENTRIES: - payloadStart[0] = 1; - packetDataLen = 3; - break; - case Op::CIRCULAR_ENTRIES: - payloadStart[0] = 2; - packetDataLen = 3; - break; - default: - break; - } - spParams.setFullPayloadLen(packetDataLen); - } -}; - -class SetShutdownTimeout : public ploc::SpTcBase { - public: - SetShutdownTimeout(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(PAYLOAD_LEN + 2); - // spParams.creator.setApid(APID_SET_SHUTDOWN_TIMEOUT); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + SetShutdownTimeout(TcParams params) + : TcBase(params, Apids::BOOT_MAN, static_cast(BootManServiceIds::SHUTDOWN_TIMEOUT), + 4) {} ReturnValue_t buildPacket(uint32_t timeout) { auto res = checkSizeAndSerializeHeader(); @@ -1070,10 +1036,6 @@ class SetShutdownTimeout : public ploc::SpTcBase { } private: - static const uint16_t PAYLOAD_LEN = 4; // uint32_t timeout - - uint32_t timeout = 0; - void initPacket(uint32_t timeout) { size_t serLen = 0; SerializeAdapter::serialize(&timeout, payloadStart, &serLen, sizeof(timeout), @@ -1084,7 +1046,7 @@ class SetShutdownTimeout : public ploc::SpTcBase { /** * @brief Command to request CRC over memory region of the supervisor. */ -class CheckMemory : public ploc::SpTcBase { +class CheckMemory : public TcBase { public: /** * @brief Constructor @@ -1093,11 +1055,8 @@ class CheckMemory : public ploc::SpTcBase { * @param startAddress Start address of CRC calculation * @param length Length in bytes of memory region */ - CheckMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - // spParams.creator.setApid(APID_CHECK_MEMORY); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + CheckMemory(TcParams params) + : TcBase(params, Apids::MEM_MAN, static_cast(MemManServiceIds::CHECK), 10) {} ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { auto res = checkSizeAndSerializeHeader(); @@ -1109,8 +1068,6 @@ class CheckMemory : public ploc::SpTcBase { } private: - static const uint16_t PAYLOAD_LENGTH = 10; // length without CRC field - uint8_t memoryId = 0; uint8_t n = 1; uint32_t startAddress = 0; @@ -1132,7 +1089,7 @@ class CheckMemory : public ploc::SpTcBase { /** * @brief This class packages the space packet transporting a part of an MPSoC update. */ -class WriteMemory : public ploc::SpTcBase { +class WriteMemory : public TcBase { public: /** * @brief Constructor @@ -1141,10 +1098,8 @@ class WriteMemory : public ploc::SpTcBase { * @param sequenceCount Sequence count (first update packet expects 1 as sequence count) * @param updateData Pointer to buffer containing update data */ - WriteMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { - // spParams.creator.setApid(APID_WRITE_MEMORY); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + WriteMemory(TcParams params) + : TcBase(params, Apids::MEM_MAN, static_cast(MemManServiceIds::WRITE), 1) {} ReturnValue_t buildPacket(ccsds::SequenceFlags seqFlags, uint16_t sequenceCount, uint8_t memoryId, uint32_t startAddress, uint16_t length, uint8_t* updateData) { @@ -1173,9 +1128,9 @@ class WriteMemory : public ploc::SpTcBase { uint8_t* updateData) { uint8_t* data = payloadStart; if (updateDataLen % 2 != 0) { - spParams.setFullPayloadLen(META_DATA_LENGTH + updateDataLen + 1 + 2); + setLenFromPayloadLen(META_DATA_LENGTH + updateDataLen + 1); } else { - spParams.setFullPayloadLen(META_DATA_LENGTH + updateDataLen + 2); + setLenFromPayloadLen(META_DATA_LENGTH + updateDataLen); } // To avoid crashes in this unexpected case ReturnValue_t result = checkPayloadLen(); @@ -1204,13 +1159,11 @@ class WriteMemory : public ploc::SpTcBase { /** * @brief This class can be used to package erase memory command */ -class EraseMemory : public ploc::SpTcBase { +class EraseMemory : public TcBase { public: - EraseMemory(ploc::SpTcParams params) : ploc::SpTcBase(params) { - spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - // spParams.creator.setApid(APID_ERASE_MEMORY); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } + EraseMemory(TcParams params) + : TcBase(params, Apids::MEM_MAN, static_cast(MemManServiceIds::ERASE), + PAYLOAD_LENGTH) {} ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { auto res = checkSizeAndSerializeHeader(); @@ -1246,9 +1199,9 @@ class EraseMemory : public ploc::SpTcBase { /** * @brief This class creates the space packet to enable the auto TM generation */ -class EnableAutoTm : public ploc::SpTcBase { +class EnableAutoTm : public TcBase { public: - EnableAutoTm(ploc::SpTcParams params) : ploc::SpTcBase(params) { + EnableAutoTm(TcParams params) : TcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); // spParams.creator.setApid(APID_AUTO_TM); // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); @@ -1271,9 +1224,9 @@ class EnableAutoTm : public ploc::SpTcBase { /** * @brief This class creates the space packet to disable the auto TM generation */ -class DisableAutoTm : public ploc::SpTcBase { +class DisableAutoTm : public TcBase { public: - DisableAutoTm(ploc::SpTcParams params) : ploc::SpTcBase(params) { + DisableAutoTm(TcParams params) : TcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); // spParams.creator.setApid(APID_AUTO_TM); // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); @@ -1296,7 +1249,7 @@ class DisableAutoTm : public ploc::SpTcBase { /** * @brief This class creates the space packet to request the logging data from the supervisor */ -class RequestLoggingData : public ploc::SpTcBase { +class RequestLoggingData : public TcBase { public: /** * Subapid @@ -1308,7 +1261,7 @@ class RequestLoggingData : public ploc::SpTcBase { SET_LOGGING_TOPIC = 4 /**< SET_LOGGING_TOPIC */ }; - RequestLoggingData(ploc::SpTcParams params) : ploc::SpTcBase(params) { + RequestLoggingData(TcParams params) : TcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); // spParams.creator.setApid(APID_REQUEST_LOGGING_DATA); // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index a5a3b5ef..36898215 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -318,14 +318,14 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d result = prepareRunAutoEmTest(commandData); break; } - case WIPE_MRAM: { - result = prepareWipeMramCmd(commandData); - break; - } - case FIRST_MRAM_DUMP: - case CONSECUTIVE_MRAM_DUMP: - result = prepareDumpMramCmd(commandData); - break; + // case WIPE_MRAM: { + // result = prepareWipeMramCmd(commandData); + // break; + // } + // case FIRST_MRAM_DUMP: + // case CONSECUTIVE_MRAM_DUMP: + // result = prepareDumpMramCmd(commandData); + // break; case SET_GPIO: { prepareSetGpioCmd(commandData); result = returnvalue::OK; @@ -341,33 +341,33 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // result = returnvalue::OK; // break; // } - case FACTORY_RESET_CLEAR_ALL: { - FactoryReset packet(spParams); - result = packet.buildPacket(FactoryReset::Op::CLEAR_ALL); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } - case FACTORY_RESET_CLEAR_MIRROR: { - FactoryReset packet(spParams); - result = packet.buildPacket(FactoryReset::Op::MIRROR_ENTRIES); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } - case FACTORY_RESET_CLEAR_CIRCULAR: { - FactoryReset packet(spParams); - result = packet.buildPacket(FactoryReset::Op::CIRCULAR_ENTRIES); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } + /* case FACTORY_RESET_CLEAR_ALL: { + FactoryReset packet(spParams); + result = packet.buildPacket(FactoryReset::Op::CLEAR_ALL); + if (result != returnvalue::OK) { + break; + } + finishTcPrep(packet.getFullPacketLen()); + break; + } + case FACTORY_RESET_CLEAR_MIRROR: { + FactoryReset packet(spParams); + result = packet.buildPacket(FactoryReset::Op::MIRROR_ENTRIES); + if (result != returnvalue::OK) { + break; + } + finishTcPrep(packet.getFullPacketLen()); + break; + } + case FACTORY_RESET_CLEAR_CIRCULAR: { + FactoryReset packet(spParams); + result = packet.buildPacket(FactoryReset::Op::CIRCULAR_ENTRIES); + if (result != returnvalue::OK) { + break; + } + finishTcPrep(packet.getFullPacketLen()); + break; + }*/ // Removed command // case START_MPSOC_QUIET: { // prepareEmptyCmd(APID_START_MPSOC_QUIET); @@ -1594,47 +1594,47 @@ ReturnValue_t PlocSupervisorHandler::prepareRunAutoEmTest(const uint8_t* command return returnvalue::OK; } -ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandData) { - uint32_t start = 0; - uint32_t stop = 0; - size_t size = sizeof(start) + sizeof(stop); - SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); - SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); - if ((stop - start) <= 0) { - return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; - } - supv::MramCmd packet(spParams); - ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(packet.getFullPacketLen()); - return returnvalue::OK; -} +// ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandData) { +// uint32_t start = 0; +// uint32_t stop = 0; +// size_t size = sizeof(start) + sizeof(stop); +// SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); +// SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); +// if ((stop - start) <= 0) { +// return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; +// } +// supv::MramCmd packet(spParams); +// ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE); +// if (result != returnvalue::OK) { +// return result; +// } +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } -ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandData) { - uint32_t start = 0; - uint32_t stop = 0; - size_t size = sizeof(start) + sizeof(stop); - SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); - SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); - if ((stop - start) <= 0) { - return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; - } - supv::MramCmd packet(spParams); - ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::DUMP); - if (result != returnvalue::OK) { - return result; - } - expectedMramDumpPackets = (stop - start) / supv::MAX_DATA_CAPACITY; - if ((stop - start) % supv::MAX_DATA_CAPACITY) { - expectedMramDumpPackets++; - } - receivedMramDumpPackets = 0; - - finishTcPrep(packet.getFullPacketLen()); - return returnvalue::OK; -} +// ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandData) { +// uint32_t start = 0; +// uint32_t stop = 0; +// size_t size = sizeof(start) + sizeof(stop); +// SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); +// SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); +// if ((stop - start) <= 0) { +// return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; +// } +// supv::MramCmd packet(spParams); +// ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::DUMP); +// if (result != returnvalue::OK) { +// return result; +// } +// expectedMramDumpPackets = (stop - start) / supv::MAX_DATA_CAPACITY; +// if ((stop - start) % supv::MAX_DATA_CAPACITY) { +// expectedMramDumpPackets++; +// } +// receivedMramDumpPackets = 0; +// +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } ReturnValue_t PlocSupervisorHandler::prepareSetGpioCmd(const uint8_t* commandData) { uint8_t port = *commandData; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 98cd22e9..08c0e846 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -278,8 +278,8 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData); ReturnValue_t prepareSetAdcThresholdCmd(const uint8_t* commandData); ReturnValue_t prepareRunAutoEmTest(const uint8_t* commandData); - ReturnValue_t prepareWipeMramCmd(const uint8_t* commandData); - ReturnValue_t prepareDumpMramCmd(const uint8_t* commandData); + // ReturnValue_t prepareWipeMramCmd(const uint8_t* commandData); + // ReturnValue_t prepareDumpMramCmd(const uint8_t* commandData); ReturnValue_t prepareSetGpioCmd(const uint8_t* commandData); ReturnValue_t prepareReadGpioCmd(const uint8_t* commandData); ReturnValue_t prepareLoggingRequest(const uint8_t* commandData, size_t commandDataLen); From 3b575acd5518da9ca56a96c512409e5b823cf894 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 10:10:57 +0100 Subject: [PATCH 103/244] comment out more code which was removed --- .../PlocSupervisorDefinitions.h | 530 +++++++++--------- linux/devices/ploc/PlocSupervisorHandler.cpp | 253 ++++----- linux/devices/ploc/PlocSupervisorHandler.h | 8 +- linux/devices/ploc/PlocSupvUartMan.cpp | 97 ++-- linux/devices/ploc/PlocSupvUartMan.h | 2 +- 5 files changed, 437 insertions(+), 453 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 128d988c..cd128ca0 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -445,45 +445,6 @@ class MPSoCBootSelect : public TcBase { } }; -/** - * @brief This class creates the command to enable or disable the NVMs connected to the - * supervisor. - */ -// class EnableNvms : public ploc::SpTcBase { -// public: -// /** -// * @brief Constructor -// * -// * @param mem The memory to boot from: NVM0 (0), NVM1 (1) -// * @param bp0 Partition pin 0 -// * @param bp1 Partition pin 1 -// * @param bp2 Partition pin 2 -// */ -// EnableNvms(ploc::SpTcParams params) : ploc::SpTcBase(params) { -// spParams.setFullPayloadLen(DATA_FIELD_LENGTH); -// spParams.creator.setApid(APID_ENABLE_NVMS); -// spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); -// } -// -// ReturnValue_t buildPacket(uint8_t nvm01, uint8_t nvm3) { -// auto res = checkSizeAndSerializeHeader(); -// if (res != returnvalue::OK) { -// return res; -// } -// initPacket(nvm01, nvm3); -// return calcAndSetCrc(); -// } -// -// private: -// static const uint8_t DATA_FIELD_LENGTH = 4; -// static const uint8_t CRC_OFFSET = 2; -// -// void initPacket(uint8_t nvm01, uint8_t nvm3) { -// payloadStart[0] = nvm01; -// payloadStart[1] = nvm3; -// } -// }; - /** * @brief This class generates the space packet to update the time of the PLOC supervisor. */ @@ -715,37 +676,6 @@ class SetAlertlimit : public TcBase { } }; -/** - * @brief This class packages the space packet to enable or disable ADC channels. - */ -class SetAdcEnabledChannels : public TcBase { - public: - /** - * @brief Constructor - * - * @param ch Defines channels to be enabled or disabled. - */ - SetAdcEnabledChannels(TcParams params) - : TcBase(params, Apids::ADC_MON, static_cast(AdcMonServiceIds::SET_ENABLED_CHANNELS), - 2) {} - - ReturnValue_t buildPacket(uint16_t ch) { - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - initPacket(ch); - return calcAndSetCrc(); - } - - private: - void initPacket(uint16_t ch) { - size_t serializedSize = 0; - SerializeAdapter::serialize(&ch, &payloadStart, &serializedSize, sizeof(ch), - SerializeIF::Endianness::BIG); - } -}; - /** * @brief This class packages the space packet to configures the window size and striding step of * the moving average filter applied to the ADC readings. @@ -842,60 +772,6 @@ class RunAutoEmTests : public TcBase { void initPacket(uint8_t test) { payloadStart[0] = test; } }; -/** - * @brief This class packages the space packet to wipe or dump parts of the MRAM. - */ -// class MramCmd : public TcBase { -// public: -// enum class MramAction { WIPE, DUMP }; -// -// /** -// * @brief Constructor -// * -// * @param start Start address of the MRAM section to wipe or dump -// * @param stop End address of the MRAM section to wipe or dump -// * @param action Dump or wipe MRAM -// * -// * @note The content at the stop address is excluded from the dump or wipe operation. -// */ -// MramCmd(TcParams params) -// : TcBase(params, Apids::DATA_LOGGER) { -// setLenFromPayloadLen(6); -// } -// -// ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { -// if (action == MramAction::WIPE) { -// setServiceId(static_cast(DataLoggerServiceIds::WIPE_MRAM)); -// } else if (action == MramAction::DUMP) { -// setServiceId(static_cast(DataLoggerServiceIds::DUMP_MRAM)); -// } else { -// sif::debug << "WipeMram: Invalid action specified"; -// } -// auto res = checkSizeAndSerializeHeader(); -// if (res != returnvalue::OK) { -// return res; -// } -// initPacket(start, stop); -// return calcAndSetCrc(); -// } -// -// private: -// -// uint32_t start = 0; -// uint32_t stop = 0; -// -// void initPacket(uint32_t start, uint32_t stop) { -// uint8_t concatBuffer[6]; -// concatBuffer[0] = static_cast(start >> 16); -// concatBuffer[1] = static_cast(start >> 8); -// concatBuffer[2] = static_cast(start); -// concatBuffer[3] = static_cast(stop >> 16); -// concatBuffer[4] = static_cast(stop >> 8); -// concatBuffer[5] = static_cast(stop); -// std::memcpy(payloadStart, concatBuffer, sizeof(concatBuffer)); -// } -// }; - /** * @brief This class packages the space packet change the state of a GPIO. This command is only * required for ground testing. @@ -967,59 +843,6 @@ class ReadGpio : public TcBase { } }; -/** - * @brief This class packages the space packet to perform the factory reset. The op parameter is - * optional. - * - * @details: Without OP: All MRAM entries will be cleared. - * OP = 0x01: Only the mirror entries will be wiped. - * OP = 0x02: Only the circular entries will be wiped. - */ -// class FactoryReset : public TcBase { -// public: -// enum class Op { CLEAR_ALL, MIRROR_ENTRIES, CIRCULAR_ENTRIES }; -// -// /** -// * @brief Constructor -// * -// * @param op -// */ -// FactoryReset(TcParams params) -// : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::)) { -// // spParams.creator.setApid(APID_FACTORY_RESET); -// spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); -// } -// -// ReturnValue_t buildPacket(Op op) { -// auto res = checkSizeAndSerializeHeader(); -// if (res != returnvalue::OK) { -// return res; -// } -// initPacket(op); -// return calcAndSetCrc(); -// } -// -// private: -// static const uint16_t DEFAULT_SEQUENCE_COUNT = 1; -// -// void initPacket(Op op) { -// size_t packetDataLen = 2; -// switch (op) { -// case Op::MIRROR_ENTRIES: -// payloadStart[0] = 1; -// packetDataLen = 3; -// break; -// case Op::CIRCULAR_ENTRIES: -// payloadStart[0] = 2; -// packetDataLen = 3; -// break; -// default: -// break; -// } -// spParams.setFullPayloadLen(packetDataLen); -// } -// }; - class SetShutdownTimeout : public TcBase { public: SetShutdownTimeout(TcParams params) @@ -1068,10 +891,7 @@ class CheckMemory : public TcBase { } private: - uint8_t memoryId = 0; uint8_t n = 1; - uint32_t startAddress = 0; - uint32_t length = 0; void initPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { uint8_t* data = payloadStart; @@ -1177,10 +997,7 @@ class EraseMemory : public TcBase { private: static const uint16_t PAYLOAD_LENGTH = 10; // length without CRC field - uint8_t memoryId = 0; uint8_t n = 1; - uint32_t startAddress = 0; - uint32_t length = 0; void initPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { uint8_t* data = payloadStart; @@ -1196,97 +1013,6 @@ class EraseMemory : public TcBase { } }; -/** - * @brief This class creates the space packet to enable the auto TM generation - */ -class EnableAutoTm : public TcBase { - public: - EnableAutoTm(TcParams params) : TcBase(params) { - spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - // spParams.creator.setApid(APID_AUTO_TM); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } - - ReturnValue_t buildPacket() { - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - payloadStart[0] = ENABLE; - return calcAndSetCrc(); - } - - private: - static const uint16_t PAYLOAD_LENGTH = 1; // length without CRC field - static const uint8_t ENABLE = 1; -}; - -/** - * @brief This class creates the space packet to disable the auto TM generation - */ -class DisableAutoTm : public TcBase { - public: - DisableAutoTm(TcParams params) : TcBase(params) { - spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - // spParams.creator.setApid(APID_AUTO_TM); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } - - ReturnValue_t buildPacket() { - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - payloadStart[0] = DISABLE; - return calcAndSetCrc(); - } - - private: - static const uint16_t PAYLOAD_LENGTH = 1; // length without CRC field - static const uint8_t DISABLE = 0; -}; - -/** - * @brief This class creates the space packet to request the logging data from the supervisor - */ -class RequestLoggingData : public TcBase { - public: - /** - * Subapid - */ - enum class Sa : uint8_t { - REQUEST_COUNTERS = 1, /**< REQUEST_COUNTERS */ - REQUEST_EVENT_BUFFERS = 2, /**< REQUEST_EVENT_BUFFERS */ - CLEAR_COUNTERS = 3, /**< CLEAR_COUNTERS */ - SET_LOGGING_TOPIC = 4 /**< SET_LOGGING_TOPIC */ - }; - - RequestLoggingData(TcParams params) : TcBase(params) { - spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); - // spParams.creator.setApid(APID_REQUEST_LOGGING_DATA); - // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); - } - - /** - * @param sa - * @param tpc Topic - * @return - */ - ReturnValue_t buildPacket(Sa sa, uint8_t tpc = 0) { - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - payloadStart[0] = static_cast(sa); - payloadStart[1] = tpc; - return calcAndSetCrc(); - } - - private: - static const uint16_t PAYLOAD_LENGTH = 2; // length without CRC field - static const uint8_t TPC_OFFSET = 1; -}; - class VerificationReport : public ploc::SpTmReader { public: VerificationReport(const uint8_t* buf, size_t maxSize) : ploc::SpTmReader(buf, maxSize) {} @@ -1961,6 +1687,262 @@ class AdcReport : public StaticLocalDataSet { sif::info << "AdcReport: ADC eng 15: " << this->adcEng15 << std::endl; } }; + +namespace notimpl { + +/** + * @brief This class packages the space packet to perform the factory reset. The op parameter is + * optional. + * + * @details: Without OP: All MRAM entries will be cleared. + * OP = 0x01: Only the mirror entries will be wiped. + * OP = 0x02: Only the circular entries will be wiped. + */ +class FactoryReset : public TcBase { + public: + enum class Op { CLEAR_ALL, MIRROR_ENTRIES, CIRCULAR_ENTRIES }; + + /** + * @brief Constructor + * + * @param op + */ + FactoryReset(TcParams params) : TcBase(params, Apids::TMTC_MAN, 0x11, 1) {} + + ReturnValue_t buildPacket(Op op) { + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + initPacket(op); + return calcAndSetCrc(); + } + + private: + void initPacket(Op op) { + size_t packetDataLen = 2; + switch (op) { + case Op::MIRROR_ENTRIES: + payloadStart[0] = 1; + packetDataLen = 3; + break; + case Op::CIRCULAR_ENTRIES: + payloadStart[0] = 2; + packetDataLen = 3; + break; + default: + break; + } + spParams.setFullPayloadLen(packetDataLen); + } +}; + +/** + * @brief This class creates the command to enable or disable the NVMs connected to the + * supervisor. + */ +class EnableNvms : public TcBase { + public: + /** + * @brief Constructor + * + * @param mem The memory to boot from: NVM0 (0), NVM1 (1) + * @param bp0 Partition pin 0 + * @param bp1 Partition pin 1 + * @param bp2 Partition pin 2 + */ + EnableNvms(TcParams params) : TcBase(params, Apids::TMTC_MAN, 0x06, 2) {} + + ReturnValue_t buildPacket(uint8_t nvm01, uint8_t nvm3) { + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + initPacket(nvm01, nvm3); + return calcAndSetCrc(); + } + + private: + void initPacket(uint8_t nvm01, uint8_t nvm3) { + payloadStart[0] = nvm01; + payloadStart[1] = nvm3; + } +}; + +/** + * @brief This class packages the space packet to wipe or dump parts of the MRAM. + */ +class MramCmd : public TcBase { + public: + enum class MramAction { WIPE, DUMP }; + + /** + * @brief Constructor + * + * @param start Start address of the MRAM section to wipe or dump + * @param stop End address of the MRAM section to wipe or dump + * @param action Dump or wipe MRAM + * + * @note The content at the stop address is excluded from the dump or wipe operation. + */ + MramCmd(TcParams params) : TcBase(params, Apids::DATA_LOGGER) { setLenFromPayloadLen(6); } + + ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { + if (action == MramAction::WIPE) { + setServiceId(static_cast(DataLoggerServiceIds::WIPE_MRAM)); + } else if (action == MramAction::DUMP) { + setServiceId(static_cast(DataLoggerServiceIds::DUMP_MRAM)); + } else { + sif::debug << "WipeMram: Invalid action specified"; + } + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + initPacket(start, stop); + return calcAndSetCrc(); + } + + private: + uint32_t start = 0; + uint32_t stop = 0; + + void initPacket(uint32_t start, uint32_t stop) { + uint8_t concatBuffer[6]; + concatBuffer[0] = static_cast(start >> 16); + concatBuffer[1] = static_cast(start >> 8); + concatBuffer[2] = static_cast(start); + concatBuffer[3] = static_cast(stop >> 16); + concatBuffer[4] = static_cast(stop >> 8); + concatBuffer[5] = static_cast(stop); + std::memcpy(payloadStart, concatBuffer, sizeof(concatBuffer)); + } +}; + +/** + * @brief This class creates the space packet to enable the auto TM generation + */ +class EnableAutoTm : public TcBase { + public: + EnableAutoTm(TcParams params) : TcBase(params) { + spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); + // spParams.creator.setApid(APID_AUTO_TM); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + } + + ReturnValue_t buildPacket() { + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + payloadStart[0] = ENABLE; + return calcAndSetCrc(); + } + + private: + static const uint16_t PAYLOAD_LENGTH = 1; // length without CRC field + static const uint8_t ENABLE = 1; +}; + +/** + * @brief This class creates the space packet to disable the auto TM generation + */ +class DisableAutoTm : public TcBase { + public: + DisableAutoTm(TcParams params) : TcBase(params) { + spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); + // spParams.creator.setApid(APID_AUTO_TM); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + } + + ReturnValue_t buildPacket() { + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + payloadStart[0] = DISABLE; + return calcAndSetCrc(); + } + + private: + static const uint16_t PAYLOAD_LENGTH = 1; // length without CRC field + static const uint8_t DISABLE = 0; +}; + +/** + * @brief This class packages the space packet to enable or disable ADC channels. + */ +class SetAdcEnabledChannels : public TcBase { + public: + /** + * @brief Constructor + * + * @param ch Defines channels to be enabled or disabled. + */ + SetAdcEnabledChannels(TcParams params) + : TcBase(params, Apids::ADC_MON, static_cast(AdcMonServiceIds::SET_ENABLED_CHANNELS), + 2) {} + + ReturnValue_t buildPacket(uint16_t ch) { + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + initPacket(ch); + return calcAndSetCrc(); + } + + private: + void initPacket(uint16_t ch) { + size_t serializedSize = 0; + SerializeAdapter::serialize(&ch, &payloadStart, &serializedSize, sizeof(ch), + SerializeIF::Endianness::BIG); + } +}; + +/** + * @brief This class creates the space packet to request the logging data from the supervisor + */ +class RequestLoggingData : public TcBase { + public: + /** + * Subapid + */ + enum class Sa : uint8_t { + REQUEST_COUNTERS = 1, /**< REQUEST_COUNTERS */ + REQUEST_EVENT_BUFFERS = 2, /**< REQUEST_EVENT_BUFFERS */ + CLEAR_COUNTERS = 3, /**< CLEAR_COUNTERS */ + SET_LOGGING_TOPIC = 4 /**< SET_LOGGING_TOPIC */ + }; + + RequestLoggingData(TcParams params) : TcBase(params) { + spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); + // spParams.creator.setApid(APID_REQUEST_LOGGING_DATA); + // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT); + } + + /** + * @param sa + * @param tpc Topic + * @return + */ + ReturnValue_t buildPacket(Sa sa, uint8_t tpc = 0) { + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + payloadStart[0] = static_cast(sa); + payloadStart[1] = tpc; + return calcAndSetCrc(); + } + + private: + static const uint16_t PAYLOAD_LENGTH = 2; // length without CRC field + static const uint8_t TPC_OFFSET = 1; +}; + +} // namespace notimpl + } // namespace supv #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_PLOCSVPDEFINITIONS_H_ */ diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 36898215..cf63bea3 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -282,21 +282,21 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d result = prepareSetAlertLimitCmd(commandData); break; } - case SET_ADC_ENABLED_CHANNELS: { - prepareSetAdcEnabledChannelsCmd(commandData); - result = returnvalue::OK; - break; - } - case SET_ADC_WINDOW_AND_STRIDE: { - prepareSetAdcWindowAndStrideCmd(commandData); - result = returnvalue::OK; - break; - } - case SET_ADC_THRESHOLD: { - prepareSetAdcThresholdCmd(commandData); - result = returnvalue::OK; - break; - } + // case SET_ADC_ENABLED_CHANNELS: { + // prepareSetAdcEnabledChannelsCmd(commandData); + // result = returnvalue::OK; + // break; + // } + // case SET_ADC_WINDOW_AND_STRIDE: { + // prepareSetAdcWindowAndStrideCmd(commandData); + // result = returnvalue::OK; + // break; + // } + // case SET_ADC_THRESHOLD: { + // prepareSetAdcThresholdCmd(commandData); + // result = returnvalue::OK; + // break; + // } case GET_LATCHUP_STATUS_REPORT: { prepareEmptyCmd(Apids::LATCHUP_MON, static_cast(LatchupMonServiceIds::GET_STATUS_REPORT)); @@ -384,55 +384,55 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d result = returnvalue::OK; break; } - case ENABLE_AUTO_TM: { - EnableAutoTm packet(spParams); - result = packet.buildPacket(); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } - case DISABLE_AUTO_TM: { - DisableAutoTm packet(spParams); - result = packet.buildPacket(); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } - case LOGGING_REQUEST_COUNTERS: { - RequestLoggingData packet(spParams); - result = packet.buildPacket(RequestLoggingData::Sa::REQUEST_COUNTERS); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } - case LOGGING_CLEAR_COUNTERS: { - RequestLoggingData packet(spParams); - result = packet.buildPacket(RequestLoggingData::Sa::CLEAR_COUNTERS); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } - case LOGGING_SET_TOPIC: { - if (commandData == nullptr or commandDataLen == 0) { - return HasActionsIF::INVALID_PARAMETERS; - } - uint8_t tpc = *(commandData); - RequestLoggingData packet(spParams); - result = packet.buildPacket(RequestLoggingData::Sa::SET_LOGGING_TOPIC, tpc); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } + // case ENABLE_AUTO_TM: { + // EnableAutoTm packet(spParams); + // result = packet.buildPacket(); + // if (result != returnvalue::OK) { + // break; + // } + // finishTcPrep(packet.getFullPacketLen()); + // break; + // } + // case DISABLE_AUTO_TM: { + // DisableAutoTm packet(spParams); + // result = packet.buildPacket(); + // if (result != returnvalue::OK) { + // break; + // } + // finishTcPrep(packet.getFullPacketLen()); + // break; + // } + // case LOGGING_REQUEST_COUNTERS: { + // RequestLoggingData packet(spParams); + // result = packet.buildPacket(RequestLoggingData::Sa::REQUEST_COUNTERS); + // if (result != returnvalue::OK) { + // break; + // } + // finishTcPrep(packet.getFullPacketLen()); + // break; + // } + // case LOGGING_CLEAR_COUNTERS: { + // RequestLoggingData packet(spParams); + // result = packet.buildPacket(RequestLoggingData::Sa::CLEAR_COUNTERS); + // if (result != returnvalue::OK) { + // break; + // } + // finishTcPrep(packet.getFullPacketLen()); + // break; + // } + // case LOGGING_SET_TOPIC: { + // if (commandData == nullptr or commandDataLen == 0) { + // return HasActionsIF::INVALID_PARAMETERS; + // } + // uint8_t tpc = *(commandData); + // RequestLoggingData packet(spParams); + // result = packet.buildPacket(RequestLoggingData::Sa::SET_LOGGING_TOPIC, tpc); + // if (result != returnvalue::OK) { + // break; + // } + // finishTcPrep(packet.getFullPacketLen()); + // break; + // } case RESET_PL: { prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::RESET_PL)); result = returnvalue::OK; @@ -744,10 +744,10 @@ ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id, result = handleLatchupStatusReport(packet); break; } - case (LOGGING_REPORT): { - result = handleLoggingReport(packet); - break; - } + // case (LOGGING_REPORT): { + // result = handleLoggingReport(packet); + // break; + // } case (ADC_REPORT): { result = handleAdcReport(packet); break; @@ -1248,41 +1248,41 @@ ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* da return result; } -ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) { - ReturnValue_t result = returnvalue::OK; - - result = verifyPacket(data, supv::SIZE_LOGGING_REPORT); - - if (result == SupvReturnValuesIF::CRC_FAILURE) { - sif::warning << "PlocSupervisorHandler::handleLoggingReport: Logging report has " - << "invalid crc" << std::endl; - return result; - } - - const uint8_t* dataField = data + supv::PAYLOAD_OFFSET + sizeof(supv::RequestLoggingData::Sa); - result = loggingReport.read(); - if (result != returnvalue::OK) { - return result; - } - loggingReport.setValidityBufferGeneration(false); - size_t size = loggingReport.getSerializedSize(); - result = loggingReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG); - if (result != returnvalue::OK) { - sif::warning << "PlocSupervisorHandler::handleLoggingReport: Deserialization failed" - << std::endl; - } - loggingReport.setValidityBufferGeneration(true); - loggingReport.setValidity(true, true); - result = loggingReport.commit(); - if (result != returnvalue::OK) { - return result; - } -#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1 - loggingReport.printSet(); -#endif - nextReplyId = supv::EXE_REPORT; - return result; -} +// ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) { +// ReturnValue_t result = returnvalue::OK; +// +// result = verifyPacket(data, supv::SIZE_LOGGING_REPORT); +// +// if (result == SupvReturnValuesIF::CRC_FAILURE) { +// sif::warning << "PlocSupervisorHandler::handleLoggingReport: Logging report has " +// << "invalid crc" << std::endl; +// return result; +// } +// +// const uint8_t* dataField = data + supv::PAYLOAD_OFFSET + sizeof(supv::RequestLoggingData::Sa); +// result = loggingReport.read(); +// if (result != returnvalue::OK) { +// return result; +// } +// loggingReport.setValidityBufferGeneration(false); +// size_t size = loggingReport.getSerializedSize(); +// result = loggingReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG); +// if (result != returnvalue::OK) { +// sif::warning << "PlocSupervisorHandler::handleLoggingReport: Deserialization failed" +// << std::endl; +// } +// loggingReport.setValidityBufferGeneration(true); +// loggingReport.setValidity(true, true); +// result = loggingReport.commit(); +// if (result != returnvalue::OK) { +// return result; +// } +//#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1 +// loggingReport.printSet(); +//#endif +// nextReplyId = supv::EXE_REPORT; +// return result; +// } ReturnValue_t PlocSupervisorHandler::handleAdcReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; @@ -1543,16 +1543,17 @@ ReturnValue_t PlocSupervisorHandler::prepareSetAlertLimitCmd(const uint8_t* comm return returnvalue::OK; } -ReturnValue_t PlocSupervisorHandler::prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData) { - uint16_t ch = *(commandData) << 8 | *(commandData + 1); - supv::SetAdcEnabledChannels packet(spParams); - ReturnValue_t result = packet.buildPacket(ch); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(packet.getFullPacketLen()); - return returnvalue::OK; -} +// ReturnValue_t PlocSupervisorHandler::prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData) +// { +// uint16_t ch = *(commandData) << 8 | *(commandData + 1); +// supv::SetAdcEnabledChannels packet(spParams); +// ReturnValue_t result = packet.buildPacket(ch); +// if (result != returnvalue::OK) { +// return result; +// } +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } ReturnValue_t PlocSupervisorHandler::prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData) { uint8_t offset = 0; @@ -1687,19 +1688,19 @@ ReturnValue_t PlocSupervisorHandler::prepareSetShutdownTimeoutCmd(const uint8_t* return returnvalue::OK; } -ReturnValue_t PlocSupervisorHandler::prepareLoggingRequest(const uint8_t* commandData, - size_t commandDataLen) { - using namespace supv; - RequestLoggingData::Sa sa = static_cast(*commandData); - uint8_t tpc = *(commandData + 1); - RequestLoggingData packet(spParams); - ReturnValue_t result = packet.buildPacket(sa, tpc); - if (result != returnvalue::OK) { - return result; - } - finishTcPrep(packet.getFullPacketLen()); - return returnvalue::OK; -} +// ReturnValue_t PlocSupervisorHandler::prepareLoggingRequest(const uint8_t* commandData, +// size_t commandDataLen) { +// using namespace supv; +// RequestLoggingData::Sa sa = static_cast(*commandData); +// uint8_t tpc = *(commandData + 1); +// RequestLoggingData packet(spParams); +// ReturnValue_t result = packet.buildPacket(sa, tpc); +// if (result != returnvalue::OK) { +// return result; +// } +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } // ReturnValue_t PlocSupervisorHandler::prepareEnableNvmsCommand(const uint8_t* commandData) { // using namespace supv; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 08c0e846..21ae743d 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -211,7 +211,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t handleBootStatusReport(const uint8_t* data); ReturnValue_t handleLatchupStatusReport(const uint8_t* data); - ReturnValue_t handleLoggingReport(const uint8_t* data); + // ReturnValue_t handleLoggingReport(const uint8_t* data); ReturnValue_t handleAdcReport(const uint8_t* data); /** @@ -274,7 +274,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t prepareLatchupConfigCmd(const uint8_t* commandData, DeviceCommandId_t deviceCommand); ReturnValue_t prepareSetAlertLimitCmd(const uint8_t* commandData); - ReturnValue_t prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData); + // ReturnValue_t prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData); ReturnValue_t prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData); ReturnValue_t prepareSetAdcThresholdCmd(const uint8_t* commandData); ReturnValue_t prepareRunAutoEmTest(const uint8_t* commandData); @@ -282,8 +282,8 @@ class PlocSupervisorHandler : public DeviceHandlerBase { // ReturnValue_t prepareDumpMramCmd(const uint8_t* commandData); ReturnValue_t prepareSetGpioCmd(const uint8_t* commandData); ReturnValue_t prepareReadGpioCmd(const uint8_t* commandData); - ReturnValue_t prepareLoggingRequest(const uint8_t* commandData, size_t commandDataLen); - // ReturnValue_t prepareEnableNvmsCommand(const uint8_t* commandData); + // ReturnValue_t prepareLoggingRequest(const uint8_t* commandData, size_t commandDataLen); + // ReturnValue_t prepareEnableNvmsCommand(const uint8_t* commandData); /** * @brief Copies the content of a space packet to the command buffer. diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 22484f31..3632a21f 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -428,45 +428,46 @@ uint32_t PlocSupvHelper::buildProgParams1(uint8_t percent, uint16_t seqCount) { return (static_cast(percent) << 24) | static_cast(seqCount); } -ReturnValue_t PlocSupvHelper::performEventBufferRequest() { - using namespace supv; - ReturnValue_t result = returnvalue::OK; - resetSpParams(); - RequestLoggingData packet(spParams); - result = packet.buildPacket(RequestLoggingData::Sa::REQUEST_EVENT_BUFFERS); - if (result != returnvalue::OK) { - return result; - } - result = sendCommand(packet); - if (result != returnvalue::OK) { - return result; - } - result = handleAck(); - if (result != returnvalue::OK) { - return result; - } - result = - handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), supv::recv_timeout::UPDATE_STATUS_REPORT); - if (result != returnvalue::OK) { - return result; - } - ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size()); - bool exeAlreadyReceived = false; - if (spReader.getApid() == supv::APID_EXE_FAILURE) { - exeAlreadyReceived = true; - result = handleRemainingExeReport(spReader); - } else if (spReader.getApid() == supv::APID_MRAM_DUMP_TM) { - result = handleEventBufferReception(spReader); - } - - if (not exeAlreadyReceived) { - result = handleExe(); - if (result != returnvalue::OK) { - return result; - } - } - return result; -} +// ReturnValue_t PlocSupvHelper::performEventBufferRequest() { +// using namespace supv; +// ReturnValue_t result = returnvalue::OK; +// resetSpParams(); +// RequestLoggingData packet(spParams); +// result = packet.buildPacket(RequestLoggingData::Sa::REQUEST_EVENT_BUFFERS); +// if (result != returnvalue::OK) { +// return result; +// } +// result = sendCommand(packet); +// if (result != returnvalue::OK) { +// return result; +// } +// result = handleAck(); +// if (result != returnvalue::OK) { +// return result; +// } +// result = +// handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), +// supv::recv_timeout::UPDATE_STATUS_REPORT); +// if (result != returnvalue::OK) { +// return result; +// } +// ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size()); +// bool exeAlreadyReceived = false; +// if (spReader.getApid() == supv::APID_EXE_FAILURE) { +// exeAlreadyReceived = true; +// result = handleRemainingExeReport(spReader); +// } else if (spReader.getApid() == supv::APID_MRAM_DUMP_TM) { +// result = handleEventBufferReception(spReader); +// } +// +// if (not exeAlreadyReceived) { +// result = handleExe(); +// if (result != returnvalue::OK) { +// return result; +// } +// } +// return result; +// } ReturnValue_t PlocSupvHelper::handleRemainingExeReport(ploc::SpTmReader& reader) { size_t remBytes = reader.getPacketDataLen() + 1; @@ -916,15 +917,15 @@ ReturnValue_t PlocSupvHelper::handleRunningLongerRequest() { break; } case Request::REQUEST_EVENT_BUFFER: { - result = performEventBufferRequest(); - if (result == returnvalue::OK) { - triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result); - } else if (result == PROCESS_TERMINATED) { - // Event already triggered - break; - } else { - triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result); - } + // result = performEventBufferRequest(); + // if (result == returnvalue::OK) { + // triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result); + // } else if (result == PROCESS_TERMINATED) { + // // Event already triggered + // break; + // } else { + // triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result); + // } break; } case Request::DEFAULT: { diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index c0f23f22..d36234e2 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -268,7 +268,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, ReturnValue_t continueUpdate(); ReturnValue_t updateOperation(); ReturnValue_t writeUpdatePackets(); - ReturnValue_t performEventBufferRequest(); + // ReturnValue_t performEventBufferRequest(); ReturnValue_t handlePacketTransmission(ploc::SpTcBase& packet, uint32_t timeoutExecutionReport = 60000); ReturnValue_t sendCommand(ploc::SpTcBase& packet); From 4bf22f689a8205a3165c115b62005b8169bf6432 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 10:22:57 +0100 Subject: [PATCH 104/244] create TM base class --- .../PlocSupervisorDefinitions.h | 21 ++++++++++++++++++- linux/devices/ploc/PlocSupvUartMan.cpp | 5 +++-- mission/devices/devicedefinitions/SpBase.h | 5 ----- 3 files changed, 23 insertions(+), 8 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index cd128ca0..4d8dbb46 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -226,7 +226,7 @@ static const size_t MAX_DATA_CAPACITY = 1016; static const size_t MAX_PACKET_SIZE = 1024; static constexpr size_t MIN_PAYLOAD_LEN = SECONDARY_HEADER_LEN + CRC_LEN; -static constexpr size_t MIN_TC_LEN = ccsds::HEADER_LEN + MIN_PAYLOAD_LEN; +static constexpr size_t MIN_TMTC_LEN = ccsds::HEADER_LEN + MIN_PAYLOAD_LEN; static constexpr size_t PAYLOAD_OFFSET = ccsds::HEADER_LEN + SECONDARY_HEADER_LEN; struct UpdateParams { @@ -386,6 +386,25 @@ class TcBase : public ploc::SpTcBase { } }; +class TmBase: public ploc::SpTmReader { +public: + TmBase(const uint8_t* data, size_t maxSize): ploc::SpTmReader(data, maxSize) { + if(maxSize < MIN_TMTC_LEN) { + sif::error << "SupvTcBase::SupvTcBase: Passed buffer is too small" << std::endl; + } + } + + uint8_t getServiceId() { + return getPacketData()[TIMESTAMP_LEN]; + } + + const uint8_t* getPayloadStart() { + return getPacketData() + SECONDARY_HEADER_LEN; + } +private: + +}; + class NoPayloadPacket : public TcBase { public: NoPayloadPacket(TcParams params, uint16_t apid, uint8_t serviceId) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 3632a21f..604a6c5e 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -859,8 +859,9 @@ ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reade file.close(); return EVENT_BUFFER_REPLY_INVALID_APID; } - file.write(reinterpret_cast(reader.getPacketData()), - reader.getPayloadDataLength()); + // TODO: Fix +// file.write(reinterpret_cast(reader.getPacketData()), +// reader.getPayloadDataLength()); } return result; } diff --git a/mission/devices/devicedefinitions/SpBase.h b/mission/devices/devicedefinitions/SpBase.h index da2f4cd4..6cd4a803 100644 --- a/mission/devices/devicedefinitions/SpBase.h +++ b/mission/devices/devicedefinitions/SpBase.h @@ -102,11 +102,6 @@ class SpTmReader : public SpacePacketReader { return setReadOnlyData(buf, maxSize); } - /** - * @brief Returns the payload data length (data field length without CRC) - */ - uint16_t getPayloadDataLength() { return getPacketDataLen() - 2; } - ReturnValue_t checkCrc() { if (CRC::crc16ccitt(getFullData(), getFullPacketLen()) != 0) { return returnvalue::FAILED; From 8cc368b131cc0d300353f2ef3ab49b84d74eb6c4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 8 Nov 2022 10:56:12 +0100 Subject: [PATCH 105/244] fixed types for actuatorCmdData --- mission/controller/AcsController.cpp | 27 +++++++++++++-------------- mission/controller/AcsController.h | 4 ++-- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0c2b58a9..d00bb816 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -164,22 +164,21 @@ void AcsController::performSafe() { { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { - double zeroVec[4] = {0, 0, 0, 0}; - std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); + 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(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(double)); + 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(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); + // PoolReadGuard pg(&dipoleSet); + // MutexGuard mg(torquer::lazyLock()); + // torquer::NEW_ACTUATION_FLAG = true; + // dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration); // } } @@ -225,12 +224,12 @@ void AcsController::performDetumble() { { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { - double zeroVec[4] = {0, 0, 0, 0}; + int32_t zeroVec[4] = {0, 0, 0, 0}; std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double)); actuatorCmdData.rwTargetTorque.setValid(false); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(double)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } @@ -295,8 +294,8 @@ void AcsController::performPointingCtrl() { 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(double)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * 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); } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index cfcc200d..ed896d8e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -158,8 +158,8 @@ IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HAND // Actuator CMD acsctrl::ActuatorCmdData actuatorCmdData; PoolEntry rwTargetTorque = PoolEntry(4); - PoolEntry rwTargetSpeed = PoolEntry(4); - PoolEntry mtqTargetDipole = PoolEntry(3); + PoolEntry rwTargetSpeed = PoolEntry(4); + PoolEntry mtqTargetDipole = PoolEntry(3); // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); From 4515703efa01a884ec3d75e903c40aa8376216ec Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 11:08:21 +0100 Subject: [PATCH 106/244] added tm service IDs --- fsfw | 2 +- .../PlocSupervisorDefinitions.h | 260 +++++++++++------- linux/devices/ploc/PlocSupervisorHandler.cpp | 240 ++++++++-------- linux/devices/ploc/PlocSupvUartMan.cpp | 88 +++--- mission/devices/devicedefinitions/SpBase.h | 2 +- 5 files changed, 329 insertions(+), 263 deletions(-) diff --git a/fsfw b/fsfw index 672fca51..0e8f5ddd 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 672fca5169b017387e58e2ff864913d932c59aa1 +Subproject commit 0e8f5ddd26d586dd40e69f52aef1a63c0d5a9da6 diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 4d8dbb46..c564b6e4 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -17,6 +17,12 @@ using namespace returnvalue; namespace supv { +static const uint8_t CLASS_ID = CLASS_ID::PLOC_SUPERVISOR_HANDLER; + +static constexpr ReturnValue_t CRC_MISSMATCH = makeCode(CLASS_ID, 1); +static constexpr ReturnValue_t APID_MISSMATCH = makeCode(CLASS_ID, 2); +static constexpr ReturnValue_t BUF_TOO_SMALL = makeCode(CLASS_ID, 3); + typedef struct { // The most significant bit of msec value is set to 0x80 to indicate that full // time and data information is transmitted, when the time has been synced with @@ -135,14 +141,16 @@ enum Apids { WDOG_MAN = 0x07 }; -enum class HkServiceIds : uint8_t { +namespace tc { + +enum class HkId : uint8_t { ENABLE = 0x01, SET_PERIOD = 0x02, GET_REPORT = 0x03, GET_HARDFAULTS_REPORT = 0x04, }; -enum class TmtcServiceIds : uint8_t { +enum class TmtcId : uint8_t { TIME_REF = 0x03, GET_SUPV_VERSION = 0x05, RUN_AUTO_EM_TEST = 0x08, @@ -151,7 +159,7 @@ enum class TmtcServiceIds : uint8_t { GET_MPSOC_POWER_INFO = 0x10 }; -enum class BootManServiceIds : uint8_t { +enum class BootManId : uint8_t { START_MPSOC = 0x01, SHUTDOWN_MPSOC = 0x02, SELECT_IMAGE = 0x03, @@ -165,7 +173,7 @@ enum class BootManServiceIds : uint8_t { FACTORY_FLASH = 0x0C }; -enum class LatchupMonServiceIds : uint8_t { +enum class LatchupMonId : uint8_t { ENABLE = 0x01, DISABLE = 0x02, SET_ALERT_LIMIT = 0x04, @@ -174,7 +182,7 @@ enum class LatchupMonServiceIds : uint8_t { // Right now, none of the commands seem to be implemented, but still // keep the enum here in case some are added -enum class AdcMonServiceIds : uint8_t { +enum class AdcMonId : uint8_t { SET_SWEEP_PERIOD = 0x01, SET_ENABLED_CHANNELS = 0x02, SET_WINDOW_STRIDE = 0x03, @@ -182,9 +190,9 @@ enum class AdcMonServiceIds : uint8_t { COPY_ADC_DATA_TO_MRAM = 0x05 }; -enum class MemManServiceIds : uint8_t { ERASE = 0x01, WRITE = 0x02, CHECK = 0x03 }; +enum class MemManId : uint8_t { ERASE = 0x01, WRITE = 0x02, CHECK = 0x03 }; -enum class DataLoggerServiceIds : uint8_t { +enum class DataLoggerServiceId : uint8_t { WIPE_MRAM = 0x05, DUMP_MRAM = 0x06, FACTORY_RESET = 0x07 @@ -192,7 +200,23 @@ enum class DataLoggerServiceIds : uint8_t { // Right now, none of the commands seem to be implemented, but still // keep the enum here in case some are added -enum class WdogManServiceIds : uint8_t {}; +enum class WdogManServiceId : uint8_t {}; + +} // namespace tc + +namespace tm { + +enum class TmtcId : uint8_t { ACK = 0x01, NAK = 0x02, EXEC_ACK = 0x03, EXEC_NAK = 0x04 }; + +enum class HkId : uint8_t { REPORT = 0x01, HARDFAULTS = 0x02 }; + +enum class BootManId : uint8_t { BOOT_STATUS_REPORT = 0x01 }; + +enum class MemManId : uint8_t { UPDATE_STATUS_REPORT = 0x01 }; + +enum class LatchupMonId : uint8_t { LATCHUP_STATUS_REPORT = 0x01 }; + +} // namespace tm static const uint16_t APID_MASK = 0x3FF; static const uint16_t SEQUENCE_COUNT_MASK = 0xFFF; @@ -209,11 +233,11 @@ static const uint32_t LATCHUP_RPT_ID = LATCHUP_REPORT; static const uint32_t LOGGING_RPT_ID = LOGGING_REPORT; static const uint32_t ADC_REPORT_SET_ID = ADC_REPORT; -namespace recv_timeout { +namespace timeout { // Erase memory can require up to 60 seconds for execution static const uint32_t ERASE_MEMORY = 60000; static const uint32_t UPDATE_STATUS_REPORT = 70000; -} // namespace recv_timeout +} // namespace timeout static constexpr size_t TIMESTAMP_LEN = 7; static constexpr size_t SECONDARY_HEADER_LEN = TIMESTAMP_LEN + 1; @@ -386,23 +410,29 @@ class TcBase : public ploc::SpTcBase { } }; -class TmBase: public ploc::SpTmReader { -public: - TmBase(const uint8_t* data, size_t maxSize): ploc::SpTmReader(data, maxSize) { - if(maxSize < MIN_TMTC_LEN) { +class TmBase : public ploc::SpTmReader { + public: + TmBase(const uint8_t* data, size_t maxSize) : ploc::SpTmReader(data, maxSize) { + if (maxSize < MIN_TMTC_LEN) { sif::error << "SupvTcBase::SupvTcBase: Passed buffer is too small" << std::endl; } } - uint8_t getServiceId() { - return getPacketData()[TIMESTAMP_LEN]; + bool verifyCrc() { + if (checkCrc() == returnvalue::OK) { + crcOk = true; + } + return crcOk; } - const uint8_t* getPayloadStart() { - return getPacketData() + SECONDARY_HEADER_LEN; - } -private: + bool crcIsOk() const { return crcOk; } + uint8_t getServiceId() const { return getPacketData()[TIMESTAMP_LEN]; } + + const uint8_t* getPayloadStart() const { return getPacketData() + SECONDARY_HEADER_LEN; } + + private: + bool crcOk = false; }; class NoPayloadPacket : public TcBase { @@ -444,7 +474,7 @@ class MPSoCBootSelect : public TcBase { * @note Selection of partitions is currently not supported. */ MPSoCBootSelect(TcParams params) - : TcBase(params, Apids::BOOT_MAN, static_cast(BootManServiceIds::SELECT_IMAGE), 4) {} + : TcBase(params, Apids::BOOT_MAN, static_cast(tc::BootManId::SELECT_IMAGE), 4) {} ReturnValue_t buildPacket(uint8_t mem = 0, uint8_t bp0 = 0, uint8_t bp1 = 0, uint8_t bp2 = 0) { auto res = checkSizeAndSerializeHeader(); @@ -471,7 +501,7 @@ class SetTimeRef : public TcBase { public: static constexpr size_t PAYLOAD_LEN = 8; SetTimeRef(TcParams params) - : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::TIME_REF), 8) {} + : TcBase(params, Apids::TMTC_MAN, static_cast(tc::TmtcId::TIME_REF), 8) {} ReturnValue_t buildPacket(Clock::TimeOfDay_t* time) { auto res = checkSizeAndSerializeHeader(); @@ -546,7 +576,7 @@ class SetBootTimeout : public TcBase { * @param timeout The boot timeout in milliseconds. */ SetBootTimeout(TcParams params) - : TcBase(params, Apids::BOOT_MAN, static_cast(BootManServiceIds::SET_BOOT_TIMEOUT), + : TcBase(params, Apids::BOOT_MAN, static_cast(tc::BootManId::SET_BOOT_TIMEOUT), PAYLOAD_LEN) {} ReturnValue_t buildPacket(uint32_t timeout) { @@ -578,8 +608,8 @@ class SetRestartTries : public TcBase { * @param restartTries Maximum restart tries to set. */ SetRestartTries(TcParams params) - : TcBase(params, Apids::BOOT_MAN, - static_cast(BootManServiceIds::SET_MAX_REBOOT_TRIES), 1) {} + : TcBase(params, Apids::BOOT_MAN, static_cast(tc::BootManId::SET_MAX_REBOOT_TRIES), + 1) {} ReturnValue_t buildPacket(uint8_t restartTries) { auto res = checkSizeAndSerializeHeader(); @@ -607,7 +637,7 @@ class DisablePeriodicHkTransmission : public TcBase { * @brief Constructor */ DisablePeriodicHkTransmission(TcParams params) - : TcBase(params, Apids::HK, static_cast(HkServiceIds::ENABLE), 1) {} + : TcBase(params, Apids::HK, static_cast(tc::HkId::ENABLE), 1) {} ReturnValue_t buildPacket() { auto res = checkSizeAndSerializeHeader(); @@ -640,9 +670,9 @@ class LatchupAlert : public TcBase { ReturnValue_t buildPacket(bool state, uint8_t latchupId) { if (state) { - setServiceId(static_cast(LatchupMonServiceIds::ENABLE)); + setServiceId(static_cast(tc::LatchupMonId::ENABLE)); } else { - setServiceId(static_cast(LatchupMonServiceIds::DISABLE)); + setServiceId(static_cast(tc::LatchupMonId::DISABLE)); } auto res = checkSizeAndSerializeHeader(); if (res != returnvalue::OK) { @@ -668,8 +698,8 @@ class SetAlertlimit : public TcBase { * @param dutycycle */ SetAlertlimit(TcParams params) - : TcBase(params, Apids::LATCHUP_MON, - static_cast(LatchupMonServiceIds::SET_ALERT_LIMIT), 5) {} + : TcBase(params, Apids::LATCHUP_MON, static_cast(tc::LatchupMonId::SET_ALERT_LIMIT), + 5) {} ReturnValue_t buildPacket(uint8_t latchupId, uint32_t dutycycle) { auto res = checkSizeAndSerializeHeader(); @@ -708,8 +738,7 @@ class SetAdcWindowAndStride : public TcBase { * @param stridingStepSize */ SetAdcWindowAndStride(TcParams params) - : TcBase(params, Apids::ADC_MON, static_cast(AdcMonServiceIds::SET_WINDOW_STRIDE), - 4) {} + : TcBase(params, Apids::ADC_MON, static_cast(tc::AdcMonId::SET_WINDOW_STRIDE), 4) {} ReturnValue_t buildPacket(uint16_t windowSize, uint16_t stridingStepSize) { auto res = checkSizeAndSerializeHeader(); @@ -742,8 +771,7 @@ class SetAdcThreshold : public TcBase { * @param threshold */ SetAdcThreshold(TcParams params) - : TcBase(params, Apids::ADC_MON, static_cast(AdcMonServiceIds::SET_ADC_THRESHOLD), - 4) {} + : TcBase(params, Apids::ADC_MON, static_cast(tc::AdcMonId::SET_ADC_THRESHOLD), 4) {} ReturnValue_t buildPacket(uint32_t threshold) { auto res = checkSizeAndSerializeHeader(); @@ -773,8 +801,7 @@ class RunAutoEmTests : public TcBase { * @param test 1 - complete EM test, 2 - Short test (only memory readback NVM0,1,3) */ RunAutoEmTests(TcParams params) - : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::RUN_AUTO_EM_TEST), 1) { - } + : TcBase(params, Apids::TMTC_MAN, static_cast(tc::TmtcId::RUN_AUTO_EM_TEST), 1) {} ReturnValue_t buildPacket(uint8_t test) { auto res = checkSizeAndSerializeHeader(); @@ -805,7 +832,7 @@ class SetGpio : public TcBase { * @param val */ SetGpio(TcParams params) - : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::SET_GPIO), 3) {} + : TcBase(params, Apids::TMTC_MAN, static_cast(tc::TmtcId::SET_GPIO), 3) {} ReturnValue_t buildPacket(uint8_t port, uint8_t pin, uint8_t val) { auto res = checkSizeAndSerializeHeader(); @@ -841,7 +868,7 @@ class ReadGpio : public TcBase { * @param pin */ ReadGpio(TcParams params) - : TcBase(params, Apids::TMTC_MAN, static_cast(TmtcServiceIds::READ_GPIO), 2) {} + : TcBase(params, Apids::TMTC_MAN, static_cast(tc::TmtcId::READ_GPIO), 2) {} ReturnValue_t buildPacket(uint8_t port, uint8_t pin) { auto res = checkSizeAndSerializeHeader(); @@ -865,8 +892,7 @@ class ReadGpio : public TcBase { class SetShutdownTimeout : public TcBase { public: SetShutdownTimeout(TcParams params) - : TcBase(params, Apids::BOOT_MAN, static_cast(BootManServiceIds::SHUTDOWN_TIMEOUT), - 4) {} + : TcBase(params, Apids::BOOT_MAN, static_cast(tc::BootManId::SHUTDOWN_TIMEOUT), 4) {} ReturnValue_t buildPacket(uint32_t timeout) { auto res = checkSizeAndSerializeHeader(); @@ -898,7 +924,7 @@ class CheckMemory : public TcBase { * @param length Length in bytes of memory region */ CheckMemory(TcParams params) - : TcBase(params, Apids::MEM_MAN, static_cast(MemManServiceIds::CHECK), 10) {} + : TcBase(params, Apids::MEM_MAN, static_cast(tc::MemManId::CHECK), 10) {} ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { auto res = checkSizeAndSerializeHeader(); @@ -938,7 +964,7 @@ class WriteMemory : public TcBase { * @param updateData Pointer to buffer containing update data */ WriteMemory(TcParams params) - : TcBase(params, Apids::MEM_MAN, static_cast(MemManServiceIds::WRITE), 1) {} + : TcBase(params, Apids::MEM_MAN, static_cast(tc::MemManId::WRITE), 1) {} ReturnValue_t buildPacket(ccsds::SequenceFlags seqFlags, uint16_t sequenceCount, uint8_t memoryId, uint32_t startAddress, uint16_t length, uint8_t* updateData) { @@ -1001,8 +1027,7 @@ class WriteMemory : public TcBase { class EraseMemory : public TcBase { public: EraseMemory(TcParams params) - : TcBase(params, Apids::MEM_MAN, static_cast(MemManServiceIds::ERASE), - PAYLOAD_LENGTH) {} + : TcBase(params, Apids::MEM_MAN, static_cast(tc::MemManId::ERASE), PAYLOAD_LENGTH) {} ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { auto res = checkSizeAndSerializeHeader(); @@ -1032,61 +1057,95 @@ class EraseMemory : public TcBase { } }; -class VerificationReport : public ploc::SpTmReader { +class VerificationReport { public: - VerificationReport(const uint8_t* buf, size_t maxSize) : ploc::SpTmReader(buf, maxSize) {} + VerificationReport(TmBase& readerBase) : readerBase(readerBase) {} + + virtual ~VerificationReport() = default; + + virtual ReturnValue_t parse() { + if (not readerBase.crcIsOk()) { + return CRC_MISSMATCH; + } + if (readerBase.getApid() != Apids::TMTC_MAN) { + return APID_MISSMATCH; + } + if (readerBase.getBufSize() < MIN_PAYLOAD_LEN + 8) { + sif::error << "VerificationReport: Invalid verification report, payload too small" + << std::endl; + return BUF_TOO_SMALL; + } + const uint8_t* payloadStart = readerBase.getPayloadStart(); + size_t remLen = PAYLOAD_LEN; + ReturnValue_t result = SerializeAdapter::deSerialize(&refApid, &payloadStart, &remLen, + SerializeIF::Endianness::BIG); + if (result != returnvalue::OK) { + sif::debug << "VerificationReport: Failed to deserialize reference APID field" << std::endl; + return result; + } + result = SerializeAdapter::deSerialize(&refServiceId, &payloadStart, &remLen, + SerializeIF::Endianness::BIG); + if (result != returnvalue::OK) { + sif::debug << "VerificationReport: Failed to deserialize reference Service ID field" + << std::endl; + return result; + } + result = SerializeAdapter::deSerialize(&refSeqCount, &payloadStart, &remLen, + SerializeIF::Endianness::BIG); + if (result != returnvalue::OK) { + sif::debug << "VerificationReport: Failed to deserialize reference sequence count field" + << std::endl; + return result; + } + result = SerializeAdapter::deSerialize(&statusCode, &payloadStart, &remLen, + SerializeIF::Endianness::BIG); + if (result != returnvalue::OK) { + sif::debug << "VerificationReport: Failed to deserialize status code field" << std::endl; + return result; + } + return returnvalue::OK; + } /** * @brief Gets the APID of command which caused the transmission of this verification report. */ - uint16_t getRefApid() { - uint16_t refApid = 0; - size_t size = 0; - const uint8_t* refApidPtr = this->getPacketData(); - ReturnValue_t result = - SerializeAdapter::deSerialize(&refApid, refApidPtr, &size, SerializeIF::Endianness::BIG); - if (result != returnvalue::OK) { - sif::debug << "ExecutionReport: Failed to deserialize reference APID field" << std::endl; - return result; - } - return refApid; - } + uint8_t getRefApid() const { return refApid; } - uint16_t getStatusCode() { - uint16_t statusCode = 0; - size_t size = 0; - const uint8_t* statusCodePtr = this->getPacketData() + OFFSET_STATUS_CODE; - ReturnValue_t result = SerializeAdapter::deSerialize(&statusCode, statusCodePtr, &size, - SerializeIF::Endianness::BIG); - if (result != returnvalue::OK) { - sif::debug << "ExecutionReport: Failed to deserialize status code field" << std::endl; - return result; - } - return statusCode; - } + uint8_t getRefServiceId() const { return refServiceId; } + + uint16_t getRefSequenceCount() const { return refSeqCount; } + + uint32_t getStatusCode() const { return statusCode; } virtual ReturnValue_t checkApid() { return returnvalue::FAILED; } private: - static const uint8_t OFFSET_STATUS_CODE = 4; + TmBase& readerBase; + uint8_t refApid = 0; + uint8_t refServiceId = 0; + uint16_t refSeqCount = 0; + uint32_t statusCode = 0; + static const size_t PAYLOAD_LEN = 8; }; class AcknowledgmentReport : public VerificationReport { public: - AcknowledgmentReport(const uint8_t* buf, size_t maxSize) : VerificationReport(buf, maxSize) {} + AcknowledgmentReport(TmBase& readerBase) : VerificationReport(readerBase) {} - ReturnValue_t checkApid() { - uint16_t apid = this->getApid(); - if (apid == APID_ACK_SUCCESS) { - return returnvalue::OK; - } else if (apid == APID_ACK_FAILURE) { - printStatusInformation(); - return SupvReturnValuesIF::RECEIVED_ACK_FAILURE; - } else { - sif::warning << "AcknowledgmentReport::checkApid: Invalid apid: 0x" << std::hex << apid - << std::endl; - return SupvReturnValuesIF::INVALID_APID; - } + virtual ReturnValue_t parse() override { + // if (readerBase.getServiceId() != ) + // uint16_t apid = this->getApid(); + // if (apid == APID_ACK_SUCCESS) { + // return returnvalue::OK; + // } else if (apid == APID_ACK_FAILURE) { + // printStatusInformation(); + // return SupvReturnValuesIF::RECEIVED_ACK_FAILURE; + // } else { + // sif::warning << "AcknowledgmentReport::checkApid: Invalid apid: 0x" << std::hex << apid + // << std::endl; + // return SupvReturnValuesIF::INVALID_APID; + // } + return OK; } void printStatusInformation() { @@ -1147,20 +1206,21 @@ class AcknowledgmentReport : public VerificationReport { class ExecutionReport : public VerificationReport { public: - ExecutionReport(const uint8_t* buf, size_t maxSize) : VerificationReport(buf, maxSize) {} + ExecutionReport(TmBase& readerBase) : VerificationReport(readerBase) {} - ReturnValue_t checkApid() { - uint16_t apid = this->getApid(); - if (apid == APID_EXE_SUCCESS) { - return returnvalue::OK; - } else if (apid == APID_EXE_FAILURE) { - printStatusInformation(); - return SupvReturnValuesIF::RECEIVED_EXE_FAILURE; - } else { - sif::warning << "ExecutionReport::checkApid: Invalid apid: 0x" << std::hex << apid - << std::endl; - return SupvReturnValuesIF::INVALID_APID; - } + ReturnValue_t parse() override { + /* uint16_t apid = this->getApid(); + if (apid == APID_EXE_SUCCESS) { + return returnvalue::OK; + } else if (apid == APID_EXE_FAILURE) { + printStatusInformation(); + return SupvReturnValuesIF::RECEIVED_EXE_FAILURE; + } else { + sif::warning << "ExecutionReport::checkApid: Invalid apid: 0x" << std::hex << apid + << std::endl; + return SupvReturnValuesIF::INVALID_APID; + }*/ + return OK; } private: @@ -1808,9 +1868,9 @@ class MramCmd : public TcBase { ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { if (action == MramAction::WIPE) { - setServiceId(static_cast(DataLoggerServiceIds::WIPE_MRAM)); + setServiceId(static_cast(tc::DataLoggerServiceId::WIPE_MRAM)); } else if (action == MramAction::DUMP) { - setServiceId(static_cast(DataLoggerServiceIds::DUMP_MRAM)); + setServiceId(static_cast(tc::DataLoggerServiceId::DUMP_MRAM)); } else { sif::debug << "WipeMram: Invalid action specified"; } @@ -1899,7 +1959,7 @@ class SetAdcEnabledChannels : public TcBase { * @param ch Defines channels to be enabled or disabled. */ SetAdcEnabledChannels(TcParams params) - : TcBase(params, Apids::ADC_MON, static_cast(AdcMonServiceIds::SET_ENABLED_CHANNELS), + : TcBase(params, Apids::ADC_MON, static_cast(tc::AdcMonId::SET_ENABLED_CHANNELS), 2) {} ReturnValue_t buildPacket(uint16_t ch) { diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index cf63bea3..f26012a7 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -221,17 +221,17 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d spParams.buf = commandBuffer; switch (deviceCommand) { case GET_HK_REPORT: { - prepareEmptyCmd(Apids::HK, static_cast(HkServiceIds::GET_REPORT)); + prepareEmptyCmd(Apids::HK, static_cast(tc::HkId::GET_REPORT)); result = returnvalue::OK; break; } case START_MPSOC: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::START_MPSOC)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::START_MPSOC)); result = returnvalue::OK; break; } case SHUTDOWN_MPSOC: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::SHUTDOWN_MPSOC)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::SHUTDOWN_MPSOC)); result = returnvalue::OK; break; } @@ -241,7 +241,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case RESET_MPSOC: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::RESET_MPSOC)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::RESET_MPSOC)); result = returnvalue::OK; break; } @@ -265,8 +265,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case GET_BOOT_STATUS_REPORT: { - prepareEmptyCmd(Apids::BOOT_MAN, - static_cast(BootManServiceIds::GET_BOOT_STATUS_REPORT)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::GET_BOOT_STATUS_REPORT)); result = returnvalue::OK; break; } @@ -299,7 +298,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // } case GET_LATCHUP_STATUS_REPORT: { prepareEmptyCmd(Apids::LATCHUP_MON, - static_cast(LatchupMonServiceIds::GET_STATUS_REPORT)); + static_cast(tc::LatchupMonId::GET_STATUS_REPORT)); result = returnvalue::OK; break; } @@ -380,7 +379,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case FACTORY_FLASH: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::FACTORY_FLASH)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::FACTORY_FLASH)); result = returnvalue::OK; break; } @@ -434,7 +433,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // break; // } case RESET_PL: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(BootManServiceIds::RESET_PL)); + prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::RESET_PL)); result = returnvalue::OK; break; } @@ -930,56 +929,57 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { using namespace supv; ReturnValue_t result = returnvalue::OK; - AcknowledgmentReport ack(data, SIZE_ACK_REPORT); - result = ack.checkSize(); - if (result != returnvalue::OK) { - return result; - } - - result = ack.checkCrc(); - if (result != returnvalue::OK) { - sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; - nextReplyId = supv::NONE; - replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); - triggerEvent(SUPV_CRC_FAILURE_EVENT); - sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::CRC_FAILURE); - disableAllReplies(); - return returnvalue::OK; - } - - result = ack.checkApid(); - - switch (result) { - case SupvReturnValuesIF::RECEIVED_ACK_FAILURE: { - DeviceCommandId_t commandId = getPendingCommand(); - if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { - triggerEvent(SUPV_ACK_FAILURE, commandId, static_cast(ack.getStatusCode())); - } - printAckFailureInfo(ack.getStatusCode(), commandId); - sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::RECEIVED_ACK_FAILURE); - disableAllReplies(); - nextReplyId = supv::NONE; - result = IGNORE_REPLY_DATA; - break; - } - case returnvalue::OK: { - setNextReplyId(); - break; - } - case SupvReturnValuesIF::INVALID_APID: - sif::warning << "PlocSupervisorHandler::handleAckReport: Invalid APID in Ack report" - << std::endl; - sendFailureReport(supv::ACK_REPORT, result); - disableAllReplies(); - nextReplyId = supv::NONE; - result = IGNORE_REPLY_DATA; - break; - default: { - sif::error << "PlocSupervisorHandler::handleAckReport: APID parsing failed" << std::endl; - result = returnvalue::FAILED; - break; - } - } + // TODO: Fix + // AcknowledgmentReport ack(data, SIZE_ACK_REPORT); + // result = ack.checkSize(); + // if (result != returnvalue::OK) { + // return result; + // } + // + // result = ack.checkCrc(); + // if (result != returnvalue::OK) { + // sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; + // nextReplyId = supv::NONE; + // replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); + // triggerEvent(SUPV_CRC_FAILURE_EVENT); + // sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::CRC_FAILURE); + // disableAllReplies(); + // return returnvalue::OK; + // } + // + // result = ack.checkApid(); + // + // switch (result) { + // case SupvReturnValuesIF::RECEIVED_ACK_FAILURE: { + // DeviceCommandId_t commandId = getPendingCommand(); + // if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { + // triggerEvent(SUPV_ACK_FAILURE, commandId, static_cast(ack.getStatusCode())); + // } + // printAckFailureInfo(ack.getStatusCode(), commandId); + // sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::RECEIVED_ACK_FAILURE); + // disableAllReplies(); + // nextReplyId = supv::NONE; + // result = IGNORE_REPLY_DATA; + // break; + // } + // case returnvalue::OK: { + // setNextReplyId(); + // break; + // } + // case SupvReturnValuesIF::INVALID_APID: + // sif::warning << "PlocSupervisorHandler::handleAckReport: Invalid APID in Ack report" + // << std::endl; + // sendFailureReport(supv::ACK_REPORT, result); + // disableAllReplies(); + // nextReplyId = supv::NONE; + // result = IGNORE_REPLY_DATA; + // break; + // default: { + // sif::error << "PlocSupervisorHandler::handleAckReport: APID parsing failed" << std::endl; + // result = returnvalue::FAILED; + // break; + // } + // } return result; } @@ -987,38 +987,39 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data) using namespace supv; ReturnValue_t result = returnvalue::OK; - ExecutionReport exe(data, SIZE_EXE_REPORT); - result = exe.checkSize(); - if (result != returnvalue::OK) { - return result; - } - - result = exe.checkCrc(); - if (result != returnvalue::OK) { - sif::error << "PlocSupervisorHandler::handleExecutionReport: CRC failure" << std::endl; - nextReplyId = supv::NONE; - return result; - } - - result = exe.checkApid(); - - switch (result) { - case (returnvalue::OK): { - handleExecutionSuccessReport(data); - break; - } - case (SupvReturnValuesIF::RECEIVED_EXE_FAILURE): { - handleExecutionFailureReport(exe.getStatusCode()); - result = returnvalue::OK; - break; - } - default: { - sif::error << "PlocSupervisorHandler::handleExecutionReport: Unknown APID" << std::endl; - result = returnvalue::FAILED; - break; - } - } - nextReplyId = supv::NONE; + // TODO: Fix + // ExecutionReport exe(data, SIZE_EXE_REPORT); + // result = exe.checkSize(); + // if (result != returnvalue::OK) { + // return result; + // } + // + // result = exe.checkCrc(); + // if (result != returnvalue::OK) { + // sif::error << "PlocSupervisorHandler::handleExecutionReport: CRC failure" << std::endl; + // nextReplyId = supv::NONE; + // return result; + // } + // + // result = exe.checkApid(); + // + // switch (result) { + // case (returnvalue::OK): { + // handleExecutionSuccessReport(data); + // break; + // } + // case (SupvReturnValuesIF::RECEIVED_EXE_FAILURE): { + // handleExecutionFailureReport(exe.getStatusCode()); + // result = returnvalue::OK; + // break; + // } + // default: { + // sif::error << "PlocSupervisorHandler::handleExecutionReport: Unknown APID" << std::endl; + // result = returnvalue::FAILED; + // break; + // } + // } + // nextReplyId = supv::NONE; return result; } @@ -2107,33 +2108,36 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(const uint8_t* DeviceCommandId_t commandId = getPendingCommand(); switch (commandId) { case supv::READ_GPIO: { - supv::ExecutionReport exe(data, supv::SIZE_EXE_REPORT); - if (exe.isNull()) { - return returnvalue::FAILED; - } - ReturnValue_t result = exe.checkSize(); - if (result != returnvalue::OK) { - return result; - } - uint16_t gpioState = exe.getStatusCode(); -#if OBSW_DEBUG_PLOC_SUPERVISOR == 1 - sif::info << "PlocSupervisorHandler: Read GPIO TM, State: " << gpioState << std::endl; -#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ - DeviceCommandMap::iterator iter = deviceCommandMap.find(commandId); - if (iter->second.sendReplyTo == NO_COMMAND_ID) { - return returnvalue::OK; - } - uint8_t data[sizeof(gpioState)]; - size_t size = 0; - result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState), - SerializeIF::Endianness::BIG); - if (result != returnvalue::OK) { - sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << std::endl; - } - result = actionHelper.reportData(iter->second.sendReplyTo, commandId, data, sizeof(data)); - if (result != returnvalue::OK) { - sif::warning << "PlocSupervisorHandler: Read GPIO, failed to report data" << std::endl; - } + // TODO: Fix + // supv::ExecutionReport exe(data, supv::SIZE_EXE_REPORT); + // if (exe.isNull()) { + // return returnvalue::FAILED; + // } + // ReturnValue_t result = exe.checkSize(); + // if (result != returnvalue::OK) { + // return result; + // } + // uint16_t gpioState = exe.getStatusCode(); + //#if OBSW_DEBUG_PLOC_SUPERVISOR == 1 + // sif::info << "PlocSupervisorHandler: Read GPIO TM, State: " << gpioState << std::endl; + //#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ + // DeviceCommandMap::iterator iter = deviceCommandMap.find(commandId); + // if (iter->second.sendReplyTo == NO_COMMAND_ID) { + // return returnvalue::OK; + // } + // uint8_t data[sizeof(gpioState)]; + // size_t size = 0; + // result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState), + // SerializeIF::Endianness::BIG); + // if (result != returnvalue::OK) { + // sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << + // std::endl; + // } + // result = actionHelper.reportData(iter->second.sendReplyTo, commandId, data, + // sizeof(data)); if (result != returnvalue::OK) { + // sif::warning << "PlocSupervisorHandler: Read GPIO, failed to report data" << + // std::endl; + // } break; } case supv::SET_TIME_REF: { diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 604a6c5e..53488ce2 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -501,7 +501,7 @@ ReturnValue_t PlocSupvHelper::prepareUpdate() { ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::NoPayloadPacket packet(spParams, Apids::BOOT_MAN, - static_cast(BootManServiceIds::PREPARE_UPDATE)); + static_cast(tc::BootManId::PREPARE_UPDATE)); result = packet.buildPacket(); if (result != returnvalue::OK) { return result; @@ -522,7 +522,7 @@ ReturnValue_t PlocSupvHelper::eraseMemory() { if (result != returnvalue::OK) { return result; } - result = handlePacketTransmission(eraseMemory, supv::recv_timeout::ERASE_MEMORY); + result = handlePacketTransmission(eraseMemory, supv::timeout::ERASE_MEMORY); if (result != returnvalue::OK) { return result; } @@ -562,27 +562,28 @@ ReturnValue_t PlocSupvHelper::sendCommand(ploc::SpTcBase& packet) { ReturnValue_t PlocSupvHelper::handleAck() { ReturnValue_t result = returnvalue::OK; - result = handleTmReception(supv::SIZE_ACK_REPORT); - if (result != returnvalue::OK) { - triggerEvent(ACK_RECEPTION_FAILURE, result, static_cast(rememberApid)); - sif::warning << "PlocSupvHelper::handleAck: Error in reception of acknowledgment report" - << std::endl; - return result; - } - supv::AcknowledgmentReport ackReport(tmBuf.data(), tmBuf.size()); - result = checkReceivedTm(ackReport); - if (result != returnvalue::OK) { - return result; - } - result = ackReport.checkApid(); - if (result != returnvalue::OK) { - if (result == SupvReturnValuesIF::RECEIVED_ACK_FAILURE) { - triggerEvent(SUPV_ACK_FAILURE_REPORT, static_cast(ackReport.getRefApid())); - } else if (result == SupvReturnValuesIF::INVALID_APID) { - triggerEvent(SUPV_ACK_INVALID_APID, static_cast(rememberApid)); - } - return result; - } + // TODO: Fix + // result = handleTmReception(supv::SIZE_ACK_REPORT); + // if (result != returnvalue::OK) { + // triggerEvent(ACK_RECEPTION_FAILURE, result, static_cast(rememberApid)); + // sif::warning << "PlocSupvHelper::handleAck: Error in reception of acknowledgment report" + // << std::endl; + // return result; + // } + // supv::AcknowledgmentReport ackReport(tmBuf.data(), tmBuf.size()); + // result = checkReceivedTm(ackReport); + // if (result != returnvalue::OK) { + // return result; + // } + // result = ackReport.checkApid(); + // if (result != returnvalue::OK) { + // if (result == SupvReturnValuesIF::RECEIVED_ACK_FAILURE) { + // triggerEvent(SUPV_ACK_FAILURE_REPORT, static_cast(ackReport.getRefApid())); + // } else if (result == SupvReturnValuesIF::INVALID_APID) { + // triggerEvent(SUPV_ACK_INVALID_APID, static_cast(rememberApid)); + // } + // return result; + // } return returnvalue::OK; } @@ -601,22 +602,23 @@ ReturnValue_t PlocSupvHelper::handleExe(uint32_t timeout) { } ReturnValue_t PlocSupvHelper::exeReportHandling() { - supv::ExecutionReport exeReport(tmBuf.data(), tmBuf.size()); - - ReturnValue_t result = checkReceivedTm(exeReport); - if (result != returnvalue::OK) { - return result; - } - result = exeReport.checkApid(); - if (result != returnvalue::OK) { - if (result == SupvReturnValuesIF::RECEIVED_EXE_FAILURE) { - triggerEvent(SUPV_EXE_FAILURE_REPORT, static_cast(exeReport.getRefApid())); - } else if (result == SupvReturnValuesIF::INVALID_APID) { - triggerEvent(SUPV_EXE_INVALID_APID, static_cast(rememberApid)); - } - return result; - } - return result; + // TODO: Fix + // supv::ExecutionReport exeReport(tmBuf.data(), tmBuf.size()); + // + // ReturnValue_t result = checkReceivedTm(exeReport); + // if (result != returnvalue::OK) { + // return result; + // } + // result = exeReport.checkApid(); + // if (result != returnvalue::OK) { + // if (result == SupvReturnValuesIF::RECEIVED_EXE_FAILURE) { + // triggerEvent(SUPV_EXE_FAILURE_REPORT, static_cast(exeReport.getRefApid())); + // } else if (result == SupvReturnValuesIF::INVALID_APID) { + // triggerEvent(SUPV_EXE_INVALID_APID, static_cast(rememberApid)); + // } + // return result; + // } + return OK; } ReturnValue_t PlocSupvHelper::handleTmReception(size_t remainingBytes, uint8_t* readBuf, @@ -753,7 +755,7 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { } bool exeAlreadyHandled = false; - uint32_t timeout = std::max(CRC_EXECUTION_TIMEOUT, supv::recv_timeout::UPDATE_STATUS_REPORT); + uint32_t timeout = std::max(CRC_EXECUTION_TIMEOUT, supv::timeout::UPDATE_STATUS_REPORT); result = handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), timeout); ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size()); if (spReader.getApid() == supv::APID_EXE_FAILURE) { @@ -762,7 +764,7 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { } else if (spReader.getApid() == supv::APID_UPDATE_STATUS_REPORT) { size_t remBytes = spReader.getPacketDataLen() + 1; result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN, - supv::recv_timeout::UPDATE_STATUS_REPORT); + supv::timeout::UPDATE_STATUS_REPORT); if (result != returnvalue::OK) { sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: Failed to receive update status report" @@ -860,8 +862,8 @@ ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reade return EVENT_BUFFER_REPLY_INVALID_APID; } // TODO: Fix -// file.write(reinterpret_cast(reader.getPacketData()), -// reader.getPayloadDataLength()); + // file.write(reinterpret_cast(reader.getPacketData()), + // reader.getPayloadDataLength()); } return result; } diff --git a/mission/devices/devicedefinitions/SpBase.h b/mission/devices/devicedefinitions/SpBase.h index 6cd4a803..f34e921a 100644 --- a/mission/devices/devicedefinitions/SpBase.h +++ b/mission/devices/devicedefinitions/SpBase.h @@ -102,7 +102,7 @@ class SpTmReader : public SpacePacketReader { return setReadOnlyData(buf, maxSize); } - ReturnValue_t checkCrc() { + ReturnValue_t checkCrc() const { if (CRC::crc16ccitt(getFullData(), getFullPacketLen()) != 0) { return returnvalue::FAILED; } From cfe7599f62787f3b7f872ec5b7a1b45303103f79 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 11:18:03 +0100 Subject: [PATCH 107/244] use result namespace instead of interface --- .../PlocSupervisorDefinitions.h | 105 +++++++++++++----- .../devicedefinitions/SupvReturnValuesIF.h | 61 ---------- linux/devices/ploc/PlocSupervisorHandler.cpp | 44 ++++---- linux/devices/ploc/PlocSupervisorHandler.h | 1 - 4 files changed, 102 insertions(+), 109 deletions(-) delete mode 100644 linux/devices/devicedefinitions/SupvReturnValuesIF.h diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index c564b6e4..aa28c143 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -10,34 +10,67 @@ #include #include -#include "linux/devices/devicedefinitions/SupvReturnValuesIF.h" #include "mission/devices/devicedefinitions/SpBase.h" using namespace returnvalue; namespace supv { -static const uint8_t CLASS_ID = CLASS_ID::PLOC_SUPERVISOR_HANDLER; +namespace result { +static const uint8_t INTERFACE_ID = CLASS_ID::SUPV_RETURN_VALUES_IF; -static constexpr ReturnValue_t CRC_MISSMATCH = makeCode(CLASS_ID, 1); -static constexpr ReturnValue_t APID_MISSMATCH = makeCode(CLASS_ID, 2); -static constexpr ReturnValue_t BUF_TOO_SMALL = makeCode(CLASS_ID, 3); +//! [EXPORT] : [COMMENT] Space Packet received from PLOC supervisor has invalid CRC +static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA0); +//! [EXPORT] : [COMMENT] Received ACK failure reply from PLOC supervisor +static const ReturnValue_t RECEIVED_ACK_FAILURE = MAKE_RETURN_CODE(0xA1); +//! [EXPORT] : [COMMENT] Received execution failure reply from PLOC supervisor +static const ReturnValue_t RECEIVED_EXE_FAILURE = MAKE_RETURN_CODE(0xA2); +//! [EXPORT] : [COMMENT] Received space packet with invalid APID from PLOC supervisor +static const ReturnValue_t INVALID_APID = MAKE_RETURN_CODE(0xA3); +//! [EXPORT] : [COMMENT] Failed to read current system time +static const ReturnValue_t GET_TIME_FAILURE = MAKE_RETURN_CODE(0xA4); +//! [EXPORT] : [COMMENT] Received command with invalid watchdog parameter. Valid watchdogs are 0 +//! for PS, 1 for PL and 2 for INT +static const ReturnValue_t INVALID_WATCHDOG = MAKE_RETURN_CODE(0xA5); +//! [EXPORT] : [COMMENT] Received watchdog timeout config command with invalid timeout. Valid +//! timeouts must be in the range between 1000 and 360000 ms. +static const ReturnValue_t INVALID_WATCHDOG_TIMEOUT = MAKE_RETURN_CODE(0xA6); +//! [EXPORT] : [COMMENT] Received latchup config command with invalid latchup ID +static const ReturnValue_t INVALID_LATCHUP_ID = MAKE_RETURN_CODE(0xA7); +//! [EXPORT] : [COMMENT] Received set adc sweep period command with invalid sweep period. Must be +//! larger than 21. +static const ReturnValue_t SWEEP_PERIOD_TOO_SMALL = MAKE_RETURN_CODE(0xA8); +//! [EXPORT] : [COMMENT] Receive auto EM test command with invalid test param. Valid params are 1 +//! and 2. +static const ReturnValue_t INVALID_TEST_PARAM = MAKE_RETURN_CODE(0xA9); +//! [EXPORT] : [COMMENT] Returned when scanning for MRAM dump packets failed. +static const ReturnValue_t MRAM_PACKET_PARSING_FAILURE = MAKE_RETURN_CODE(0xAA); +//! [EXPORT] : [COMMENT] Returned when the start and stop addresses of the MRAM dump or MRAM wipe +//! commands are invalid (e.g. start address bigger than stop address) +static const ReturnValue_t INVALID_MRAM_ADDRESSES = MAKE_RETURN_CODE(0xAB); +//! [EXPORT] : [COMMENT] Expect reception of an MRAM dump packet but received space packet with +//! other apid. +static const ReturnValue_t NO_MRAM_PACKET = MAKE_RETURN_CODE(0xAC); +//! [EXPORT] : [COMMENT] Path to PLOC directory on SD card does not exist +static const ReturnValue_t PATH_DOES_NOT_EXIST = MAKE_RETURN_CODE(0xAD); +//! [EXPORT] : [COMMENT] MRAM dump file does not exists. The file should actually already have +//! been created with the reception of the first dump packet. +static const ReturnValue_t MRAM_FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xAE); +//! [EXPORT] : [COMMENT] Received action command has invalid length +static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xAF); +//! [EXPORT] : [COMMENT] Filename too long +static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xB0); +//! [EXPORT] : [COMMENT] Received update status report with invalid packet length field +static const ReturnValue_t UPDATE_STATUS_REPORT_INVALID_LENGTH = MAKE_RETURN_CODE(0xB1); +//! [EXPORT] : [COMMENT] Update status report does not contain expected CRC. There might be a bit +//! flip in the update memory region. +static const ReturnValue_t UPDATE_CRC_FAILURE = MAKE_RETURN_CODE(0xB2); +//! [EXPORT] : [COMMENT] Supervisor helper task ist currently executing a command (wait until +//! helper tas has finished or interrupt by sending the terminate command) +static const ReturnValue_t SUPV_HELPER_EXECUTING = MAKE_RETURN_CODE(0xB3); -typedef struct { - // The most significant bit of msec value is set to 0x80 to indicate that full - // time and data information is transmitted, when the time has been synced with - // the reference. If the time has not been synced with reference, then the most - // significant bit is set to 0x00. Only the most significant bit is used for - // this purpose (bit 15 of the field tm_msec) - uint16_t tm_msec; // miliseconds 0-999; - uint8_t tm_sec; // seconds after the minute, 0 to 60 - // (0 - 60 allows for the occasional leap second) - uint8_t tm_min; // minutes after the hour, 0 to 59 - uint8_t tm_hour; // hours since midnight, 0 to 23 - uint8_t tm_mday; // day of the month, 1 to 31 - uint8_t tm_mon; // months 1 to 12 - uint8_t tm_year; // years since 1900 -} tas_time_t; +static constexpr ReturnValue_t BUF_TOO_SMALL = MAKE_RETURN_CODE(0xC0); +}; // namespace result static constexpr uint16_t DEFAULT_SEQ_COUNT = 0; @@ -1065,15 +1098,15 @@ class VerificationReport { virtual ReturnValue_t parse() { if (not readerBase.crcIsOk()) { - return CRC_MISSMATCH; + return result::CRC_FAILURE; } if (readerBase.getApid() != Apids::TMTC_MAN) { - return APID_MISSMATCH; + return result::INVALID_APID; } if (readerBase.getBufSize() < MIN_PAYLOAD_LEN + 8) { sif::error << "VerificationReport: Invalid verification report, payload too small" << std::endl; - return BUF_TOO_SMALL; + return result::BUF_TOO_SMALL; } const uint8_t* payloadStart = readerBase.getPayloadStart(); size_t remLen = PAYLOAD_LEN; @@ -1119,7 +1152,7 @@ class VerificationReport { virtual ReturnValue_t checkApid() { return returnvalue::FAILED; } - private: + protected: TmBase& readerBase; uint8_t refApid = 0; uint8_t refServiceId = 0; @@ -1209,6 +1242,10 @@ class ExecutionReport : public VerificationReport { ExecutionReport(TmBase& readerBase) : VerificationReport(readerBase) {} ReturnValue_t parse() override { + if (readerBase.getServiceId() == static_cast(tm::TmtcId::EXEC_NAK)) { + printStatusInformation(); + return result::RECEIVED_EXE_FAILURE; + } /* uint16_t apid = this->getApid(); if (apid == APID_EXE_SUCCESS) { return returnvalue::OK; @@ -1660,7 +1697,7 @@ class UpdateStatusReport : public ploc::SpTmReader { ReturnValue_t verifycrc(uint16_t goodCrc) const { if (crc != goodCrc) { - return SupvReturnValuesIF::UPDATE_CRC_FAILURE; + return result::UPDATE_CRC_FAILURE; } return returnvalue::OK; } @@ -1681,7 +1718,7 @@ class UpdateStatusReport : public ploc::SpTmReader { ReturnValue_t lengthCheck() { if (getFullPacketLen() != FULL_SIZE) { - return SupvReturnValuesIF::UPDATE_STATUS_REPORT_INVALID_LENGTH; + return result::UPDATE_STATUS_REPORT_INVALID_LENGTH; } return returnvalue::OK; } @@ -2020,6 +2057,22 @@ class RequestLoggingData : public TcBase { static const uint8_t TPC_OFFSET = 1; }; +typedef struct { + // The most significant bit of msec value is set to 0x80 to indicate that full + // time and data information is transmitted, when the time has been synced with + // the reference. If the time has not been synced with reference, then the most + // significant bit is set to 0x00. Only the most significant bit is used for + // this purpose (bit 15 of the field tm_msec) + uint16_t tm_msec; // miliseconds 0-999; + uint8_t tm_sec; // seconds after the minute, 0 to 60 + // (0 - 60 allows for the occasional leap second) + uint8_t tm_min; // minutes after the hour, 0 to 59 + uint8_t tm_hour; // hours since midnight, 0 to 23 + uint8_t tm_mday; // day of the month, 1 to 31 + uint8_t tm_mon; // months 1 to 12 + uint8_t tm_year; // years since 1900 +} tas_time_t; + } // namespace notimpl } // namespace supv diff --git a/linux/devices/devicedefinitions/SupvReturnValuesIF.h b/linux/devices/devicedefinitions/SupvReturnValuesIF.h deleted file mode 100644 index f4557735..00000000 --- a/linux/devices/devicedefinitions/SupvReturnValuesIF.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef SUPV_RETURN_VALUES_IF_H_ -#define SUPV_RETURN_VALUES_IF_H_ - -#include "fsfw/returnvalues/returnvalue.h" - -class SupvReturnValuesIF { - public: - static const uint8_t INTERFACE_ID = CLASS_ID::SUPV_RETURN_VALUES_IF; - - //! [EXPORT] : [COMMENT] Space Packet received from PLOC supervisor has invalid CRC - static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA0); - //! [EXPORT] : [COMMENT] Received ACK failure reply from PLOC supervisor - static const ReturnValue_t RECEIVED_ACK_FAILURE = MAKE_RETURN_CODE(0xA1); - //! [EXPORT] : [COMMENT] Received execution failure reply from PLOC supervisor - static const ReturnValue_t RECEIVED_EXE_FAILURE = MAKE_RETURN_CODE(0xA2); - //! [EXPORT] : [COMMENT] Received space packet with invalid APID from PLOC supervisor - static const ReturnValue_t INVALID_APID = MAKE_RETURN_CODE(0xA3); - //! [EXPORT] : [COMMENT] Failed to read current system time - static const ReturnValue_t GET_TIME_FAILURE = MAKE_RETURN_CODE(0xA4); - //! [EXPORT] : [COMMENT] Received command with invalid watchdog parameter. Valid watchdogs are 0 - //! for PS, 1 for PL and 2 for INT - static const ReturnValue_t INVALID_WATCHDOG = MAKE_RETURN_CODE(0xA5); - //! [EXPORT] : [COMMENT] Received watchdog timeout config command with invalid timeout. Valid - //! timeouts must be in the range between 1000 and 360000 ms. - static const ReturnValue_t INVALID_WATCHDOG_TIMEOUT = MAKE_RETURN_CODE(0xA6); - //! [EXPORT] : [COMMENT] Received latchup config command with invalid latchup ID - static const ReturnValue_t INVALID_LATCHUP_ID = MAKE_RETURN_CODE(0xA7); - //! [EXPORT] : [COMMENT] Received set adc sweep period command with invalid sweep period. Must be - //! larger than 21. - static const ReturnValue_t SWEEP_PERIOD_TOO_SMALL = MAKE_RETURN_CODE(0xA8); - //! [EXPORT] : [COMMENT] Receive auto EM test command with invalid test param. Valid params are 1 - //! and 2. - static const ReturnValue_t INVALID_TEST_PARAM = MAKE_RETURN_CODE(0xA9); - //! [EXPORT] : [COMMENT] Returned when scanning for MRAM dump packets failed. - static const ReturnValue_t MRAM_PACKET_PARSING_FAILURE = MAKE_RETURN_CODE(0xAA); - //! [EXPORT] : [COMMENT] Returned when the start and stop addresses of the MRAM dump or MRAM wipe - //! commands are invalid (e.g. start address bigger than stop address) - static const ReturnValue_t INVALID_MRAM_ADDRESSES = MAKE_RETURN_CODE(0xAB); - //! [EXPORT] : [COMMENT] Expect reception of an MRAM dump packet but received space packet with - //! other apid. - static const ReturnValue_t NO_MRAM_PACKET = MAKE_RETURN_CODE(0xAC); - //! [EXPORT] : [COMMENT] Path to PLOC directory on SD card does not exist - static const ReturnValue_t PATH_DOES_NOT_EXIST = MAKE_RETURN_CODE(0xAD); - //! [EXPORT] : [COMMENT] MRAM dump file does not exists. The file should actually already have - //! been created with the reception of the first dump packet. - static const ReturnValue_t MRAM_FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xAE); - //! [EXPORT] : [COMMENT] Received action command has invalid length - static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xAF); - //! [EXPORT] : [COMMENT] Filename too long - static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xB0); - //! [EXPORT] : [COMMENT] Received update status report with invalid packet length field - static const ReturnValue_t UPDATE_STATUS_REPORT_INVALID_LENGTH = MAKE_RETURN_CODE(0xB1); - //! [EXPORT] : [COMMENT] Update status report does not contain expected CRC. There might be a bit - //! flip in the update memory region. - static const ReturnValue_t UPDATE_CRC_FAILURE = MAKE_RETURN_CODE(0xB2); - //! [EXPORT] : [COMMENT] Supervisor helper task ist currently executing a command (wait until - //! helper tas has finished or interrupt by sending the terminate command) - static const ReturnValue_t SUPV_HELPER_EXECUTING = MAKE_RETURN_CODE(0xB3); -}; - -#endif /* SUPV_RETURN_VALUES_IF_H_ */ diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index f26012a7..6ec92f98 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -14,6 +14,8 @@ #include "fsfw/ipc/QueueFactory.h" #include "fsfw/timemanager/Clock.h" +using namespace supv; + PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, Gpio uartIsolatorSwitch, power::Switch_t powerSwitch, @@ -103,7 +105,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, } if (plocSupvHelperExecuting) { - return SupvReturnValuesIF::SUPV_HELPER_EXECUTING; + return result::SUPV_HELPER_EXECUTING; } result = acceptExternalDeviceCommands(); @@ -114,7 +116,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, switch (actionId) { case PERFORM_UPDATE: { if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { - return SupvReturnValuesIF::FILENAME_TOO_LONG; + return result::FILENAME_TOO_LONG; } UpdateParams params; result = extractUpdateCommand(data, size, params); @@ -148,7 +150,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, } case LOGGING_REQUEST_EVENT_BUFFERS: { if (size > config::MAX_PATH_SIZE) { - return SupvReturnValuesIF::FILENAME_TOO_LONG; + return result::FILENAME_TOO_LONG; } result = supvHelper->startEventBufferRequest( std::string(reinterpret_cast(data), size)); @@ -704,7 +706,7 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r default: { sif::debug << "PlocSupervisorHandler::scanForReply: Reply has invalid apid" << std::endl; *foundLen = remainingSize; - return SupvReturnValuesIF::INVALID_APID; + return result::INVALID_APID; } } @@ -920,7 +922,7 @@ void PlocSupervisorHandler::setExecutionTimeout(DeviceCommandId_t command) { ReturnValue_t PlocSupervisorHandler::verifyPacket(const uint8_t* start, size_t foundLen) { if (CRC::crc16ccitt(start, foundLen) != 0) { - return SupvReturnValuesIF::CRC_FAILURE; + return result::CRC_FAILURE; } return returnvalue::OK; } @@ -1028,7 +1030,7 @@ ReturnValue_t PlocSupervisorHandler::handleHkReport(const uint8_t* data) { result = verifyPacket(data, supv::SIZE_HK_REPORT); - if (result == SupvReturnValuesIF::CRC_FAILURE) { + if (result == result::CRC_FAILURE) { sif::error << "PlocSupervisorHandler::handleHkReport: Hk report has invalid crc" << std::endl; return result; } @@ -1104,7 +1106,7 @@ ReturnValue_t PlocSupervisorHandler::handleBootStatusReport(const uint8_t* data) result = verifyPacket(data, supv::SIZE_BOOT_STATUS_REPORT); - if (result == SupvReturnValuesIF::CRC_FAILURE) { + if (result == result::CRC_FAILURE) { sif::error << "PlocSupervisorHandler::handleBootStatusReport: Boot status report has invalid" " crc" << std::endl; @@ -1170,7 +1172,7 @@ ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* da result = verifyPacket(data, supv::SIZE_LATCHUP_STATUS_REPORT); - if (result == SupvReturnValuesIF::CRC_FAILURE) { + if (result == result::CRC_FAILURE) { sif::error << "PlocSupervisorHandler::handleLatchupStatusReport: Latchup status report has " << "invalid crc" << std::endl; return result; @@ -1290,7 +1292,7 @@ ReturnValue_t PlocSupervisorHandler::handleAdcReport(const uint8_t* data) { result = verifyPacket(data, supv::SIZE_ADC_REPORT); - if (result == SupvReturnValuesIF::CRC_FAILURE) { + if (result == result::CRC_FAILURE) { sif::error << "PlocSupervisorHandler::handleAdcReport: ADC report has " << "invalid crc" << std::endl; return result; @@ -1446,7 +1448,7 @@ ReturnValue_t PlocSupervisorHandler::prepareSetTimeRefCmd() { if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::prepareSetTimeRefCmd: Failed to get current time" << std::endl; - return SupvReturnValuesIF::GET_TIME_FAILURE; + return result::GET_TIME_FAILURE; } supv::SetTimeRef packet(spParams); result = packet.buildPacket(&time); @@ -1495,7 +1497,7 @@ ReturnValue_t PlocSupervisorHandler::prepareLatchupConfigCmd(const uint8_t* comm ReturnValue_t result = returnvalue::OK; uint8_t latchupId = *commandData; if (latchupId > 6) { - return SupvReturnValuesIF::INVALID_LATCHUP_ID; + return result::INVALID_LATCHUP_ID; } switch (deviceCommand) { case (supv::ENABLE_LATCHUP_ALERT): { @@ -1533,7 +1535,7 @@ ReturnValue_t PlocSupervisorHandler::prepareSetAlertLimitCmd(const uint8_t* comm uint32_t dutycycle = *(commandData + offset) << 24 | *(commandData + offset + 1) << 16 | *(commandData + offset + 2) << 8 | *(commandData + offset + 3); if (latchupId > 6) { - return SupvReturnValuesIF::INVALID_LATCHUP_ID; + return result::INVALID_LATCHUP_ID; } supv::SetAlertlimit packet(spParams); ReturnValue_t result = packet.buildPacket(latchupId, dutycycle); @@ -1585,7 +1587,7 @@ ReturnValue_t PlocSupervisorHandler::prepareSetAdcThresholdCmd(const uint8_t* co ReturnValue_t PlocSupervisorHandler::prepareRunAutoEmTest(const uint8_t* commandData) { uint8_t test = *commandData; if (test != 1 && test != 2) { - return SupvReturnValuesIF::INVALID_TEST_PARAM; + return result::INVALID_TEST_PARAM; } supv::RunAutoEmTests packet(spParams); ReturnValue_t result = packet.buildPacket(test); @@ -1831,7 +1833,7 @@ ReturnValue_t PlocSupervisorHandler::parseMramPackets(const uint8_t* packet, siz sif::info << "PlocSupervisorHandler::parseMramPackets: Can not find MRAM packet in space " "packet buffer" << std::endl; - return SupvReturnValuesIF::MRAM_PACKET_PARSING_FAILURE; + return result::MRAM_PACKET_PARSING_FAILURE; } } @@ -1917,7 +1919,7 @@ void PlocSupervisorHandler::increaseExpectedMramReplies(DeviceCommandId_t id) { ReturnValue_t PlocSupervisorHandler::checkMramPacketApid() { uint16_t apid = (spacePacketBuffer[0] << 8 | spacePacketBuffer[1]) & supv::APID_MASK; if (apid != supv::APID_MRAM_DUMP_TM) { - return SupvReturnValuesIF::NO_MRAM_PACKET; + return result::NO_MRAM_PACKET; } return APERIODIC_REPLY; } @@ -1943,7 +1945,7 @@ ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) { if (not std::filesystem::exists(activeMramFile)) { sif::warning << "PlocSupervisorHandler::handleMramDumpFile: MRAM file does not exist" << std::endl; - return SupvReturnValuesIF::MRAM_FILE_NOT_EXISTS; + return result::MRAM_FILE_NOT_EXISTS; } std::ofstream file(activeMramFile, std::ios_base::app | std::ios_base::out); file.write(reinterpret_cast(spacePacketBuffer + ccsds::HEADER_LEN), packetLen - 1); @@ -1979,7 +1981,7 @@ ReturnValue_t PlocSupervisorHandler::createMramDumpFile() { if (not std::filesystem::exists(std::string(currentMountPrefix + "/" + supervisorFilePath))) { sif::warning << "PlocSupervisorHandler::createMramDumpFile: Supervisor path does not exist" << std::endl; - return SupvReturnValuesIF::PATH_DOES_NOT_EXIST; + return result::PATH_DOES_NOT_EXIST; } activeMramFile = currentMountPrefix + "/" + supervisorFilePath + "/" + filename; // Create new file @@ -1995,7 +1997,7 @@ ReturnValue_t PlocSupervisorHandler::getTimeStampString(std::string& timeStamp) if (result != returnvalue::OK) { sif::warning << "PlocSupervisorHandler::getTimeStampString: Failed to get current time" << std::endl; - return SupvReturnValuesIF::GET_TIME_FAILURE; + return result::GET_TIME_FAILURE; } timeStamp = std::to_string(time.year) + "-" + std::to_string(time.month) + "-" + std::to_string(time.day) + "--" + std::to_string(time.hour) + "-" + @@ -2010,7 +2012,7 @@ ReturnValue_t PlocSupervisorHandler::extractUpdateCommand(const uint8_t* command sizeof(params.startAddr) + sizeof(params.bytesWritten) + sizeof(params.seqCount) + sizeof(uint8_t)) { sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Data size too big" << std::endl; - return SupvReturnValuesIF::INVALID_LENGTH; + return result::INVALID_LENGTH; } ReturnValue_t result = returnvalue::OK; result = extractBaseParams(&commandData, size, params); @@ -2059,7 +2061,7 @@ ReturnValue_t PlocSupervisorHandler::extractBaseParams(const uint8_t** commandDa params.file = std::string(reinterpret_cast(*commandData)); if (params.file.size() > (config::MAX_FILENAME_SIZE + config::MAX_PATH_SIZE)) { sif::warning << "PlocSupervisorHandler::extractUpdateCommand: Filename too long" << std::endl; - return SupvReturnValuesIF::FILENAME_TOO_LONG; + return result::FILENAME_TOO_LONG; } *commandData += params.file.size() + SIZE_NULL_TERMINATOR; remSize -= (params.file.size() + SIZE_NULL_TERMINATOR); @@ -2158,7 +2160,7 @@ void PlocSupervisorHandler::handleExecutionFailureReport(uint16_t statusCode) { if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { triggerEvent(SUPV_EXE_FAILURE, commandId, static_cast(statusCode)); } - sendFailureReport(EXE_REPORT, SupvReturnValuesIF::RECEIVED_EXE_FAILURE); + sendFailureReport(EXE_REPORT, result::RECEIVED_EXE_FAILURE); disableExeReportReply(); } diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 21ae743d..744d05ea 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -12,7 +12,6 @@ #include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" #include "fsfw_hal/linux/uart/UartComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" -#include "linux/devices/devicedefinitions/SupvReturnValuesIF.h" /** * @brief This is the device handler for the supervisor of the PLOC which is programmed by From 9d02322cd733aa2e9d21fce3ba90754c860c47f9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 14:26:52 +0100 Subject: [PATCH 108/244] continue refactoring --- .../PlocSupervisorDefinitions.h | 76 ++-- linux/devices/ploc/PlocSupervisorHandler.cpp | 328 +++++++++--------- linux/devices/ploc/PlocSupervisorHandler.h | 19 +- linux/devices/ploc/PlocSupvUartMan.cpp | 242 ++++++------- 4 files changed, 347 insertions(+), 318 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index aa28c143..34ac413e 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -142,20 +142,20 @@ static const uint16_t SIZE_ADC_REPORT = 72; /** * SpacePacket apids of telemetry packets */ -static const uint16_t APID_ACK_SUCCESS = 0x200; -static const uint16_t APID_ACK_FAILURE = 0x201; -static const uint16_t APID_EXE_SUCCESS = 0x202; -static const uint16_t APID_EXE_FAILURE = 0x203; -static const uint16_t APID_HK_REPORT = 0x204; -static const uint16_t APID_BOOT_STATUS_REPORT = 0x205; -static const uint16_t APID_UPDATE_STATUS_REPORT = 0x206; -static const uint16_t APID_ADC_REPORT = 0x207; -static const uint16_t APID_LATCHUP_STATUS_REPORT = 0x208; -static const uint16_t APID_SOC_SYSMON = 0x209; -static const uint16_t APID_MRAM_DUMP_TM = 0x20A; -static const uint16_t APID_SRAM = 0x20B; -static const uint16_t APID_NOR_DATA = 0x20C; -static const uint16_t APID_DATA_LOGGER_DATA = 0x20D; +// static const uint16_t APID_ACK_SUCCESS = 0x200; +// static const uint16_t APID_ACK_FAILURE = 0x201; +// static const uint16_t APID_EXE_SUCCESS = 0x202; +// static const uint16_t APID_EXE_FAILURE = 0x203; +// static const uint16_t APID_HK_REPORT = 0x204; +// static const uint16_t APID_BOOT_STATUS_REPORT = 0x205; +// static const uint16_t APID_UPDATE_STATUS_REPORT = 0x206; +// static const uint16_t APID_ADC_REPORT = 0x207; +// static const uint16_t APID_LATCHUP_STATUS_REPORT = 0x208; +// static const uint16_t APID_SOC_SYSMON = 0x209; +// static const uint16_t APID_MRAM_DUMP_TM = 0x20A; +// static const uint16_t APID_SRAM = 0x20B; +// static const uint16_t APID_NOR_DATA = 0x20C; +// static const uint16_t APID_DATA_LOGGER_DATA = 0x20D; /** * APIDs of telecommand packets @@ -163,7 +163,7 @@ static const uint16_t APID_DATA_LOGGER_DATA = 0x20D; // 2 bits APID SRC, 00 for OBC, 2 bits APID DEST, 01 for SUPV, 7 bits CMD ID -> Mask 0x080 static constexpr uint16_t APID_TC_SUPV_MASK = 0x080; -enum Apids { +enum Apid { TMTC_MAN = 0x00, HK = 0x01, BOOT_MAN = 0x02, @@ -445,6 +445,8 @@ class TcBase : public ploc::SpTcBase { class TmBase : public ploc::SpTmReader { public: + TmBase() = default; + TmBase(const uint8_t* data, size_t maxSize) : ploc::SpTmReader(data, maxSize) { if (maxSize < MIN_TMTC_LEN) { sif::error << "SupvTcBase::SupvTcBase: Passed buffer is too small" << std::endl; @@ -507,7 +509,7 @@ class MPSoCBootSelect : public TcBase { * @note Selection of partitions is currently not supported. */ MPSoCBootSelect(TcParams params) - : TcBase(params, Apids::BOOT_MAN, static_cast(tc::BootManId::SELECT_IMAGE), 4) {} + : TcBase(params, Apid::BOOT_MAN, static_cast(tc::BootManId::SELECT_IMAGE), 4) {} ReturnValue_t buildPacket(uint8_t mem = 0, uint8_t bp0 = 0, uint8_t bp1 = 0, uint8_t bp2 = 0) { auto res = checkSizeAndSerializeHeader(); @@ -534,7 +536,7 @@ class SetTimeRef : public TcBase { public: static constexpr size_t PAYLOAD_LEN = 8; SetTimeRef(TcParams params) - : TcBase(params, Apids::TMTC_MAN, static_cast(tc::TmtcId::TIME_REF), 8) {} + : TcBase(params, Apid::TMTC_MAN, static_cast(tc::TmtcId::TIME_REF), 8) {} ReturnValue_t buildPacket(Clock::TimeOfDay_t* time) { auto res = checkSizeAndSerializeHeader(); @@ -609,7 +611,7 @@ class SetBootTimeout : public TcBase { * @param timeout The boot timeout in milliseconds. */ SetBootTimeout(TcParams params) - : TcBase(params, Apids::BOOT_MAN, static_cast(tc::BootManId::SET_BOOT_TIMEOUT), + : TcBase(params, Apid::BOOT_MAN, static_cast(tc::BootManId::SET_BOOT_TIMEOUT), PAYLOAD_LEN) {} ReturnValue_t buildPacket(uint32_t timeout) { @@ -641,7 +643,7 @@ class SetRestartTries : public TcBase { * @param restartTries Maximum restart tries to set. */ SetRestartTries(TcParams params) - : TcBase(params, Apids::BOOT_MAN, static_cast(tc::BootManId::SET_MAX_REBOOT_TRIES), + : TcBase(params, Apid::BOOT_MAN, static_cast(tc::BootManId::SET_MAX_REBOOT_TRIES), 1) {} ReturnValue_t buildPacket(uint8_t restartTries) { @@ -670,7 +672,7 @@ class DisablePeriodicHkTransmission : public TcBase { * @brief Constructor */ DisablePeriodicHkTransmission(TcParams params) - : TcBase(params, Apids::HK, static_cast(tc::HkId::ENABLE), 1) {} + : TcBase(params, Apid::HK, static_cast(tc::HkId::ENABLE), 1) {} ReturnValue_t buildPacket() { auto res = checkSizeAndSerializeHeader(); @@ -699,7 +701,7 @@ class LatchupAlert : public TcBase { * @param latchupId Identifies the latchup to enable/disable (0 - 0.85V, 1 - 1.8V, 2 - MISC, * 3 - 3.3V, 4 - NVM_4XO, 5 - MISSION, 6 - SAFECOTS) */ - LatchupAlert(TcParams params) : TcBase(params, Apids::LATCHUP_MON) { setLenFromPayloadLen(1); } + LatchupAlert(TcParams params) : TcBase(params, Apid::LATCHUP_MON) { setLenFromPayloadLen(1); } ReturnValue_t buildPacket(bool state, uint8_t latchupId) { if (state) { @@ -731,7 +733,7 @@ class SetAlertlimit : public TcBase { * @param dutycycle */ SetAlertlimit(TcParams params) - : TcBase(params, Apids::LATCHUP_MON, static_cast(tc::LatchupMonId::SET_ALERT_LIMIT), + : TcBase(params, Apid::LATCHUP_MON, static_cast(tc::LatchupMonId::SET_ALERT_LIMIT), 5) {} ReturnValue_t buildPacket(uint8_t latchupId, uint32_t dutycycle) { @@ -771,7 +773,7 @@ class SetAdcWindowAndStride : public TcBase { * @param stridingStepSize */ SetAdcWindowAndStride(TcParams params) - : TcBase(params, Apids::ADC_MON, static_cast(tc::AdcMonId::SET_WINDOW_STRIDE), 4) {} + : TcBase(params, Apid::ADC_MON, static_cast(tc::AdcMonId::SET_WINDOW_STRIDE), 4) {} ReturnValue_t buildPacket(uint16_t windowSize, uint16_t stridingStepSize) { auto res = checkSizeAndSerializeHeader(); @@ -804,7 +806,7 @@ class SetAdcThreshold : public TcBase { * @param threshold */ SetAdcThreshold(TcParams params) - : TcBase(params, Apids::ADC_MON, static_cast(tc::AdcMonId::SET_ADC_THRESHOLD), 4) {} + : TcBase(params, Apid::ADC_MON, static_cast(tc::AdcMonId::SET_ADC_THRESHOLD), 4) {} ReturnValue_t buildPacket(uint32_t threshold) { auto res = checkSizeAndSerializeHeader(); @@ -834,7 +836,7 @@ class RunAutoEmTests : public TcBase { * @param test 1 - complete EM test, 2 - Short test (only memory readback NVM0,1,3) */ RunAutoEmTests(TcParams params) - : TcBase(params, Apids::TMTC_MAN, static_cast(tc::TmtcId::RUN_AUTO_EM_TEST), 1) {} + : TcBase(params, Apid::TMTC_MAN, static_cast(tc::TmtcId::RUN_AUTO_EM_TEST), 1) {} ReturnValue_t buildPacket(uint8_t test) { auto res = checkSizeAndSerializeHeader(); @@ -865,7 +867,7 @@ class SetGpio : public TcBase { * @param val */ SetGpio(TcParams params) - : TcBase(params, Apids::TMTC_MAN, static_cast(tc::TmtcId::SET_GPIO), 3) {} + : TcBase(params, Apid::TMTC_MAN, static_cast(tc::TmtcId::SET_GPIO), 3) {} ReturnValue_t buildPacket(uint8_t port, uint8_t pin, uint8_t val) { auto res = checkSizeAndSerializeHeader(); @@ -901,7 +903,7 @@ class ReadGpio : public TcBase { * @param pin */ ReadGpio(TcParams params) - : TcBase(params, Apids::TMTC_MAN, static_cast(tc::TmtcId::READ_GPIO), 2) {} + : TcBase(params, Apid::TMTC_MAN, static_cast(tc::TmtcId::READ_GPIO), 2) {} ReturnValue_t buildPacket(uint8_t port, uint8_t pin) { auto res = checkSizeAndSerializeHeader(); @@ -925,7 +927,7 @@ class ReadGpio : public TcBase { class SetShutdownTimeout : public TcBase { public: SetShutdownTimeout(TcParams params) - : TcBase(params, Apids::BOOT_MAN, static_cast(tc::BootManId::SHUTDOWN_TIMEOUT), 4) {} + : TcBase(params, Apid::BOOT_MAN, static_cast(tc::BootManId::SHUTDOWN_TIMEOUT), 4) {} ReturnValue_t buildPacket(uint32_t timeout) { auto res = checkSizeAndSerializeHeader(); @@ -957,7 +959,7 @@ class CheckMemory : public TcBase { * @param length Length in bytes of memory region */ CheckMemory(TcParams params) - : TcBase(params, Apids::MEM_MAN, static_cast(tc::MemManId::CHECK), 10) {} + : TcBase(params, Apid::MEM_MAN, static_cast(tc::MemManId::CHECK), 10) {} ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { auto res = checkSizeAndSerializeHeader(); @@ -997,7 +999,7 @@ class WriteMemory : public TcBase { * @param updateData Pointer to buffer containing update data */ WriteMemory(TcParams params) - : TcBase(params, Apids::MEM_MAN, static_cast(tc::MemManId::WRITE), 1) {} + : TcBase(params, Apid::MEM_MAN, static_cast(tc::MemManId::WRITE), 1) {} ReturnValue_t buildPacket(ccsds::SequenceFlags seqFlags, uint16_t sequenceCount, uint8_t memoryId, uint32_t startAddress, uint16_t length, uint8_t* updateData) { @@ -1060,7 +1062,7 @@ class WriteMemory : public TcBase { class EraseMemory : public TcBase { public: EraseMemory(TcParams params) - : TcBase(params, Apids::MEM_MAN, static_cast(tc::MemManId::ERASE), PAYLOAD_LENGTH) {} + : TcBase(params, Apid::MEM_MAN, static_cast(tc::MemManId::ERASE), PAYLOAD_LENGTH) {} ReturnValue_t buildPacket(uint8_t memoryId, uint32_t startAddress, uint32_t length) { auto res = checkSizeAndSerializeHeader(); @@ -1100,7 +1102,7 @@ class VerificationReport { if (not readerBase.crcIsOk()) { return result::CRC_FAILURE; } - if (readerBase.getApid() != Apids::TMTC_MAN) { + if (readerBase.getApid() != Apid::TMTC_MAN) { return result::INVALID_APID; } if (readerBase.getBufSize() < MIN_PAYLOAD_LEN + 8) { @@ -1823,7 +1825,7 @@ class FactoryReset : public TcBase { * * @param op */ - FactoryReset(TcParams params) : TcBase(params, Apids::TMTC_MAN, 0x11, 1) {} + FactoryReset(TcParams params) : TcBase(params, Apid::TMTC_MAN, 0x11, 1) {} ReturnValue_t buildPacket(Op op) { auto res = checkSizeAndSerializeHeader(); @@ -1867,7 +1869,7 @@ class EnableNvms : public TcBase { * @param bp1 Partition pin 1 * @param bp2 Partition pin 2 */ - EnableNvms(TcParams params) : TcBase(params, Apids::TMTC_MAN, 0x06, 2) {} + EnableNvms(TcParams params) : TcBase(params, Apid::TMTC_MAN, 0x06, 2) {} ReturnValue_t buildPacket(uint8_t nvm01, uint8_t nvm3) { auto res = checkSizeAndSerializeHeader(); @@ -1901,7 +1903,7 @@ class MramCmd : public TcBase { * * @note The content at the stop address is excluded from the dump or wipe operation. */ - MramCmd(TcParams params) : TcBase(params, Apids::DATA_LOGGER) { setLenFromPayloadLen(6); } + MramCmd(TcParams params) : TcBase(params, Apid::DATA_LOGGER) { setLenFromPayloadLen(6); } ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { if (action == MramAction::WIPE) { @@ -1996,8 +1998,8 @@ class SetAdcEnabledChannels : public TcBase { * @param ch Defines channels to be enabled or disabled. */ SetAdcEnabledChannels(TcParams params) - : TcBase(params, Apids::ADC_MON, static_cast(tc::AdcMonId::SET_ENABLED_CHANNELS), - 2) {} + : TcBase(params, Apid::ADC_MON, static_cast(tc::AdcMonId::SET_ENABLED_CHANNELS), 2) { + } ReturnValue_t buildPacket(uint16_t ch) { auto res = checkSizeAndSerializeHeader(); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 6ec92f98..9f9ac9ea 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -223,17 +223,17 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d spParams.buf = commandBuffer; switch (deviceCommand) { case GET_HK_REPORT: { - prepareEmptyCmd(Apids::HK, static_cast(tc::HkId::GET_REPORT)); + prepareEmptyCmd(Apid::HK, static_cast(tc::HkId::GET_REPORT)); result = returnvalue::OK; break; } case START_MPSOC: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::START_MPSOC)); + prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::START_MPSOC)); result = returnvalue::OK; break; } case SHUTDOWN_MPSOC: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::SHUTDOWN_MPSOC)); + prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::SHUTDOWN_MPSOC)); result = returnvalue::OK; break; } @@ -243,7 +243,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case RESET_MPSOC: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::RESET_MPSOC)); + prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::RESET_MPSOC)); result = returnvalue::OK; break; } @@ -267,7 +267,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case GET_BOOT_STATUS_REPORT: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::GET_BOOT_STATUS_REPORT)); + prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::GET_BOOT_STATUS_REPORT)); result = returnvalue::OK; break; } @@ -299,8 +299,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // break; // } case GET_LATCHUP_STATUS_REPORT: { - prepareEmptyCmd(Apids::LATCHUP_MON, - static_cast(tc::LatchupMonId::GET_STATUS_REPORT)); + prepareEmptyCmd(Apid::LATCHUP_MON, static_cast(tc::LatchupMonId::GET_STATUS_REPORT)); result = returnvalue::OK; break; } @@ -381,7 +380,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d break; } case FACTORY_FLASH: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::FACTORY_FLASH)); + prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::FACTORY_FLASH)); result = returnvalue::OK; break; } @@ -435,7 +434,7 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // break; // } case RESET_PL: { - prepareEmptyCmd(Apids::BOOT_MAN, static_cast(tc::BootManId::RESET_PL)); + prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::RESET_PL)); result = returnvalue::OK; break; } @@ -654,73 +653,66 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { using namespace supv; - if (nextReplyId == FIRST_MRAM_DUMP) { - *foundId = FIRST_MRAM_DUMP; - return parseMramPackets(start, remainingSize, foundLen); - } else if (nextReplyId == CONSECUTIVE_MRAM_DUMP) { - *foundId = CONSECUTIVE_MRAM_DUMP; - return parseMramPackets(start, remainingSize, foundLen); - } + // TODO: Is this still required? + // if (nextReplyId == FIRST_MRAM_DUMP) { + // *foundId = FIRST_MRAM_DUMP; + // return parseMramPackets(start, remainingSize, foundLen); + // } else if (nextReplyId == CONSECUTIVE_MRAM_DUMP) { + // *foundId = CONSECUTIVE_MRAM_DUMP; + // return parseMramPackets(start, remainingSize, foundLen); + // } - ReturnValue_t result = returnvalue::OK; - - uint16_t apid = (*(start) << 8 | *(start + 1)) & APID_MASK; + tmReader.setData(start, remainingSize); + uint16_t apid = tmReader.getApid(); //(*(start) << 8 | *(start + 1)) & APID_MASK; switch (apid) { - case (APID_ACK_SUCCESS): - *foundLen = SIZE_ACK_REPORT; - *foundId = ACK_REPORT; + case (Apid::TMTC_MAN): { + switch (tmReader.getServiceId()) { + case (static_cast(supv::tm::TmtcId::ACK)): + case (static_cast(supv::tm::TmtcId::NAK)): { + *foundLen = SIZE_ACK_REPORT; + *foundId = ACK_REPORT; + return OK; + } + case (static_cast(supv::tm::TmtcId::EXEC_ACK)): + case (static_cast(supv::tm::TmtcId::EXEC_NAK)): { + *foundLen = SIZE_EXE_REPORT; + *foundId = EXE_REPORT; + return OK; + } + } break; - case (APID_ACK_FAILURE): - *foundLen = SIZE_ACK_REPORT; - *foundId = ACK_REPORT; + } + case (Apid::HK): { + if (tmReader.getServiceId() == static_cast(supv::tm::HkId::REPORT)) { + *foundLen = SIZE_HK_REPORT; + *foundId = HK_REPORT; + return OK; + } else if (tmReader.getServiceId() == static_cast(supv::tm::HkId::HARDFAULTS)) { + handleBadApidServiceCombination(SUPV_UNINIMPLEMENTED_TM, apid, tmReader.getServiceId()); + return INVALID_DATA; + } break; - case (APID_HK_REPORT): - *foundLen = SIZE_HK_REPORT; - *foundId = HK_REPORT; + } + case (Apid::BOOT_MAN): { + if (tmReader.getServiceId() == + static_cast(supv::tm::BootManId::BOOT_STATUS_REPORT)) { + *foundLen = SIZE_BOOT_STATUS_REPORT; + *foundId = BOOT_STATUS_REPORT; + return OK; + } break; - case (APID_BOOT_STATUS_REPORT): - *foundLen = SIZE_BOOT_STATUS_REPORT; - *foundId = BOOT_STATUS_REPORT; - break; - case (APID_LATCHUP_STATUS_REPORT): - *foundLen = SIZE_LATCHUP_STATUS_REPORT; - *foundId = LATCHUP_REPORT; - break; - case (APID_DATA_LOGGER_DATA): - *foundLen = SIZE_LOGGING_REPORT; - *foundId = LOGGING_REPORT; - break; - case (APID_ADC_REPORT): - *foundLen = SIZE_ADC_REPORT; - *foundId = ADC_REPORT; - break; - case (APID_EXE_SUCCESS): - *foundLen = SIZE_EXE_REPORT; - *foundId = EXE_REPORT; - break; - case (APID_EXE_FAILURE): - *foundLen = SIZE_EXE_REPORT; - *foundId = EXE_REPORT; - break; - default: { - sif::debug << "PlocSupervisorHandler::scanForReply: Reply has invalid apid" << std::endl; - *foundLen = remainingSize; - return result::INVALID_APID; + } + case (Apid::MEM_MAN): { + if (tmReader.getServiceId() == + static_cast(supv::tm::MemManId::UPDATE_STATUS_REPORT)) { + // TODO: I think this will be handled by the uart manager + } } } - - return result; -} - -ReturnValue_t PlocSupervisorHandler::getSwitches(const uint8_t** switches, - uint8_t* numberOfSwitches) { - if (powerSwitch == power::NO_SWITCH) { - return DeviceHandlerBase::NO_SWITCH; - } - *numberOfSwitches = 1; - *switches = &powerSwitch; - return returnvalue::OK; + handleBadApidServiceCombination(SUPV_UNKNOWN_TM, apid, tmReader.getServiceId()); + *foundLen = remainingSize; + return INVALID_DATA; } ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id, @@ -771,6 +763,16 @@ ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id, return result; } +ReturnValue_t PlocSupervisorHandler::getSwitches(const uint8_t** switches, + uint8_t* numberOfSwitches) { + if (powerSwitch == power::NO_SWITCH) { + return DeviceHandlerBase::NO_SWITCH; + } + *numberOfSwitches = 1; + *switches = &powerSwitch; + return returnvalue::OK; +} + void PlocSupervisorHandler::setNormalDatapoolEntriesInvalid() {} uint32_t PlocSupervisorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { @@ -932,56 +934,57 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; // TODO: Fix - // AcknowledgmentReport ack(data, SIZE_ACK_REPORT); - // result = ack.checkSize(); - // if (result != returnvalue::OK) { - // return result; - // } + + // AcknowledgmentReport ack(data, SIZE_ACK_REPORT); + // result = ack.checkSize(); + // if (result != returnvalue::OK) { + // return result; + // } // - // result = ack.checkCrc(); - // if (result != returnvalue::OK) { - // sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; - // nextReplyId = supv::NONE; - // replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); - // triggerEvent(SUPV_CRC_FAILURE_EVENT); - // sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::CRC_FAILURE); - // disableAllReplies(); - // return returnvalue::OK; - // } + // result = ack.checkCrc(); + // if (result != returnvalue::OK) { + // sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; + // nextReplyId = supv::NONE; + // replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); + // triggerEvent(SUPV_CRC_FAILURE_EVENT); + // sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::CRC_FAILURE); + // disableAllReplies(); + // return returnvalue::OK; + // } // - // result = ack.checkApid(); + // result = ack.checkApid(); // - // switch (result) { - // case SupvReturnValuesIF::RECEIVED_ACK_FAILURE: { - // DeviceCommandId_t commandId = getPendingCommand(); - // if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { - // triggerEvent(SUPV_ACK_FAILURE, commandId, static_cast(ack.getStatusCode())); - // } - // printAckFailureInfo(ack.getStatusCode(), commandId); - // sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::RECEIVED_ACK_FAILURE); - // disableAllReplies(); - // nextReplyId = supv::NONE; - // result = IGNORE_REPLY_DATA; - // break; - // } - // case returnvalue::OK: { - // setNextReplyId(); - // break; - // } - // case SupvReturnValuesIF::INVALID_APID: - // sif::warning << "PlocSupervisorHandler::handleAckReport: Invalid APID in Ack report" - // << std::endl; - // sendFailureReport(supv::ACK_REPORT, result); - // disableAllReplies(); - // nextReplyId = supv::NONE; - // result = IGNORE_REPLY_DATA; - // break; - // default: { - // sif::error << "PlocSupervisorHandler::handleAckReport: APID parsing failed" << std::endl; - // result = returnvalue::FAILED; - // break; - // } - // } + // switch (result) { + // case SupvReturnValuesIF::RECEIVED_ACK_FAILURE: { + // DeviceCommandId_t commandId = getPendingCommand(); + // if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { + // triggerEvent(SUPV_ACK_FAILURE, commandId, static_cast(ack.getStatusCode())); + // } + // printAckFailureInfo(ack.getStatusCode(), commandId); + // sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::RECEIVED_ACK_FAILURE); + // disableAllReplies(); + // nextReplyId = supv::NONE; + // result = IGNORE_REPLY_DATA; + // break; + // } + // case returnvalue::OK: { + // setNextReplyId(); + // break; + // } + // case SupvReturnValuesIF::INVALID_APID: + // sif::warning << "PlocSupervisorHandler::handleAckReport: Invalid APID in Ack report" + // << std::endl; + // sendFailureReport(supv::ACK_REPORT, result); + // disableAllReplies(); + // nextReplyId = supv::NONE; + // result = IGNORE_REPLY_DATA; + // break; + // default: { + // sif::error << "PlocSupervisorHandler::handleAckReport: APID parsing failed" << std::endl; + // result = returnvalue::FAILED; + // break; + // } + // } return result; } @@ -1806,39 +1809,40 @@ void PlocSupervisorHandler::disableExeReportReply() { info->command->second.expectedReplies = 1; } -ReturnValue_t PlocSupervisorHandler::parseMramPackets(const uint8_t* packet, size_t remainingSize, - size_t* foundLen) { - ReturnValue_t result = IGNORE_FULL_PACKET; - uint16_t packetLen = 0; - *foundLen = 0; - - for (size_t idx = 0; idx < remainingSize; idx++) { - std::memcpy(spacePacketBuffer + bufferTop, packet + idx, 1); - bufferTop += 1; - *foundLen += 1; - if (bufferTop >= ccsds::HEADER_LEN) { - packetLen = readSpacePacketLength(spacePacketBuffer); - } - - if (bufferTop == ccsds::HEADER_LEN + packetLen + 1) { - packetInBuffer = true; - bufferTop = 0; - return checkMramPacketApid(); - } - - if (bufferTop == supv::MAX_PACKET_SIZE) { - *foundLen = remainingSize; - disableAllReplies(); - bufferTop = 0; - sif::info << "PlocSupervisorHandler::parseMramPackets: Can not find MRAM packet in space " - "packet buffer" - << std::endl; - return result::MRAM_PACKET_PARSING_FAILURE; - } - } - - return result; -} +// ReturnValue_t PlocSupervisorHandler::parseMramPackets(const uint8_t* packet, size_t +// remainingSize, +// size_t* foundLen) { +// ReturnValue_t result = IGNORE_FULL_PACKET; +// uint16_t packetLen = 0; +// *foundLen = 0; +// +// for (size_t idx = 0; idx < remainingSize; idx++) { +// std::memcpy(spacePacketBuffer + bufferTop, packet + idx, 1); +// bufferTop += 1; +// *foundLen += 1; +// if (bufferTop >= ccsds::HEADER_LEN) { +// packetLen = readSpacePacketLength(spacePacketBuffer); +// } +// +// if (bufferTop == ccsds::HEADER_LEN + packetLen + 1) { +// packetInBuffer = true; +// bufferTop = 0; +// return checkMramPacketApid(); +// } +// +// if (bufferTop == supv::MAX_PACKET_SIZE) { +// *foundLen = remainingSize; +// disableAllReplies(); +// bufferTop = 0; +// sif::info << "PlocSupervisorHandler::parseMramPackets: Can not find MRAM packet in space " +// "packet buffer" +// << std::endl; +// return result::MRAM_PACKET_PARSING_FAILURE; +// } +// } +// +// return result; +// } ReturnValue_t PlocSupervisorHandler::handleMramDumpPacket(DeviceCommandId_t id) { ReturnValue_t result = returnvalue::FAILED; @@ -1916,13 +1920,14 @@ void PlocSupervisorHandler::increaseExpectedMramReplies(DeviceCommandId_t id) { return; } -ReturnValue_t PlocSupervisorHandler::checkMramPacketApid() { - uint16_t apid = (spacePacketBuffer[0] << 8 | spacePacketBuffer[1]) & supv::APID_MASK; - if (apid != supv::APID_MRAM_DUMP_TM) { - return result::NO_MRAM_PACKET; - } - return APERIODIC_REPLY; -} +// ReturnValue_t PlocSupervisorHandler::checkMramPacketApid() { +// uint16_t apid = (spacePacketBuffer[0] << 8 | spacePacketBuffer[1]) & supv::APID_MASK; +// TODO: Fix +// if (apid != supv::APID_MRAM_DUMP_TM) { +// return result::NO_MRAM_PACKET; +// } +// return APERIODIC_REPLY; +//} ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) { #ifdef XIPHOS_Q7S @@ -2164,6 +2169,19 @@ void PlocSupervisorHandler::handleExecutionFailureReport(uint16_t statusCode) { disableExeReportReply(); } +void PlocSupervisorHandler::handleBadApidServiceCombination(Event event, unsigned int apid, + unsigned int serviceId) { + const char* printString = ""; + if (event == SUPV_UNKNOWN_TM) { + printString = "Unknown"; + } else if (event == SUPV_UNINIMPLEMENTED_TM) { + printString = "Unimplemented"; + } + triggerEvent(event, apid, tmReader.getServiceId()); + sif::error << printString << " APID service combination 0x" << std::setw(2) << std::setfill('0') + << std::hex << apid << ", " << std::setw(2) << serviceId << std::endl; +} + void PlocSupervisorHandler::printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId) { sif::warning << "PlocSupervisorHandler: Received Ack failure report with status code: 0x" << std::hex << statusCode << std::endl; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 744d05ea..4221bf4d 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -64,18 +64,21 @@ class PlocSupervisorHandler : public DeviceHandlerBase { //! [EXPORT] : [COMMENT] PLOC supervisor crc failure in telemetry packet static const Event SUPV_MEMORY_READ_RPT_CRC_FAILURE = MAKE_EVENT(1, severity::LOW); + //! [EXPORT] : [COMMENT] Unhandled event. P1: APID, P2: Service ID + static constexpr Event SUPV_UNKNOWN_TM = MAKE_EVENT(2, severity::LOW); + static constexpr Event SUPV_UNINIMPLEMENTED_TM = MAKE_EVENT(3, severity::LOW); //! [EXPORT] : [COMMENT] PLOC supervisor received acknowledgment failure report - static const Event SUPV_ACK_FAILURE = MAKE_EVENT(2, severity::LOW); + static const Event SUPV_ACK_FAILURE = MAKE_EVENT(4, severity::LOW); //! [EXPORT] : [COMMENT] PLOC received execution failure report //! P1: ID of command for which the execution failed //! P2: Status code sent by the supervisor handler - static const Event SUPV_EXE_FAILURE = MAKE_EVENT(3, severity::LOW); + static const Event SUPV_EXE_FAILURE = MAKE_EVENT(5, severity::LOW); //! [EXPORT] : [COMMENT] PLOC supervisor reply has invalid crc - static const Event SUPV_CRC_FAILURE_EVENT = MAKE_EVENT(4, severity::LOW); + static const Event SUPV_CRC_FAILURE_EVENT = MAKE_EVENT(6, severity::LOW); //! [EXPORT] : [COMMENT] Supervisor helper currently executing a command - static const Event SUPV_HELPER_EXECUTING = MAKE_EVENT(5, severity::LOW); + static const Event SUPV_HELPER_EXECUTING = MAKE_EVENT(7, severity::LOW); //! [EXPORT] : [COMMENT] Failed to build the command to shutdown the MPSoC - static const Event SUPV_MPSOC_SHUWDOWN_BUILD_FAILED = MAKE_EVENT(5, severity::LOW); + static const Event SUPV_MPSOC_SHUWDOWN_BUILD_FAILED = MAKE_EVENT(8, severity::LOW); static const uint16_t APID_MASK = 0x7FF; static const uint16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF; @@ -121,6 +124,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { supv::AdcReport adcReport; const power::Switch_t powerSwitch = power::NO_SWITCH; + supv::TmBase tmReader; PlocSupvHelper* supvHelper = nullptr; MessageQueueIF* eventQueue = nullptr; @@ -210,6 +214,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t handleBootStatusReport(const uint8_t* data); ReturnValue_t handleLatchupStatusReport(const uint8_t* data); + void handleBadApidServiceCombination(Event result, unsigned int apid, unsigned int serviceId); // ReturnValue_t handleLoggingReport(const uint8_t* data); ReturnValue_t handleAdcReport(const uint8_t* data); @@ -317,7 +322,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { * @brief Function is called in scanForReply and fills the spacePacketBuffer with the read * data until a full packet has been received. */ - ReturnValue_t parseMramPackets(const uint8_t* packet, size_t remainingSize, size_t* foundlen); + // ReturnValue_t parseMramPackets(const uint8_t* packet, size_t remainingSize, size_t* foundlen); /** * @brief This function generates the Service 8 packets for the MRAM dump data. @@ -335,7 +340,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { * @brief Function checks if the packet written to the space packet buffer is really a * MRAM dump packet. */ - ReturnValue_t checkMramPacketApid(); + // ReturnValue_t checkMramPacketApid(); /** * @brief Writes the data of the MRAM dump to a file. The file will be created when receiving diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 53488ce2..997c6a7a 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -500,7 +500,7 @@ ReturnValue_t PlocSupvHelper::selectMemory() { ReturnValue_t PlocSupvHelper::prepareUpdate() { ReturnValue_t result = returnvalue::OK; resetSpParams(); - supv::NoPayloadPacket packet(spParams, Apids::BOOT_MAN, + supv::NoPayloadPacket packet(spParams, Apid::BOOT_MAN, static_cast(tc::BootManId::PREPARE_UPDATE)); result = packet.buildPacket(); if (result != returnvalue::OK) { @@ -584,7 +584,7 @@ ReturnValue_t PlocSupvHelper::handleAck() { // } // return result; // } - return returnvalue::OK; + return result; } ReturnValue_t PlocSupvHelper::handleExe(uint32_t timeout) { @@ -735,74 +735,76 @@ ReturnValue_t PlocSupvHelper::calcImageCrc() { ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { ReturnValue_t result = returnvalue::OK; - resetSpParams(); - // Will hold status report for later processing - std::array statusReportBuf{}; - supv::UpdateStatusReport updateStatusReport(tmBuf.data(), tmBuf.size()); - // Verification of update write procedure - supv::CheckMemory packet(spParams); - result = packet.buildPacket(update.memoryId, update.startAddress, update.fullFileSize); - if (result != returnvalue::OK) { - return result; - } - result = sendCommand(packet); - if (result != returnvalue::OK) { - return result; - } - result = handleAck(); - if (result != returnvalue::OK) { - return result; - } - - bool exeAlreadyHandled = false; - uint32_t timeout = std::max(CRC_EXECUTION_TIMEOUT, supv::timeout::UPDATE_STATUS_REPORT); - result = handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), timeout); - ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size()); - if (spReader.getApid() == supv::APID_EXE_FAILURE) { - exeAlreadyHandled = true; - result = handleRemainingExeReport(spReader); - } else if (spReader.getApid() == supv::APID_UPDATE_STATUS_REPORT) { - size_t remBytes = spReader.getPacketDataLen() + 1; - result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN, - supv::timeout::UPDATE_STATUS_REPORT); - if (result != returnvalue::OK) { - sif::warning - << "PlocSupvHelper::handleCheckMemoryCommand: Failed to receive update status report" - << std::endl; - return result; - } - result = updateStatusReport.checkCrc(); - if (result != returnvalue::OK) { - sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC check failed" << std::endl; - return result; - } - // Copy into other buffer because data will be overwritten when reading execution report - std::memcpy(statusReportBuf.data(), tmBuf.data(), updateStatusReport.getNominalSize()); - } - - if (not exeAlreadyHandled) { - result = handleExe(CRC_EXECUTION_TIMEOUT); - if (result != returnvalue::OK) { - return result; - } - } - - // Now process the status report - updateStatusReport.setData(statusReportBuf.data(), statusReportBuf.size()); - result = updateStatusReport.parseDataField(); - if (result != returnvalue::OK) { - return result; - } - if (update.crcShouldBeChecked) { - result = updateStatusReport.verifycrc(update.crc); - if (result != returnvalue::OK) { - sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC failure. Expected CRC 0x" - << std::setfill('0') << std::hex << std::setw(4) - << static_cast(update.crc) << " but received CRC 0x" << std::setw(4) - << updateStatusReport.getCrc() << std::dec << std::endl; - return result; - } - } + // TODO: Fix + // resetSpParams(); + // // Will hold status report for later processing + // std::array statusReportBuf{}; + // supv::UpdateStatusReport updateStatusReport(tmBuf.data(), tmBuf.size()); + // // Verification of update write procedure + // supv::CheckMemory packet(spParams); + // result = packet.buildPacket(update.memoryId, update.startAddress, update.fullFileSize); + // if (result != returnvalue::OK) { + // return result; + // } + // result = sendCommand(packet); + // if (result != returnvalue::OK) { + // return result; + // } + // result = handleAck(); + // if (result != returnvalue::OK) { + // return result; + // } + // + // bool exeAlreadyHandled = false; + // uint32_t timeout = std::max(CRC_EXECUTION_TIMEOUT, supv::timeout::UPDATE_STATUS_REPORT); + // result = handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), timeout); + // ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size()); + // if (spReader.getApid() == supv::APID_EXE_FAILURE) { + // exeAlreadyHandled = true; + // result = handleRemainingExeReport(spReader); + // } else if (spReader.getApid() == supv::APID_UPDATE_STATUS_REPORT) { + // size_t remBytes = spReader.getPacketDataLen() + 1; + // result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN, + // supv::timeout::UPDATE_STATUS_REPORT); + // if (result != returnvalue::OK) { + // sif::warning + // << "PlocSupvHelper::handleCheckMemoryCommand: Failed to receive update status report" + // << std::endl; + // return result; + // } + // result = updateStatusReport.checkCrc(); + // if (result != returnvalue::OK) { + // sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC check failed" << std::endl; + // return result; + // } + // // Copy into other buffer because data will be overwritten when reading execution report + // std::memcpy(statusReportBuf.data(), tmBuf.data(), updateStatusReport.getNominalSize()); + // } + // + // if (not exeAlreadyHandled) { + // result = handleExe(CRC_EXECUTION_TIMEOUT); + // if (result != returnvalue::OK) { + // return result; + // } + // } + // + // // Now process the status report + // updateStatusReport.setData(statusReportBuf.data(), statusReportBuf.size()); + // result = updateStatusReport.parseDataField(); + // if (result != returnvalue::OK) { + // return result; + // } + // if (update.crcShouldBeChecked) { + // result = updateStatusReport.verifycrc(update.crc); + // if (result != returnvalue::OK) { + // sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC failure. Expected CRC 0x" + // << std::setfill('0') << std::hex << std::setw(4) + // << static_cast(update.crc) << " but received CRC 0x" << + // std::setw(4) + // << updateStatusReport.getCrc() << std::dec << std::endl; + // return result; + // } + // } return result; } @@ -816,55 +818,57 @@ uint32_t PlocSupvHelper::getFileSize(std::string filename) { ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reader) { ReturnValue_t result = returnvalue::OK; -#ifdef XIPHOS_Q7S - if (not sdcMan->getActiveSdCard()) { - return HasFileSystemIF::FILESYSTEM_INACTIVE; - } -#endif - std::string filename = Filenaming::generateAbsoluteFilename( - eventBufferReq.path, eventBufferReq.filename, timestamping); - std::ofstream file(filename, std::ios_base::app | std::ios_base::out); - uint32_t packetsRead = 0; - size_t requestLen = 0; - bool firstPacket = true; - for (packetsRead = 0; packetsRead < NUM_EVENT_BUFFER_PACKETS; packetsRead++) { - if (terminate) { - triggerEvent(SUPV_EVENT_BUFFER_REQUEST_TERMINATED, packetsRead - 1); - file.close(); - return PROCESS_TERMINATED; - } - if (packetsRead == NUM_EVENT_BUFFER_PACKETS - 1) { - requestLen = SIZE_EVENT_BUFFER_LAST_PACKET; - } else { - requestLen = SIZE_EVENT_BUFFER_FULL_PACKET; - } - if (firstPacket) { - firstPacket = false; - requestLen -= 6; - } - result = handleTmReception(requestLen); - if (result != returnvalue::OK) { - sif::debug << "PlocSupvHelper::handleEventBufferReception: Failed while trying to read packet" - << " " << packetsRead + 1 << std::endl; - file.close(); - return result; - } - ReturnValue_t result = reader.checkCrc(); - if (result != returnvalue::OK) { - triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid); - return result; - } - uint16_t apid = reader.getApid(); - if (apid != supv::APID_MRAM_DUMP_TM) { - sif::warning << "PlocSupvHelper::handleEventBufferReception: Did not expect space packet " - << "with APID 0x" << std::hex << apid << std::endl; - file.close(); - return EVENT_BUFFER_REPLY_INVALID_APID; - } - // TODO: Fix - // file.write(reinterpret_cast(reader.getPacketData()), - // reader.getPayloadDataLength()); - } + // TODO: Fix + //#ifdef XIPHOS_Q7S + // if (not sdcMan->getActiveSdCard()) { + // return HasFileSystemIF::FILESYSTEM_INACTIVE; + // } + //#endif + // std::string filename = Filenaming::generateAbsoluteFilename( + // eventBufferReq.path, eventBufferReq.filename, timestamping); + // std::ofstream file(filename, std::ios_base::app | std::ios_base::out); + // uint32_t packetsRead = 0; + // size_t requestLen = 0; + // bool firstPacket = true; + // for (packetsRead = 0; packetsRead < NUM_EVENT_BUFFER_PACKETS; packetsRead++) { + // if (terminate) { + // triggerEvent(SUPV_EVENT_BUFFER_REQUEST_TERMINATED, packetsRead - 1); + // file.close(); + // return PROCESS_TERMINATED; + // } + // if (packetsRead == NUM_EVENT_BUFFER_PACKETS - 1) { + // requestLen = SIZE_EVENT_BUFFER_LAST_PACKET; + // } else { + // requestLen = SIZE_EVENT_BUFFER_FULL_PACKET; + // } + // if (firstPacket) { + // firstPacket = false; + // requestLen -= 6; + // } + // result = handleTmReception(requestLen); + // if (result != returnvalue::OK) { + // sif::debug << "PlocSupvHelper::handleEventBufferReception: Failed while trying to read + // packet" + // << " " << packetsRead + 1 << std::endl; + // file.close(); + // return result; + // } + // ReturnValue_t result = reader.checkCrc(); + // if (result != returnvalue::OK) { + // triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid); + // return result; + // } + // uint16_t apid = reader.getApid(); + // if (apid != supv::APID_MRAM_DUMP_TM) { + // sif::warning << "PlocSupvHelper::handleEventBufferReception: Did not expect space packet " + // << "with APID 0x" << std::hex << apid << std::endl; + // file.close(); + // return EVENT_BUFFER_REPLY_INVALID_APID; + // } + // // TODO: Fix + // // file.write(reinterpret_cast(reader.getPacketData()), + // // reader.getPayloadDataLength()); + // } return result; } From 763bf2d85bc9524c13cc0ef0a5fbc322b9ca6ec6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 14:29:41 +0100 Subject: [PATCH 109/244] remove flush call, belongs in lower layer --- linux/devices/ploc/PlocSupervisorHandler.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 9f9ac9ea..df0796cc 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -448,14 +448,6 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d result = DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; break; } - - if (result == returnvalue::OK) { - /** - * Flushing the receive buffer to make sure there are no data left from a faulty reply. - */ - uartComIf->flushUartRxBuffer(comCookie); - } - return result; } @@ -706,7 +698,10 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r case (Apid::MEM_MAN): { if (tmReader.getServiceId() == static_cast(supv::tm::MemManId::UPDATE_STATUS_REPORT)) { - // TODO: I think this will be handled by the uart manager + // TODO: I think this will be handled by the uart manager? + // Actually, this is a bit tricky. Maybe the lower level will have two separate ring + // buffers, one for internally handled packets and one for packets which are handled + // here? } } } From f564fb5c1af297d40077f0341412cb065d886dfe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 14:52:07 +0100 Subject: [PATCH 110/244] some fixes for thread shutdown code --- linux/devices/ploc/PlocSupervisorHandler.cpp | 28 ++-- linux/devices/ploc/PlocSupvUartMan.cpp | 137 +++++++++++-------- linux/devices/ploc/PlocSupvUartMan.h | 13 +- 3 files changed, 102 insertions(+), 76 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index df0796cc..f71cafe3 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -64,7 +64,7 @@ ReturnValue_t PlocSupervisorHandler::initialize() { if (result != returnvalue::OK) { return ObjectManagerIF::CHILD_INIT_FAILED; } - supvHelper->setComCookie(comCookie); + // supvHelper->setComCookie(comCookie); result = eventSubscription(); if (result != returnvalue::OK) { @@ -97,7 +97,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, switch (actionId) { case TERMINATE_SUPV_HELPER: { - supvHelper->stopProcess(); + supvHelper->stop(); return EXECUTION_FINISHED; } default: @@ -148,18 +148,18 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } - case LOGGING_REQUEST_EVENT_BUFFERS: { - if (size > config::MAX_PATH_SIZE) { - return result::FILENAME_TOO_LONG; - } - result = supvHelper->startEventBufferRequest( - std::string(reinterpret_cast(data), size)); - if (result != returnvalue::OK) { - return result; - } - plocSupvHelperExecuting = true; - return EXECUTION_FINISHED; - } + // case LOGGING_REQUEST_EVENT_BUFFERS: { + // if (size > config::MAX_PATH_SIZE) { + // return result::FILENAME_TOO_LONG; + // } + // result = supvHelper->startEventBufferRequest( + // std::string(reinterpret_cast(data), size)); + // if (result != returnvalue::OK) { + // return result; + // } + // plocSupvHelperExecuting = true; + // return EXECUTION_FINISHED; + // } default: break; } diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 997c6a7a..c9e9ad72 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -167,7 +167,7 @@ ReturnValue_t PlocSupvHelper::setComIF(UartComIF* uartComIF_) { return returnvalue::OK; } -void PlocSupvHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } +// void PlocSupvHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } ReturnValue_t PlocSupvHelper::startUpdate(std::string file, uint8_t memoryId, uint32_t startAddress) { @@ -182,6 +182,12 @@ ReturnValue_t PlocSupvHelper::startUpdate(std::string file, uint8_t memoryId, } ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { + lock->lockMutex(); + InternalState current = state; + lock->unlockMutex(); + if (current != InternalState::DEFAULT) { + return HasActionsIF::IS_BUSY; + } ReturnValue_t result = returnvalue::OK; #ifdef XIPHOS_Q7S result = FilesystemHelper::checkPath(params.file); @@ -204,70 +210,93 @@ ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { return returnvalue::FAILED; } #endif - update.file = params.file; - update.fullFileSize = getFileSize(update.file); - if (params.bytesWritten > update.fullFileSize) { - sif::warning << "Invalid start bytes counter " << params.bytesWritten - << ", smaller than full file length" << update.fullFileSize << std::endl; - return returnvalue::FAILED; + { + MutexGuard mg(lock); + + update.file = params.file; + update.fullFileSize = getFileSize(update.file); + if (params.bytesWritten > update.fullFileSize) { + sif::warning << "Invalid start bytes counter " << params.bytesWritten + << ", smaller than full file length" << update.fullFileSize << std::endl; + return returnvalue::FAILED; + } + update.length = update.fullFileSize - params.bytesWritten; + update.memoryId = params.memId; + update.startAddress = params.startAddr; + update.progressPercent = 0; + update.bytesWritten = params.bytesWritten; + update.crcShouldBeChecked = true; + update.packetNum = 1; + update.deleteMemory = params.deleteMemory; + update.sequenceCount = params.seqCount; + state = InternalState::LONGER_REQUEST; + request = Request::UPDATE; } - update.length = update.fullFileSize - params.bytesWritten; - update.memoryId = params.memId; - update.startAddress = params.startAddr; - update.progressPercent = 0; - update.bytesWritten = params.bytesWritten; - update.crcShouldBeChecked = true; - update.packetNum = 1; - update.deleteMemory = params.deleteMemory; - update.sequenceCount = params.seqCount; - request = Request::UPDATE; - uartComIF->flushUartTxAndRxBuf(comCookie); - semaphore->release(); return result; } ReturnValue_t PlocSupvHelper::performMemCheck(std::string file, uint8_t memoryId, uint32_t startAddress) { - update.file = file; - update.fullFileSize = getFileSize(file); - return performMemCheck(memoryId, startAddress, getFileSize(update.file), true); + lock->lockMutex(); + InternalState current = state; + lock->unlockMutex(); + if (current != InternalState::DEFAULT) { + return HasActionsIF::IS_BUSY; + } + return performMemCheck(file, memoryId, startAddress, getFileSize(update.file), true); } -ReturnValue_t PlocSupvHelper::performMemCheck(uint8_t memoryId, uint32_t startAddress, - size_t sizeToCheck, bool checkCrc) { - update.memoryId = memoryId; - update.startAddress = startAddress; - update.length = sizeToCheck; - update.crcShouldBeChecked = checkCrc; - request = Request::CHECK_MEMORY; - uartComIF->flushUartTxAndRxBuf(comCookie); - semaphore->release(); +ReturnValue_t PlocSupvHelper::performMemCheck(std::string file, uint8_t memoryId, + uint32_t startAddress, size_t sizeToCheck, + bool checkCrc) { + { + MutexGuard mg(lock); + update.file = file; + update.fullFileSize = getFileSize(file); + state = InternalState::LONGER_REQUEST; + request = Request::CHECK_MEMORY; + update.memoryId = memoryId; + update.startAddress = startAddress; + update.length = sizeToCheck; + update.crcShouldBeChecked = checkCrc; + } return returnvalue::OK; } -void PlocSupvHelper::initiateUpdateContinuation() { +ReturnValue_t PlocSupvHelper::initiateUpdateContinuation() { + lock->lockMutex(); + InternalState current = state; + lock->unlockMutex(); + if (current != InternalState::DEFAULT) { + return HasActionsIF::IS_BUSY; + } + MutexGuard mg(lock); + state = InternalState::LONGER_REQUEST; request = Request::CONTINUE_UPDATE; - semaphore->release(); -} - -ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) { -#ifdef XIPHOS_Q7S - ReturnValue_t result = FilesystemHelper::checkPath(path); - if (result != returnvalue::OK) { - return result; - } -#endif - if (not std::filesystem::exists(path)) { - return PATH_NOT_EXISTS; - } - eventBufferReq.path = path; - request = Request::REQUEST_EVENT_BUFFER; - uartComIF->flushUartTxAndRxBuf(comCookie); - semaphore->release(); return returnvalue::OK; } -void PlocSupvHelper::stopProcess() { terminate = true; } +// ReturnValue_t PlocSupvHelper::startEventBufferRequest(std::string path) { +//#ifdef XIPHOS_Q7S +// ReturnValue_t result = FilesystemHelper::checkPath(path); +// if (result != returnvalue::OK) { +// return result; +// } +//#endif +// if (not std::filesystem::exists(path)) { +// return PATH_NOT_EXISTS; +// } +// eventBufferReq.path = path; +// request = Request::REQUEST_EVENT_BUFFER; +// //uartComIF->flushUartTxAndRxBuf(comCookie); +// semaphore->release(); +// return returnvalue::OK; +// } + +void PlocSupvHelper::stop() { + MutexGuard mg(lock); + state = InternalState::GO_TO_SLEEP; +} void PlocSupvHelper::executeFullCheckMemoryCommand() { ReturnValue_t result; @@ -356,11 +385,6 @@ ReturnValue_t PlocSupvHelper::writeUpdatePackets() { uint16_t dataLength = 0; ccsds::SequenceFlags seqFlags; while (update.bytesWritten < update.fullFileSize) { - if (terminate) { - terminate = false; - triggerEvent(TERMINATED_UPDATE_PROCEDURE); - return PROCESS_TERMINATED; - } size_t remainingSize = update.fullFileSize - update.bytesWritten; bool lastSegment = false; if (remainingSize > supv::WriteMemory::CHUNK_MAX) { @@ -1031,5 +1055,8 @@ ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { void PlocSupvHelper::performUartShutdown() { tcflush(serialPort, TCIOFLUSH); + // Clear ring buffers + recRingBuf.clear(); + ipcRingBuf.clear(); state = InternalState::GO_TO_SLEEP; } diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index d36234e2..9426b2e4 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -122,7 +122,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, ReturnValue_t performOperation(uint8_t operationCode = 0) override; ReturnValue_t setComIF(UartComIF* uartComfIF_); - void setComCookie(CookieIF* comCookie_); + // void setComCookie(CookieIF* comCookie_); /** * @brief Starts update procedure @@ -136,24 +136,24 @@ class PlocSupvHelper : public DeviceCommunicationIF, ReturnValue_t performUpdate(const supv::UpdateParams& params); ReturnValue_t startUpdate(std::string file, uint8_t memoryId, uint32_t startAddress); - ReturnValue_t performMemCheck(uint8_t memoryId, uint32_t startAddress, size_t sizeToCheck, - bool checkCrc); + ReturnValue_t performMemCheck(std::string file, uint8_t memoryId, uint32_t startAddress, + size_t sizeToCheck, bool checkCrc); ReturnValue_t performMemCheck(std::string file, uint8_t memoryId, uint32_t startAddress); /** * @brief This initiate the continuation of a failed update. */ - void initiateUpdateContinuation(); + ReturnValue_t initiateUpdateContinuation(); /** * @brief Calling this function will initiate the procedure to request the event buffer */ - ReturnValue_t startEventBufferRequest(std::string path); + // ReturnValue_t startEventBufferRequest(std::string path); /** * @brief Can be used to interrupt a running data transfer. */ - void stopProcess(); + void stop(); static uint32_t buildProgParams1(uint8_t percent, uint16_t seqCount); @@ -242,7 +242,6 @@ class PlocSupvHelper : public DeviceCommunicationIF, std::array tmBuf{}; - bool terminate = false; bool debugMode = false; /** * Communication interface responsible for data transactions between OBC and Supervisor. From 28cd78db96ba6999d4b1376bb35fe2336d851073 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 16:26:30 +0100 Subject: [PATCH 111/244] implemented the last tricky parts --- .../PlocSupervisorDefinitions.h | 151 ++++---- linux/devices/ploc/PlocSupervisorHandler.cpp | 10 +- linux/devices/ploc/PlocSupvUartMan.cpp | 364 ++++++++++-------- linux/devices/ploc/PlocSupvUartMan.h | 34 +- mission/devices/devicedefinitions/SpBase.h | 2 + 5 files changed, 297 insertions(+), 264 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 34ac413e..2b248404 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -70,6 +70,8 @@ static const ReturnValue_t UPDATE_CRC_FAILURE = MAKE_RETURN_CODE(0xB2); static const ReturnValue_t SUPV_HELPER_EXECUTING = MAKE_RETURN_CODE(0xB3); static constexpr ReturnValue_t BUF_TOO_SMALL = MAKE_RETURN_CODE(0xC0); +static constexpr ReturnValue_t NO_REPLY_TIMEOUT = MAKE_RETURN_CODE(0xC1); + }; // namespace result static constexpr uint16_t DEFAULT_SEQ_COUNT = 0; @@ -270,6 +272,7 @@ namespace timeout { // Erase memory can require up to 60 seconds for execution static const uint32_t ERASE_MEMORY = 60000; static const uint32_t UPDATE_STATUS_REPORT = 70000; +static const uint32_t CRC_EXECUTION_TIMEOUT = 60000; } // namespace timeout static constexpr size_t TIMESTAMP_LEN = 7; @@ -421,6 +424,8 @@ class TcBase : public ploc::SpTcBase { payloadStart[supv::PAYLOAD_OFFSET] = id; } + uint8_t getServiceId() const { return payloadStart[supv::PAYLOAD_OFFSET]; } + static size_t fullSpDataLenFromPayloadLen(size_t payloadLen) { return SECONDARY_HEADER_LEN + payloadLen + CRC_LEN; } @@ -1168,19 +1173,11 @@ class AcknowledgmentReport : public VerificationReport { AcknowledgmentReport(TmBase& readerBase) : VerificationReport(readerBase) {} virtual ReturnValue_t parse() override { - // if (readerBase.getServiceId() != ) - // uint16_t apid = this->getApid(); - // if (apid == APID_ACK_SUCCESS) { - // return returnvalue::OK; - // } else if (apid == APID_ACK_FAILURE) { - // printStatusInformation(); - // return SupvReturnValuesIF::RECEIVED_ACK_FAILURE; - // } else { - // sif::warning << "AcknowledgmentReport::checkApid: Invalid apid: 0x" << std::hex << apid - // << std::endl; - // return SupvReturnValuesIF::INVALID_APID; - // } - return OK; + if (readerBase.getServiceId() != static_cast(tm::TmtcId::ACK) and + readerBase.getServiceId() != static_cast(tm::TmtcId::NAK)) { + return returnvalue::FAILED; + } + return VerificationReport::parse(); } void printStatusInformation() { @@ -1244,78 +1241,12 @@ class ExecutionReport : public VerificationReport { ExecutionReport(TmBase& readerBase) : VerificationReport(readerBase) {} ReturnValue_t parse() override { - if (readerBase.getServiceId() == static_cast(tm::TmtcId::EXEC_NAK)) { - printStatusInformation(); - return result::RECEIVED_EXE_FAILURE; + if (readerBase.getServiceId() != static_cast(tm::TmtcId::EXEC_ACK) and + readerBase.getServiceId() != static_cast(tm::TmtcId::EXEC_NAK)) { + return returnvalue::FAILED; } - /* uint16_t apid = this->getApid(); - if (apid == APID_EXE_SUCCESS) { - return returnvalue::OK; - } else if (apid == APID_EXE_FAILURE) { - printStatusInformation(); - return SupvReturnValuesIF::RECEIVED_EXE_FAILURE; - } else { - sif::warning << "ExecutionReport::checkApid: Invalid apid: 0x" << std::hex << apid - << std::endl; - return SupvReturnValuesIF::INVALID_APID; - }*/ - return OK; + return VerificationReport::parse(); } - - private: - static constexpr char STATUS_PRINTOUT_PREFIX[] = "Supervisor execution failure report status: "; - - enum class StatusCode : uint16_t { - OK = 0x0, - INIT_ERROR = 0x1, - BAD_PARAM = 0x2, - NOT_INITIALIZED = 0x3, - BAD_PERIPH_ID = 0x4, - TIMEOUT = 0x5, - RX_ERROR = 0x6, - TX_ERROR = 0x7, - BUF_EMPTY = 0x8, - BUF_FULL = 0x9, - NAK = 0xA, - ARB_LOST = 0xB, - BUSY = 0xC, - NOT_IMPLEMENTED = 0xD, - ALIGNEMENT_ERROR = 0xE, - PERIPH_ERR = 0xF, - FAILED_LATCH = 0x10, - GPIO_HIGH = 0x11, - GPIO_LOW = 0x12, - TEST_PASSED = 0x13, - TEST_FAILED = 0x14, - NOTHING_TODO = 0x100, - POWER_FAULT = 0x101, - INVALID_LENGTH = 0x102, - OUT_OF_RANGE = 0x103, - OUT_OF_HEAP_MEMORY = 0x104, - INVALID_STATE_TRANSITION = 0x105, - MPSOC_ALREADY_BOOTING = 0x106, - MPSOC_ALREADY_OPERATIONAL = 0x107, - MPSOC_BOOT_FAILED = 0x108, - SP_NOT_AVAILABLE = 0x200, - SP_DATA_INSUFFICIENT = 0x201, - SP_MEMORY_ID_INVALID = 0x202, - MPSOC_NOT_IN_RESET = 0x203, - FLASH_INIT_FAILED = 0x204, - FLASH_ERASE_FAILED = 0x205, - FLASH_WRITE_FAILED = 0x206, - FLASH_VERIFY_FAILED = 0x207, - CANNOT_ACCESS_TM = 0x208, - CANNOT_SEND_TM = 0x209, - PG_LOW = 0x300, - PG_5V_LOW = 0x301, - PG_0V85_LOW = 0x302, - PG_1V8_LOW = 0x303, - PG_MISC_LOW = 0x304, - PG_3V3_LOW = 0x305, - PG_MB_VAIO_LOW = 0x306, - PG_MB_MPSOCIO_LOW = 0x307 - }; - void printStatusInformation() { StatusCode statusCode = static_cast(getStatusCode()); switch (statusCode) { @@ -1520,6 +1451,60 @@ class ExecutionReport : public VerificationReport { break; } } + + private: + static constexpr char STATUS_PRINTOUT_PREFIX[] = "Supervisor execution failure report status: "; + + enum class StatusCode : uint16_t { + OK = 0x0, + INIT_ERROR = 0x1, + BAD_PARAM = 0x2, + NOT_INITIALIZED = 0x3, + BAD_PERIPH_ID = 0x4, + TIMEOUT = 0x5, + RX_ERROR = 0x6, + TX_ERROR = 0x7, + BUF_EMPTY = 0x8, + BUF_FULL = 0x9, + NAK = 0xA, + ARB_LOST = 0xB, + BUSY = 0xC, + NOT_IMPLEMENTED = 0xD, + ALIGNEMENT_ERROR = 0xE, + PERIPH_ERR = 0xF, + FAILED_LATCH = 0x10, + GPIO_HIGH = 0x11, + GPIO_LOW = 0x12, + TEST_PASSED = 0x13, + TEST_FAILED = 0x14, + NOTHING_TODO = 0x100, + POWER_FAULT = 0x101, + INVALID_LENGTH = 0x102, + OUT_OF_RANGE = 0x103, + OUT_OF_HEAP_MEMORY = 0x104, + INVALID_STATE_TRANSITION = 0x105, + MPSOC_ALREADY_BOOTING = 0x106, + MPSOC_ALREADY_OPERATIONAL = 0x107, + MPSOC_BOOT_FAILED = 0x108, + SP_NOT_AVAILABLE = 0x200, + SP_DATA_INSUFFICIENT = 0x201, + SP_MEMORY_ID_INVALID = 0x202, + MPSOC_NOT_IN_RESET = 0x203, + FLASH_INIT_FAILED = 0x204, + FLASH_ERASE_FAILED = 0x205, + FLASH_WRITE_FAILED = 0x206, + FLASH_VERIFY_FAILED = 0x207, + CANNOT_ACCESS_TM = 0x208, + CANNOT_SEND_TM = 0x209, + PG_LOW = 0x300, + PG_5V_LOW = 0x301, + PG_0V85_LOW = 0x302, + PG_1V8_LOW = 0x303, + PG_MISC_LOW = 0x304, + PG_3V3_LOW = 0x305, + PG_MB_VAIO_LOW = 0x306, + PG_MB_MPSOCIO_LOW = 0x307 + }; }; /** diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index f71cafe3..f0e56347 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -60,11 +60,11 @@ ReturnValue_t PlocSupervisorHandler::initialize() { sif::warning << "PlocSupervisorHandler::initialize: Invalid supervisor helper" << std::endl; return ObjectManagerIF::CHILD_INIT_FAILED; } - result = supvHelper->setComIF(uartComIf); - if (result != returnvalue::OK) { - return ObjectManagerIF::CHILD_INIT_FAILED; - } - // supvHelper->setComCookie(comCookie); + // result = supvHelper->setComIF(uartComIf); + // if (result != returnvalue::OK) { + // return ObjectManagerIF::CHILD_INIT_FAILED; + // } + // supvHelper->setComCookie(comCookie); result = eventSubscription(); if (result != returnvalue::OK) { diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index c9e9ad72..ef571c11 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -29,12 +29,13 @@ using namespace supv; PlocSupvHelper::PlocSupvHelper(object_id_t objectId) : SystemObject(objectId), recRingBuf(4096, true), + decodedRingBuf(1200 * MAX_STORED_DECODED_PACKETS, true), ipcRingBuf(1200 * MAX_STORED_DECODED_PACKETS, true) { - spParams.maxSize = sizeof(commandBuffer); resetSpParams(); semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); semaphore->acquire(); lock = MutexFactory::instance()->createMutex(); + ipcLock = MutexFactory::instance()->createMutex(); } PlocSupvHelper::~PlocSupvHelper() = default; @@ -158,14 +159,14 @@ bool PlocSupvHelper::handleUartReception() { return false; } -ReturnValue_t PlocSupvHelper::setComIF(UartComIF* uartComIF_) { - if (uartComIF_ == nullptr) { - sif::warning << "PlocSupvHelper::initialize: Provided invalid uart com if" << std::endl; - return returnvalue::FAILED; - } - uartComIF = uartComIF_; - return returnvalue::OK; -} +// ReturnValue_t PlocSupvHelper::setComIF(UartComIF* uartComIF_) { +// if (uartComIF_ == nullptr) { +// sif::warning << "PlocSupvHelper::initialize: Provided invalid uart com if" << std::endl; +// return returnvalue::FAILED; +// } +// //uartComIF = uartComIF_; +// return returnvalue::OK; +// } // void PlocSupvHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } @@ -431,7 +432,8 @@ ReturnValue_t PlocSupvHelper::writeUpdatePackets() { update.bytesWritten); return result; } - result = handlePacketTransmission(packet); + // TODO: Fix + // result = handlePacketTransmission(packet); if (result != returnvalue::OK) { triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount), update.bytesWritten); @@ -514,7 +516,7 @@ ReturnValue_t PlocSupvHelper::selectMemory() { if (result != returnvalue::OK) { return result; } - result = handlePacketTransmission(packet); + result = handlePacketTransmissionNoReply(packet); if (result != returnvalue::OK) { return result; } @@ -530,7 +532,7 @@ ReturnValue_t PlocSupvHelper::prepareUpdate() { if (result != returnvalue::OK) { return result; } - result = handlePacketTransmission(packet, PREPARE_UPDATE_EXECUTION_REPORT); + result = handlePacketTransmissionNoReply(packet, PREPARE_UPDATE_EXECUTION_REPORT); if (result != returnvalue::OK) { return result; } @@ -546,140 +548,131 @@ ReturnValue_t PlocSupvHelper::eraseMemory() { if (result != returnvalue::OK) { return result; } - result = handlePacketTransmission(eraseMemory, supv::timeout::ERASE_MEMORY); + result = handlePacketTransmissionNoReply(eraseMemory, supv::timeout::ERASE_MEMORY); if (result != returnvalue::OK) { return result; } return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::handlePacketTransmission(ploc::SpTcBase& packet, - uint32_t timeoutExecutionReport) { +ReturnValue_t PlocSupvHelper::handlePacketTransmissionNoReply(supv::TcBase& packet, + uint32_t timeoutExecutionReport) { ReturnValue_t result = returnvalue::OK; - result = sendCommand(packet); + result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); if (result != returnvalue::OK) { return result; } - result = handleAck(); - if (result != returnvalue::OK) { - return result; - } - result = handleExe(timeoutExecutionReport); - if (result != returnvalue::OK) { - return result; + Countdown countdown(timeoutExecutionReport); + bool ackReceived = false; + while (true) { + if (not decodedQueue.empty()) { + size_t packetLen = 0; + decodedQueue.retrieve(&packetLen); + decodedRingBuf.readData(decodedBuf.data(), packetLen); + tmReader.setData(decodedBuf.data(), packetLen); + result = checkReceivedTm(); + if (result != returnvalue::OK) { + continue; + } + if (tmReader.getApid() == Apid::TMTC_MAN) { + uint8_t serviceId = tmReader.getServiceId(); + int retval = 0; + if (not ackReceived) { + retval = handleAckReception(packet, serviceId, packetLen); + if (retval == 1) { + ackReceived = true; + } else if (retval == -1) { + return returnvalue::FAILED; + } + } else { + retval = handleExeAckReception(packet, serviceId, packetLen); + if (retval == 1) { + break; + } else if (retval == -1) { + return returnvalue::FAILED; + } + } + } else { + pushIpcData(decodedBuf.data(), packetLen); + decodedRingBuf.deleteData(packetLen); + } + } else { + TaskFactory::delayTask(50); + } + if (countdown.hasTimedOut()) { + return result::NO_REPLY_TIMEOUT; + } } return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::sendCommand(ploc::SpTcBase& packet) { - ReturnValue_t result = returnvalue::OK; - rememberApid = packet.getApid(); - result = uartComIF->sendMessage(comCookie, packet.getFullPacket(), packet.getFullPacketLen()); - if (result != returnvalue::OK) { - sif::warning << "PlocSupvHelper::sendCommand: Failed to send command" << std::endl; - triggerEvent(SUPV_SENDING_COMMAND_FAILED, result, static_cast(state)); - return result; - } - return result; -} - -ReturnValue_t PlocSupvHelper::handleAck() { - ReturnValue_t result = returnvalue::OK; - - // TODO: Fix - // result = handleTmReception(supv::SIZE_ACK_REPORT); - // if (result != returnvalue::OK) { - // triggerEvent(ACK_RECEPTION_FAILURE, result, static_cast(rememberApid)); - // sif::warning << "PlocSupvHelper::handleAck: Error in reception of acknowledgment report" - // << std::endl; - // return result; - // } - // supv::AcknowledgmentReport ackReport(tmBuf.data(), tmBuf.size()); - // result = checkReceivedTm(ackReport); - // if (result != returnvalue::OK) { - // return result; - // } - // result = ackReport.checkApid(); - // if (result != returnvalue::OK) { - // if (result == SupvReturnValuesIF::RECEIVED_ACK_FAILURE) { - // triggerEvent(SUPV_ACK_FAILURE_REPORT, static_cast(ackReport.getRefApid())); - // } else if (result == SupvReturnValuesIF::INVALID_APID) { - // triggerEvent(SUPV_ACK_INVALID_APID, static_cast(rememberApid)); - // } - // return result; - // } - return result; -} - -ReturnValue_t PlocSupvHelper::handleExe(uint32_t timeout) { - ReturnValue_t result = returnvalue::OK; - - result = handleTmReception(supv::SIZE_EXE_REPORT, tmBuf.data(), timeout); - if (result != returnvalue::OK) { - triggerEvent(EXE_RECEPTION_FAILURE, result, static_cast(rememberApid)); - sif::warning << "PlocSupvHelper::handleExe: Error in reception of execution report" - << std::endl; - return result; - } - - return exeReportHandling(); -} - -ReturnValue_t PlocSupvHelper::exeReportHandling() { - // TODO: Fix - // supv::ExecutionReport exeReport(tmBuf.data(), tmBuf.size()); - // - // ReturnValue_t result = checkReceivedTm(exeReport); - // if (result != returnvalue::OK) { - // return result; - // } - // result = exeReport.checkApid(); - // if (result != returnvalue::OK) { - // if (result == SupvReturnValuesIF::RECEIVED_EXE_FAILURE) { - // triggerEvent(SUPV_EXE_FAILURE_REPORT, static_cast(exeReport.getRefApid())); - // } else if (result == SupvReturnValuesIF::INVALID_APID) { - // triggerEvent(SUPV_EXE_INVALID_APID, static_cast(rememberApid)); - // } - // return result; - // } - return OK; -} - -ReturnValue_t PlocSupvHelper::handleTmReception(size_t remainingBytes, uint8_t* readBuf, - uint32_t timeout) { - ReturnValue_t result = returnvalue::OK; - size_t readBytes = 0; - size_t currentBytes = 0; - Countdown countdown(timeout); - if (readBuf == nullptr) { - readBuf = tmBuf.data(); - } - while (!countdown.hasTimedOut()) { - result = receive(readBuf + readBytes, ¤tBytes, remainingBytes); +int PlocSupvHelper::handleAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen) { + if (serviceId == static_cast(supv::tm::TmtcId::ACK) or + serviceId == static_cast(supv::tm::TmtcId::NAK)) { + AcknowledgmentReport ackReport(tmReader); + ReturnValue_t result = ackReport.parse(); if (result != returnvalue::OK) { - return result; + triggerEvent(ACK_RECEPTION_FAILURE); + return returnvalue::FAILED; } - readBytes += currentBytes; - remainingBytes = remainingBytes - currentBytes; - if (remainingBytes == 0) { - break; + if (ackReport.getRefApid() == tc.getApid() and + ackReport.getRefServiceId() == tc.getServiceId()) { + if (serviceId == static_cast(supv::tm::TmtcId::ACK)) { + return 1; + } else if (serviceId == static_cast(supv::tm::TmtcId::NAK)) { + ackReport.printStatusInformation(); + triggerEvent(SUPV_ACK_FAILURE_REPORT, + buildApidServiceParam1(ackReport.getRefApid(), ackReport.getRefServiceId()), + ackReport.getStatusCode()); + return -1; + } + // Should never happen + return -1; + } else { + pushIpcData(decodedBuf.data(), packetLen); + decodedRingBuf.deleteData(packetLen); } } - if (remainingBytes != 0) { - sif::warning << "PlocSupvHelper::handleTmReception: Failed to read " << std::dec - << remainingBytes << " remaining bytes" << std::endl; - return returnvalue::FAILED; - } - return result; + return 0; } -ReturnValue_t PlocSupvHelper::checkReceivedTm(ploc::SpTmReader& reader) { - ReturnValue_t result = reader.checkSize(); +int PlocSupvHelper::handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen) { + if (serviceId == static_cast(supv::tm::TmtcId::EXEC_ACK) or + serviceId == static_cast(supv::tm::TmtcId::EXEC_NAK)) { + ExecutionReport exeReport(tmReader); + ReturnValue_t result = exeReport.parse(); + if (result != returnvalue::OK) { + triggerEvent(EXE_RECEPTION_FAILURE); + return returnvalue::FAILED; + } + if (exeReport.getRefApid() == tc.getApid() and + exeReport.getRefServiceId() == tc.getServiceId()) { + if (serviceId == static_cast(supv::tm::TmtcId::EXEC_ACK)) { + return 1; + } else if (serviceId == static_cast(supv::tm::TmtcId::EXEC_NAK)) { + exeReport.printStatusInformation(); + triggerEvent(SUPV_EXE_FAILURE_REPORT, + buildApidServiceParam1(exeReport.getRefApid(), exeReport.getRefServiceId()), + exeReport.getStatusCode()); + return -1; + } + // Should never happen + return -1; + } else { + pushIpcData(decodedBuf.data(), packetLen); + decodedRingBuf.deleteData(packetLen); + } + } + return 0; +} + +ReturnValue_t PlocSupvHelper::checkReceivedTm() { + ReturnValue_t result = tmReader.checkSize(); if (result != returnvalue::OK) { triggerEvent(SUPV_REPLY_SIZE_MISSMATCH, rememberApid); return result; } - result = reader.checkCrc(); + result = tmReader.checkCrc(); if (result != returnvalue::OK) { triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid); return result; @@ -687,30 +680,6 @@ ReturnValue_t PlocSupvHelper::checkReceivedTm(ploc::SpTmReader& reader) { return result; } -ReturnValue_t PlocSupvHelper::receive(uint8_t* data, size_t* readBytes, size_t requestBytes) { - ReturnValue_t result = returnvalue::OK; - uint8_t* buffer = nullptr; - result = uartComIF->requestReceiveMessage(comCookie, requestBytes); - if (result != returnvalue::OK) { - sif::warning << "PlocSupvHelper::receive: Failed to request reply" << std::endl; - triggerEvent(SUPV_HELPER_REQUESTING_REPLY_FAILED, result, - static_cast(static_cast(state))); - return returnvalue::FAILED; - } - result = uartComIF->readReceivedMessage(comCookie, &buffer, readBytes); - if (result != returnvalue::OK) { - sif::warning << "PlocSupvHelper::receive: Failed to read received message" << std::endl; - triggerEvent(SUPV_HELPER_READING_REPLY_FAILED, result, static_cast(state)); - return returnvalue::FAILED; - } - if (*readBytes > 0) { - std::memcpy(data, buffer, *readBytes); - } else { - TaskFactory::delayTask(40); - } - return result; -} - ReturnValue_t PlocSupvHelper::calcImageCrc() { ReturnValue_t result = returnvalue::OK; if (update.fullFileSize == 0) { @@ -760,20 +729,64 @@ ReturnValue_t PlocSupvHelper::calcImageCrc() { ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { ReturnValue_t result = returnvalue::OK; // TODO: Fix - // resetSpParams(); - // // Will hold status report for later processing + resetSpParams(); + // Will hold status report for later processing // std::array statusReportBuf{}; // supv::UpdateStatusReport updateStatusReport(tmBuf.data(), tmBuf.size()); - // // Verification of update write procedure - // supv::CheckMemory packet(spParams); - // result = packet.buildPacket(update.memoryId, update.startAddress, update.fullFileSize); - // if (result != returnvalue::OK) { - // return result; - // } - // result = sendCommand(packet); - // if (result != returnvalue::OK) { - // return result; - // } + // Verification of update write procedure + supv::CheckMemory packet(spParams); + result = packet.buildPacket(update.memoryId, update.startAddress, update.fullFileSize); + if (result != returnvalue::OK) { + return result; + } + result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); + if (result != returnvalue::OK) { + return result; + } + Countdown countdown(timeout::CRC_EXECUTION_TIMEOUT); + bool ackReceived = false; + bool checkReplyReceived = false; + while (true) { + if (not decodedQueue.empty()) { + size_t packetLen = 0; + decodedQueue.retrieve(&packetLen); + decodedRingBuf.readData(decodedBuf.data(), packetLen); + tmReader.setData(decodedBuf.data(), packetLen); + result = checkReceivedTm(); + if (result != returnvalue::OK) { + continue; + } + if (tmReader.getApid() == Apid::TMTC_MAN) { + uint8_t serviceId = tmReader.getServiceId(); + int retval = 0; + if (not ackReceived) { + retval = handleAckReception(packet, serviceId, packetLen); + if (retval == 1) { + ackReceived = true; + } else if (retval == -1) { + return returnvalue::FAILED; + } + } else if (not checkReplyReceived) { + // if (serviceId == ) + retval = handleExeAckReception(packet, serviceId, packetLen); + if (retval == 1) { + break; + } else if (retval == -1) { + return returnvalue::FAILED; + } + } + } else { + pushIpcData(decodedBuf.data(), packetLen); + decodedRingBuf.deleteData(packetLen); + } + } else { + TaskFactory::delayTask(50); + } + if (countdown.hasTimedOut()) { + return result::NO_REPLY_TIMEOUT; + } + } + return returnvalue::OK; // result = handleAck(); // if (result != returnvalue::OK) { // return result; @@ -896,7 +909,7 @@ ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reade return result; } -void PlocSupvHelper::resetSpParams() { spParams.buf = commandBuffer; } +void PlocSupvHelper::resetSpParams() { spParams.buf = cmdBuf.data(); } ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { @@ -968,8 +981,8 @@ ReturnValue_t PlocSupvHelper::handleRunningLongerRequest() { ReturnValue_t PlocSupvHelper::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) { size_t encodedLen = 0; - hdlc_add_framing(sendData, sendLen, sendBuf.data(), &encodedLen); - size_t bytesWritten = write(serialPort, sendBuf.data(), encodedLen); + hdlc_add_framing(sendData, sendLen, encodedSendBuf.data(), &encodedLen); + size_t bytesWritten = write(serialPort, encodedSendBuf.data(), encodedLen); if (bytesWritten != encodedLen) { sif::warning << "ScexUartReader::sendMessage: Sending ping command to solar experiment failed" << std::endl; @@ -980,7 +993,7 @@ ReturnValue_t PlocSupvHelper::encodeAndSendPacket(const uint8_t* sendData, size_ ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { - MutexGuard mg(lock); + MutexGuard mg(ipcLock); if (ipcQueue.empty()) { *size = 0; return OK; @@ -999,7 +1012,14 @@ ReturnValue_t PlocSupvHelper::tryHdlcParsing() { ReturnValue_t result = parseRecRingBufForHdlc(bytesRead); if (result == returnvalue::OK) { // Packet found, advance read pointer. - ipcRingBuf.writeData(decodedBuf.data(), bytesRead); + if (state == InternalState::LONGER_REQUEST) { + decodedRingBuf.writeData(decodedBuf.data(), bytesRead); + decodedQueue.insert(bytesRead); + } else { + MutexGuard mg(ipcLock); + ipcRingBuf.writeData(decodedBuf.data(), bytesRead); + ipcQueue.insert(bytesRead); + } recRingBuf.deleteData(bytesRead); } else if (result != NO_PACKET_FOUND) { sif::warning << "ScexUartReader::performOperation: Possible packet loss" << std::endl; @@ -1053,10 +1073,28 @@ ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { return NO_PACKET_FOUND; } +void PlocSupvHelper::pushIpcData(const uint8_t* data, size_t len) { + MutexGuard mg(ipcLock); + ipcRingBuf.writeData(data, len); + ipcQueue.insert(len); +} + +uint32_t PlocSupvHelper::buildApidServiceParam1(uint8_t apid, uint8_t serviceId) { + return (apid << 8) | serviceId; +} + void PlocSupvHelper::performUartShutdown() { tcflush(serialPort, TCIOFLUSH); // Clear ring buffers recRingBuf.clear(); + decodedRingBuf.clear(); + while (not decodedQueue.empty()) { + decodedQueue.pop(); + } + MutexGuard mg(ipcLock); ipcRingBuf.clear(); + while (not ipcQueue.empty()) { + ipcQueue.pop(); + } state = InternalState::GO_TO_SLEEP; } diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 9426b2e4..34d8bd2e 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -121,8 +121,8 @@ class PlocSupvHelper : public DeviceCommunicationIF, ReturnValue_t initialize() override; ReturnValue_t performOperation(uint8_t operationCode = 0) override; - ReturnValue_t setComIF(UartComIF* uartComfIF_); - // void setComCookie(CookieIF* comCookie_); + // ReturnValue_t setComIF(UartComIF* uartComfIF_); + // void setComCookie(CookieIF* comCookie_); /** * @brief Starts update procedure @@ -156,6 +156,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, void stop(); static uint32_t buildProgParams1(uint8_t percent, uint16_t seqCount); + static uint32_t buildApidServiceParam1(uint8_t apid, uint8_t serviceId); private: static constexpr ReturnValue_t REQUEST_DONE = returnvalue::makeCode(1, 0); @@ -171,7 +172,6 @@ class PlocSupvHelper : public DeviceCommunicationIF, static const uint8_t NUM_EVENT_BUFFER_PACKETS = 25; static const size_t SIZE_EVENT_BUFFER_FULL_PACKET = 1024; static const size_t SIZE_EVENT_BUFFER_LAST_PACKET = 200; - static const uint32_t CRC_EXECUTION_TIMEOUT = 60000; static const uint32_t PREPARE_UPDATE_EXECUTION_REPORT = 2000; static constexpr uint8_t MAX_STORED_DECODED_PACKETS = 4; @@ -200,6 +200,8 @@ class PlocSupvHelper : public DeviceCommunicationIF, SemaphoreIF* semaphore; MutexIF* lock; + MutexIF* ipcLock; + supv::TmBase tmReader; int serialPort = 0; struct termios tty = {}; @@ -228,15 +230,17 @@ class PlocSupvHelper : public DeviceCommunicationIF, SdCardManager* sdcMan = nullptr; #endif SimpleRingBuffer recRingBuf; - std::array sendBuf = {}; + std::array cmdBuf = {}; + std::array encodedSendBuf = {}; std::array recBuf = {}; std::array encodedBuf = {}; std::array decodedBuf = {}; std::array ipcBuffer = {}; + SimpleRingBuffer decodedRingBuf; + FIFO decodedQueue; SimpleRingBuffer ipcRingBuf; FIFO ipcQueue; - uint8_t commandBuffer[supv::MAX_COMMAND_SIZE]{}; SpacePacketCreator creator; supv::TcParams spParams = supv::TcParams(creator); @@ -246,9 +250,9 @@ class PlocSupvHelper : public DeviceCommunicationIF, /** * Communication interface responsible for data transactions between OBC and Supervisor. */ - UartComIF* uartComIF = nullptr; - // Communication cookie. Must be set by the supervisor Handler - CookieIF* comCookie = nullptr; + // UartComIF* uartComIF = nullptr; + // Communication cookie. Must be set by the supervisor Handler + // CookieIF* comCookie = nullptr; bool timestamping = true; @@ -268,9 +272,12 @@ class PlocSupvHelper : public DeviceCommunicationIF, ReturnValue_t updateOperation(); ReturnValue_t writeUpdatePackets(); // ReturnValue_t performEventBufferRequest(); - ReturnValue_t handlePacketTransmission(ploc::SpTcBase& packet, - uint32_t timeoutExecutionReport = 60000); - ReturnValue_t sendCommand(ploc::SpTcBase& packet); + ReturnValue_t handlePacketTransmissionNoReply(supv::TcBase& packet, + uint32_t timeoutExecutionReport = 60000); + int handleAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen); + int handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen); + + // ReturnValue_t sendCommand(ploc::SpTcBase& packet); /** * @brief Function which reads form the communication interface * @@ -278,7 +285,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, * @param raedBytes Actual number of bytes read * @param requestBytes Number of bytes to read */ - ReturnValue_t receive(uint8_t* data, size_t* readBytes, size_t requestBytes); + // ReturnValue_t receive(uint8_t* data, size_t* readBytes, size_t requestBytes); ReturnValue_t handleAck(); ReturnValue_t handleExe(uint32_t timeout = 1000); /** @@ -293,7 +300,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, */ ReturnValue_t handleTmReception(size_t remainingBytes, uint8_t* readBuf = nullptr, uint32_t timeout = 70000); - ReturnValue_t checkReceivedTm(ploc::SpTmReader& reader); + ReturnValue_t checkReceivedTm(); ReturnValue_t selectMemory(); ReturnValue_t prepareUpdate(); @@ -315,6 +322,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, ReturnValue_t handleRemainingExeReport(ploc::SpTmReader& reader); void resetSpParams(); + void pushIpcData(const uint8_t* data, size_t len); /** * @brief Device specific initialization, using the cookie. diff --git a/mission/devices/devicedefinitions/SpBase.h b/mission/devices/devicedefinitions/SpBase.h index f34e921a..22c138b5 100644 --- a/mission/devices/devicedefinitions/SpBase.h +++ b/mission/devices/devicedefinitions/SpBase.h @@ -50,6 +50,8 @@ class SpTcBase { uint16_t getApid() const { return spParams.creator.getApid(); } + uint16_t getSeqCount() const { return spParams.creator.getSequenceCount(); } + ReturnValue_t checkPayloadLen() { if (ccsds::HEADER_LEN + spParams.fullPayloadLen > spParams.maxSize) { return SerializeIF::BUFFER_TOO_SHORT; From 1d6258d2234b39a53ec890d5bcfd4755d5ce6883 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 16:56:42 +0100 Subject: [PATCH 112/244] start fixing dev handler ack handling --- .../PlocSupervisorDefinitions.h | 123 ++++++++++-------- linux/devices/ploc/PlocSupervisorHandler.cpp | 38 +++--- linux/devices/ploc/PlocSupvUartMan.cpp | 42 +++--- linux/devices/ploc/PlocSupvUartMan.h | 11 -- 4 files changed, 104 insertions(+), 110 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 2b248404..dfc7acce 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -56,18 +56,19 @@ static const ReturnValue_t PATH_DOES_NOT_EXIST = MAKE_RETURN_CODE(0xAD); //! [EXPORT] : [COMMENT] MRAM dump file does not exists. The file should actually already have //! been created with the reception of the first dump packet. static const ReturnValue_t MRAM_FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xAE); +static constexpr ReturnValue_t INVALID_REPLY_LENGTH = MAKE_RETURN_CODE(0xAF); //! [EXPORT] : [COMMENT] Received action command has invalid length -static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xAF); +static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xB0); //! [EXPORT] : [COMMENT] Filename too long -static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xB0); +static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xB1); //! [EXPORT] : [COMMENT] Received update status report with invalid packet length field -static const ReturnValue_t UPDATE_STATUS_REPORT_INVALID_LENGTH = MAKE_RETURN_CODE(0xB1); +static const ReturnValue_t UPDATE_STATUS_REPORT_INVALID_LENGTH = MAKE_RETURN_CODE(0xB2); //! [EXPORT] : [COMMENT] Update status report does not contain expected CRC. There might be a bit //! flip in the update memory region. -static const ReturnValue_t UPDATE_CRC_FAILURE = MAKE_RETURN_CODE(0xB2); +static const ReturnValue_t UPDATE_CRC_FAILURE = MAKE_RETURN_CODE(0xB3); //! [EXPORT] : [COMMENT] Supervisor helper task ist currently executing a command (wait until //! helper tas has finished or interrupt by sending the terminate command) -static const ReturnValue_t SUPV_HELPER_EXECUTING = MAKE_RETURN_CODE(0xB3); +static const ReturnValue_t SUPV_HELPER_EXECUTING = MAKE_RETURN_CODE(0xB4); static constexpr ReturnValue_t BUF_TOO_SMALL = MAKE_RETURN_CODE(0xC0); static constexpr ReturnValue_t NO_REPLY_TIMEOUT = MAKE_RETURN_CODE(0xC1); @@ -470,6 +471,12 @@ class TmBase : public ploc::SpTmReader { uint8_t getServiceId() const { return getPacketData()[TIMESTAMP_LEN]; } const uint8_t* getPayloadStart() const { return getPacketData() + SECONDARY_HEADER_LEN; } + size_t getPayloadLen() const { + if (getFullPacketLen() > SECONDARY_HEADER_LEN + ccsds::HEADER_LEN) { + return getFullPacketLen() - SECONDARY_HEADER_LEN - ccsds::HEADER_LEN; + } + return 0; + } private: bool crcOk = false; @@ -1110,7 +1117,8 @@ class VerificationReport { if (readerBase.getApid() != Apid::TMTC_MAN) { return result::INVALID_APID; } - if (readerBase.getBufSize() < MIN_PAYLOAD_LEN + 8) { + if (readerBase.getBufSize() < MIN_TMTC_LEN + PAYLOAD_LEN or + readerBase.getPayloadLen() < PAYLOAD_LEN) { sif::error << "VerificationReport: Invalid verification report, payload too small" << std::endl; return result::BUF_TOO_SMALL; @@ -1507,6 +1515,59 @@ class ExecutionReport : public VerificationReport { }; }; +class UpdateStatusReport { + public: + UpdateStatusReport(TmBase& tmReader) : tmReader(tmReader) {} + + ReturnValue_t parse() { + if (not tmReader.crcIsOk()) { + return result::CRC_FAILURE; + } + if (tmReader.getApid() != Apid::MEM_MAN) { + return result::INVALID_APID; + } + if (tmReader.getBufSize() < MIN_TMTC_LEN + PAYLOAD_LEN or + tmReader.getPayloadLen() < PAYLOAD_LEN) { + sif::error << "VerificationReport: Invalid verification report, payload too small" + << std::endl; + return result::BUF_TOO_SMALL; + } + size_t remLen = PAYLOAD_LEN; + if (remLen < PAYLOAD_LEN) { + return result::INVALID_REPLY_LENGTH; + } + const uint8_t* dataFieldPtr = tmReader.getPayloadStart(); + SerializeAdapter::deSerialize(&memoryId, &dataFieldPtr, &remLen, SerializeIF::Endianness::BIG); + SerializeAdapter::deSerialize(&n, &dataFieldPtr, &remLen, SerializeIF::Endianness::BIG); + SerializeAdapter::deSerialize(&startAddress, &dataFieldPtr, &remLen, + SerializeIF::Endianness::BIG); + SerializeAdapter::deSerialize(&length, &dataFieldPtr, &remLen, SerializeIF::Endianness::BIG); + SerializeAdapter::deSerialize(&crc, &dataFieldPtr, &remLen, SerializeIF::Endianness::BIG); + return returnvalue::OK; + } + + ReturnValue_t verifyCrc(uint16_t goodCrc) const { + if (crc != goodCrc) { + return result::UPDATE_CRC_FAILURE; + } + return returnvalue::OK; + } + + uint16_t getCrc() const { return crc; } + + private: + TmBase& tmReader; + + // Nominal size of the space packet + static const uint16_t PAYLOAD_LEN = 12; // header, data field and crc + + uint8_t memoryId = 0; + uint8_t n = 0; + uint32_t startAddress = 0; + uint32_t length = 0; + uint16_t crc = 0; +}; + /** * @brief This dataset stores the boot status report of the supervisor. */ @@ -1661,56 +1722,6 @@ class LoggingReport : public StaticLocalDataSet { } }; -class UpdateStatusReport : public ploc::SpTmReader { - public: - UpdateStatusReport() = default; - UpdateStatusReport(const uint8_t* buf, size_t maxSize) : ploc::SpTmReader(buf, maxSize) {} - - ReturnValue_t parseDataField() { - ReturnValue_t result = lengthCheck(); - if (result != returnvalue::OK) { - return result; - } - const uint8_t* dataFieldPtr = getFullData() + ccsds::HEADER_LEN; - size_t size = 12; - SerializeAdapter::deSerialize(&memoryId, &dataFieldPtr, &size, SerializeIF::Endianness::BIG); - SerializeAdapter::deSerialize(&n, &dataFieldPtr, &size, SerializeIF::Endianness::BIG); - SerializeAdapter::deSerialize(&startAddress, &dataFieldPtr, &size, - SerializeIF::Endianness::BIG); - SerializeAdapter::deSerialize(&length, &dataFieldPtr, &size, SerializeIF::Endianness::BIG); - SerializeAdapter::deSerialize(&crc, &dataFieldPtr, &size, SerializeIF::Endianness::BIG); - return returnvalue::OK; - } - - ReturnValue_t verifycrc(uint16_t goodCrc) const { - if (crc != goodCrc) { - return result::UPDATE_CRC_FAILURE; - } - return returnvalue::OK; - } - - uint16_t getCrc() const { return crc; } - - uint16_t getNominalSize() const { return FULL_SIZE; } - - private: - // Nominal size of the space packet - static const uint16_t FULL_SIZE = 20; // header, data field and crc - - uint8_t memoryId = 0; - uint8_t n = 0; - uint32_t startAddress = 0; - uint32_t length = 0; - uint16_t crc = 0; - - ReturnValue_t lengthCheck() { - if (getFullPacketLen() != FULL_SIZE) { - return result::UPDATE_STATUS_REPORT_INVALID_LENGTH; - } - return returnvalue::OK; - } -}; - /** * @brief This dataset stores the ADC report. */ diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index f0e56347..3e31437b 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -60,11 +60,6 @@ ReturnValue_t PlocSupervisorHandler::initialize() { sif::warning << "PlocSupervisorHandler::initialize: Invalid supervisor helper" << std::endl; return ObjectManagerIF::CHILD_INIT_FAILED; } - // result = supvHelper->setComIF(uartComIf); - // if (result != returnvalue::OK) { - // return ObjectManagerIF::CHILD_INIT_FAILED; - // } - // supvHelper->setComCookie(comCookie); result = eventSubscription(); if (result != returnvalue::OK) { @@ -928,25 +923,22 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { using namespace supv; ReturnValue_t result = returnvalue::OK; + tmReader.setData(data, SIZE_ACK_REPORT); + if(tmReader.checkCrc() != returnvalue::OK) { + sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; + nextReplyId = supv::NONE; + replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); + triggerEvent(SUPV_CRC_FAILURE_EVENT); + sendFailureReport(supv::ACK_REPORT, result::CRC_FAILURE); + disableAllReplies(); + return returnvalue::OK; + } + AcknowledgmentReport ack(tmReader); + result = ack.parse(); + if (result != returnvalue::OK) { + return result; + } // TODO: Fix - - // AcknowledgmentReport ack(data, SIZE_ACK_REPORT); - // result = ack.checkSize(); - // if (result != returnvalue::OK) { - // return result; - // } - // - // result = ack.checkCrc(); - // if (result != returnvalue::OK) { - // sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; - // nextReplyId = supv::NONE; - // replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); - // triggerEvent(SUPV_CRC_FAILURE_EVENT); - // sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::CRC_FAILURE); - // disableAllReplies(); - // return returnvalue::OK; - // } - // // result = ack.checkApid(); // // switch (result) { diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index ef571c11..622acc89 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -159,17 +159,6 @@ bool PlocSupvHelper::handleUartReception() { return false; } -// ReturnValue_t PlocSupvHelper::setComIF(UartComIF* uartComIF_) { -// if (uartComIF_ == nullptr) { -// sif::warning << "PlocSupvHelper::initialize: Provided invalid uart com if" << std::endl; -// return returnvalue::FAILED; -// } -// //uartComIF = uartComIF_; -// return returnvalue::OK; -// } - -// void PlocSupvHelper::setComCookie(CookieIF* comCookie_) { comCookie = comCookie_; } - ReturnValue_t PlocSupvHelper::startUpdate(std::string file, uint8_t memoryId, uint32_t startAddress) { supv::UpdateParams params; @@ -432,8 +421,7 @@ ReturnValue_t PlocSupvHelper::writeUpdatePackets() { update.bytesWritten); return result; } - // TODO: Fix - // result = handlePacketTransmission(packet); + result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); if (result != returnvalue::OK) { triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount), update.bytesWritten); @@ -728,12 +716,7 @@ ReturnValue_t PlocSupvHelper::calcImageCrc() { ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { ReturnValue_t result = returnvalue::OK; - // TODO: Fix resetSpParams(); - // Will hold status report for later processing - // std::array statusReportBuf{}; - // supv::UpdateStatusReport updateStatusReport(tmBuf.data(), tmBuf.size()); - // Verification of update write procedure supv::CheckMemory packet(spParams); result = packet.buildPacket(update.memoryId, update.startAddress, update.fullFileSize); if (result != returnvalue::OK) { @@ -766,8 +749,7 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { } else if (retval == -1) { return returnvalue::FAILED; } - } else if (not checkReplyReceived) { - // if (serviceId == ) + } else if (checkReplyReceived) { retval = handleExeAckReception(packet, serviceId, packetLen); if (retval == 1) { break; @@ -775,6 +757,26 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { return returnvalue::FAILED; } } + } else if (tmReader.getApid() == Apid::MEM_MAN) { + if (ackReceived) { + supv::UpdateStatusReport report(tmReader); + result = report.parse(); + if (result != returnvalue::OK) { + return result; + } + if (update.crcShouldBeChecked) { + result = report.verifyCrc(update.crc); + if (result != returnvalue::OK) { + sif::warning + << "PlocSupvHelper::handleCheckMemoryCommand: CRC failure. Expected CRC 0x" + << std::setfill('0') << std::hex << std::setw(4) + << static_cast(update.crc) << " but received CRC 0x" << std::setw(4) + << report.getCrc() << std::dec << std::endl; + return result; + } + } + checkReplyReceived = true; + } } else { pushIpcData(decodedBuf.data(), packetLen); decodedRingBuf.deleteData(packetLen); diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 34d8bd2e..f8f175d5 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -277,17 +277,6 @@ class PlocSupvHelper : public DeviceCommunicationIF, int handleAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen); int handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen); - // ReturnValue_t sendCommand(ploc::SpTcBase& packet); - /** - * @brief Function which reads form the communication interface - * - * @param data Pointer to buffer where read data will be written to - * @param raedBytes Actual number of bytes read - * @param requestBytes Number of bytes to read - */ - // ReturnValue_t receive(uint8_t* data, size_t* readBytes, size_t requestBytes); - ReturnValue_t handleAck(); - ReturnValue_t handleExe(uint32_t timeout = 1000); /** * @brief Handles reading of TM packets from the communication interface * From 54523b25d1cf3e6031e119337d49a6b5ac0c826d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 17:07:12 +0100 Subject: [PATCH 113/244] fixed ack reply handling --- .../PlocSupervisorDefinitions.h | 45 ++++++------ linux/devices/ploc/PlocSupervisorHandler.cpp | 70 +++++++------------ 2 files changed, 49 insertions(+), 66 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index dfc7acce..08146e5e 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -21,54 +21,55 @@ static const uint8_t INTERFACE_ID = CLASS_ID::SUPV_RETURN_VALUES_IF; //! [EXPORT] : [COMMENT] Space Packet received from PLOC supervisor has invalid CRC static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA0); +static constexpr ReturnValue_t INVALID_SERVICE_ID = MAKE_RETURN_CODE(0xA1); //! [EXPORT] : [COMMENT] Received ACK failure reply from PLOC supervisor -static const ReturnValue_t RECEIVED_ACK_FAILURE = MAKE_RETURN_CODE(0xA1); +static const ReturnValue_t RECEIVED_ACK_FAILURE = MAKE_RETURN_CODE(0xA2); //! [EXPORT] : [COMMENT] Received execution failure reply from PLOC supervisor -static const ReturnValue_t RECEIVED_EXE_FAILURE = MAKE_RETURN_CODE(0xA2); +static const ReturnValue_t RECEIVED_EXE_FAILURE = MAKE_RETURN_CODE(0xA3); //! [EXPORT] : [COMMENT] Received space packet with invalid APID from PLOC supervisor -static const ReturnValue_t INVALID_APID = MAKE_RETURN_CODE(0xA3); +static const ReturnValue_t INVALID_APID = MAKE_RETURN_CODE(0xA4); //! [EXPORT] : [COMMENT] Failed to read current system time -static const ReturnValue_t GET_TIME_FAILURE = MAKE_RETURN_CODE(0xA4); +static const ReturnValue_t GET_TIME_FAILURE = MAKE_RETURN_CODE(0xA5); //! [EXPORT] : [COMMENT] Received command with invalid watchdog parameter. Valid watchdogs are 0 //! for PS, 1 for PL and 2 for INT -static const ReturnValue_t INVALID_WATCHDOG = MAKE_RETURN_CODE(0xA5); +static const ReturnValue_t INVALID_WATCHDOG = MAKE_RETURN_CODE(0xA6); //! [EXPORT] : [COMMENT] Received watchdog timeout config command with invalid timeout. Valid //! timeouts must be in the range between 1000 and 360000 ms. -static const ReturnValue_t INVALID_WATCHDOG_TIMEOUT = MAKE_RETURN_CODE(0xA6); +static const ReturnValue_t INVALID_WATCHDOG_TIMEOUT = MAKE_RETURN_CODE(0xA7); //! [EXPORT] : [COMMENT] Received latchup config command with invalid latchup ID -static const ReturnValue_t INVALID_LATCHUP_ID = MAKE_RETURN_CODE(0xA7); +static const ReturnValue_t INVALID_LATCHUP_ID = MAKE_RETURN_CODE(0xA8); //! [EXPORT] : [COMMENT] Received set adc sweep period command with invalid sweep period. Must be //! larger than 21. -static const ReturnValue_t SWEEP_PERIOD_TOO_SMALL = MAKE_RETURN_CODE(0xA8); +static const ReturnValue_t SWEEP_PERIOD_TOO_SMALL = MAKE_RETURN_CODE(0xA9); //! [EXPORT] : [COMMENT] Receive auto EM test command with invalid test param. Valid params are 1 //! and 2. -static const ReturnValue_t INVALID_TEST_PARAM = MAKE_RETURN_CODE(0xA9); +static const ReturnValue_t INVALID_TEST_PARAM = MAKE_RETURN_CODE(0xAA); //! [EXPORT] : [COMMENT] Returned when scanning for MRAM dump packets failed. -static const ReturnValue_t MRAM_PACKET_PARSING_FAILURE = MAKE_RETURN_CODE(0xAA); +static const ReturnValue_t MRAM_PACKET_PARSING_FAILURE = MAKE_RETURN_CODE(0xAB); //! [EXPORT] : [COMMENT] Returned when the start and stop addresses of the MRAM dump or MRAM wipe //! commands are invalid (e.g. start address bigger than stop address) -static const ReturnValue_t INVALID_MRAM_ADDRESSES = MAKE_RETURN_CODE(0xAB); +static const ReturnValue_t INVALID_MRAM_ADDRESSES = MAKE_RETURN_CODE(0xAC); //! [EXPORT] : [COMMENT] Expect reception of an MRAM dump packet but received space packet with //! other apid. -static const ReturnValue_t NO_MRAM_PACKET = MAKE_RETURN_CODE(0xAC); +static const ReturnValue_t NO_MRAM_PACKET = MAKE_RETURN_CODE(0xAD); //! [EXPORT] : [COMMENT] Path to PLOC directory on SD card does not exist -static const ReturnValue_t PATH_DOES_NOT_EXIST = MAKE_RETURN_CODE(0xAD); +static const ReturnValue_t PATH_DOES_NOT_EXIST = MAKE_RETURN_CODE(0xAE); //! [EXPORT] : [COMMENT] MRAM dump file does not exists. The file should actually already have //! been created with the reception of the first dump packet. -static const ReturnValue_t MRAM_FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xAE); -static constexpr ReturnValue_t INVALID_REPLY_LENGTH = MAKE_RETURN_CODE(0xAF); +static const ReturnValue_t MRAM_FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xAF); +static constexpr ReturnValue_t INVALID_REPLY_LENGTH = MAKE_RETURN_CODE(0xB0); //! [EXPORT] : [COMMENT] Received action command has invalid length -static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xB0); +static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xB1); //! [EXPORT] : [COMMENT] Filename too long -static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xB1); +static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xB2); //! [EXPORT] : [COMMENT] Received update status report with invalid packet length field -static const ReturnValue_t UPDATE_STATUS_REPORT_INVALID_LENGTH = MAKE_RETURN_CODE(0xB2); +static const ReturnValue_t UPDATE_STATUS_REPORT_INVALID_LENGTH = MAKE_RETURN_CODE(0xB3); //! [EXPORT] : [COMMENT] Update status report does not contain expected CRC. There might be a bit //! flip in the update memory region. -static const ReturnValue_t UPDATE_CRC_FAILURE = MAKE_RETURN_CODE(0xB3); +static const ReturnValue_t UPDATE_CRC_FAILURE = MAKE_RETURN_CODE(0xB4); //! [EXPORT] : [COMMENT] Supervisor helper task ist currently executing a command (wait until //! helper tas has finished or interrupt by sending the terminate command) -static const ReturnValue_t SUPV_HELPER_EXECUTING = MAKE_RETURN_CODE(0xB4); +static const ReturnValue_t SUPV_HELPER_EXECUTING = MAKE_RETURN_CODE(0xB5); static constexpr ReturnValue_t BUF_TOO_SMALL = MAKE_RETURN_CODE(0xC0); static constexpr ReturnValue_t NO_REPLY_TIMEOUT = MAKE_RETURN_CODE(0xC1); @@ -1165,8 +1166,6 @@ class VerificationReport { uint32_t getStatusCode() const { return statusCode; } - virtual ReturnValue_t checkApid() { return returnvalue::FAILED; } - protected: TmBase& readerBase; uint8_t refApid = 0; @@ -1183,7 +1182,7 @@ class AcknowledgmentReport : public VerificationReport { virtual ReturnValue_t parse() override { if (readerBase.getServiceId() != static_cast(tm::TmtcId::ACK) and readerBase.getServiceId() != static_cast(tm::TmtcId::NAK)) { - return returnvalue::FAILED; + return result::INVALID_SERVICE_ID; } return VerificationReport::parse(); } diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 3e31437b..d77ffb1c 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -924,54 +924,38 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; tmReader.setData(data, SIZE_ACK_REPORT); - if(tmReader.checkCrc() != returnvalue::OK) { - sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; - nextReplyId = supv::NONE; - replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); - triggerEvent(SUPV_CRC_FAILURE_EVENT); - sendFailureReport(supv::ACK_REPORT, result::CRC_FAILURE); - disableAllReplies(); - return returnvalue::OK; + if (tmReader.checkCrc() != returnvalue::OK) { + sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; + nextReplyId = supv::NONE; + replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); + triggerEvent(SUPV_CRC_FAILURE_EVENT); + sendFailureReport(supv::ACK_REPORT, result::CRC_FAILURE); + disableAllReplies(); + return returnvalue::OK; } AcknowledgmentReport ack(tmReader); result = ack.parse(); if (result != returnvalue::OK) { - return result; + nextReplyId = supv::NONE; + replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); + triggerEvent(SUPV_CRC_FAILURE_EVENT); + sendFailureReport(supv::ACK_REPORT, result); + disableAllReplies(); + return result; + } + if (tmReader.getServiceId() == static_cast(supv::tm::TmtcId::NAK)) { + DeviceCommandId_t commandId = getPendingCommand(); + if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { + triggerEvent(SUPV_ACK_FAILURE, commandId, static_cast(ack.getStatusCode())); + } + printAckFailureInfo(ack.getStatusCode(), commandId); + sendFailureReport(supv::ACK_REPORT, result::RECEIVED_ACK_FAILURE); + disableAllReplies(); + nextReplyId = supv::NONE; + result = IGNORE_REPLY_DATA; + } else if (tmReader.getServiceId() == static_cast(supv::tm::TmtcId::ACK)) { + setNextReplyId(); } - // TODO: Fix - // result = ack.checkApid(); - // - // switch (result) { - // case SupvReturnValuesIF::RECEIVED_ACK_FAILURE: { - // DeviceCommandId_t commandId = getPendingCommand(); - // if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { - // triggerEvent(SUPV_ACK_FAILURE, commandId, static_cast(ack.getStatusCode())); - // } - // printAckFailureInfo(ack.getStatusCode(), commandId); - // sendFailureReport(supv::ACK_REPORT, SupvReturnValuesIF::RECEIVED_ACK_FAILURE); - // disableAllReplies(); - // nextReplyId = supv::NONE; - // result = IGNORE_REPLY_DATA; - // break; - // } - // case returnvalue::OK: { - // setNextReplyId(); - // break; - // } - // case SupvReturnValuesIF::INVALID_APID: - // sif::warning << "PlocSupervisorHandler::handleAckReport: Invalid APID in Ack report" - // << std::endl; - // sendFailureReport(supv::ACK_REPORT, result); - // disableAllReplies(); - // nextReplyId = supv::NONE; - // result = IGNORE_REPLY_DATA; - // break; - // default: { - // sif::error << "PlocSupervisorHandler::handleAckReport: APID parsing failed" << std::endl; - // result = returnvalue::FAILED; - // break; - // } - // } return result; } From 27e46615b6ab94e629d4742e9f3000186d45c46b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 19:53:26 +0100 Subject: [PATCH 114/244] repaire exe handling --- .../PlocSupervisorDefinitions.h | 21 +++---- linux/devices/ploc/PlocSupervisorHandler.cpp | 59 +++++++++++-------- linux/devices/ploc/PlocSupervisorHandler.h | 6 +- 3 files changed, 51 insertions(+), 35 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 08146e5e..4b482a2b 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -12,8 +12,6 @@ #include "mission/devices/devicedefinitions/SpBase.h" -using namespace returnvalue; - namespace supv { namespace result { @@ -126,13 +124,16 @@ static const DeviceCommandId_t CONTINUE_UPDATE = 60; static const DeviceCommandId_t MEMORY_CHECK_WITH_FILE = 61; /** Reply IDs */ -static const DeviceCommandId_t ACK_REPORT = 100; -static const DeviceCommandId_t EXE_REPORT = 101; -static const DeviceCommandId_t HK_REPORT = 102; -static const DeviceCommandId_t BOOT_STATUS_REPORT = 103; -static const DeviceCommandId_t LATCHUP_REPORT = 104; -static const DeviceCommandId_t LOGGING_REPORT = 105; -static const DeviceCommandId_t ADC_REPORT = 106; +enum ReplyId : DeviceCommandId_t { + ACK_REPORT = 100, + EXE_REPORT = 101, + HK_REPORT = 102, + BOOT_STATUS_REPORT = 103, + LATCHUP_REPORT = 104, + LOGGING_REPORT = 105, + ADC_REPORT = 106, + UPDATE_STATUS_REPORT = 107, +}; // Size of complete space packet (6 byte header + size of data + 2 byte CRC) static const uint16_t SIZE_ACK_REPORT = 14; @@ -493,7 +494,7 @@ class NoPayloadPacket : public TcBase { ReturnValue_t buildPacket() { ReturnValue_t result = checkSizeAndSerializeHeader(); - if (result != OK) { + if (result != returnvalue::OK) { return result; } return calcAndSetCrc(); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index d77ffb1c..5374bc84 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -15,6 +15,7 @@ #include "fsfw/timemanager/Clock.h" using namespace supv; +using namespace returnvalue; PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, Gpio uartIsolatorSwitch, @@ -29,7 +30,7 @@ PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t u adcReport(this), powerSwitch(powerSwitch), supvHelper(supvHelper) { - if (comCookie == NULL) { + if (comCookie == nullptr) { sif::error << "PlocSupervisorHandler: Invalid com cookie" << std::endl; } if (supvHelper == nullptr) { @@ -48,11 +49,6 @@ ReturnValue_t PlocSupervisorHandler::initialize() { if (result != returnvalue::OK) { return result; } - uartComIf = dynamic_cast(communicationInterface); - if (uartComIf == nullptr) { - sif::warning << "PlocSupervisorHandler::initialize: Invalid uart com if" << std::endl; - return ObjectManagerIF::CHILD_INIT_FAILED; - } #ifndef TE0720_1CFA sdcMan = SdCardManager::instance(); #endif /* TE0720_1CFA */ @@ -447,10 +443,9 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d } void PlocSupervisorHandler::fillCommandAndReplyMap() { - using namespace supv; - this->insertInCommandMap(GET_HK_REPORT); - this->insertInCommandMap(START_MPSOC); - this->insertInCommandMap(SHUTDOWN_MPSOC); + insertInCommandMap(GET_HK_REPORT); + insertInCommandMap(START_MPSOC); + insertInCommandMap(SHUTDOWN_MPSOC); this->insertInCommandMap(SEL_MPSOC_BOOT_IMAGE); this->insertInCommandMap(SET_BOOT_TIMEOUT); this->insertInCommandMap(SET_MAX_RESTART_TRIES); @@ -503,8 +498,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite uint8_t expectedReplies, bool useAlternateId, DeviceCommandId_t alternateReplyID) { - using namespace supv; - ReturnValue_t result = returnvalue::OK; + ReturnValue_t result = OK; uint8_t enabledReplies = 0; @@ -657,13 +651,13 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r switch (tmReader.getServiceId()) { case (static_cast(supv::tm::TmtcId::ACK)): case (static_cast(supv::tm::TmtcId::NAK)): { - *foundLen = SIZE_ACK_REPORT; - *foundId = ACK_REPORT; + *foundLen = tmReader.getFullPacketLen(); + *foundId = ReplyId::ACK_REPORT; return OK; } case (static_cast(supv::tm::TmtcId::EXEC_ACK)): case (static_cast(supv::tm::TmtcId::EXEC_NAK)): { - *foundLen = SIZE_EXE_REPORT; + *foundLen = tmReader.getFullPacketLen(); *foundId = EXE_REPORT; return OK; } @@ -672,8 +666,8 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r } case (Apid::HK): { if (tmReader.getServiceId() == static_cast(supv::tm::HkId::REPORT)) { - *foundLen = SIZE_HK_REPORT; - *foundId = HK_REPORT; + *foundLen = tmReader.getFullPacketLen(); + *foundId = ReplyId::HK_REPORT; return OK; } else if (tmReader.getServiceId() == static_cast(supv::tm::HkId::HARDFAULTS)) { handleBadApidServiceCombination(SUPV_UNINIMPLEMENTED_TM, apid, tmReader.getServiceId()); @@ -684,8 +678,8 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r case (Apid::BOOT_MAN): { if (tmReader.getServiceId() == static_cast(supv::tm::BootManId::BOOT_STATUS_REPORT)) { - *foundLen = SIZE_BOOT_STATUS_REPORT; - *foundId = BOOT_STATUS_REPORT; + *foundLen = tmReader.getFullPacketLen(); + *foundId = ReplyId::BOOT_STATUS_REPORT; return OK; } break; @@ -693,6 +687,8 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r case (Apid::MEM_MAN): { if (tmReader.getServiceId() == static_cast(supv::tm::MemManId::UPDATE_STATUS_REPORT)) { + *foundLen = tmReader.getFullPacketLen(); + *foundId = ReplyId::UPDATE_STATUS_REPORT; // TODO: I think this will be handled by the uart manager? // Actually, this is a bit tricky. Maybe the lower level will have two separate ring // buffers, one for internally handled packets and one for packets which are handled @@ -923,7 +919,6 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { using namespace supv; ReturnValue_t result = returnvalue::OK; - tmReader.setData(data, SIZE_ACK_REPORT); if (tmReader.checkCrc() != returnvalue::OK) { sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; nextReplyId = supv::NONE; @@ -963,6 +958,23 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data) using namespace supv; ReturnValue_t result = returnvalue::OK; + if (tmReader.checkCrc() != OK) { + nextReplyId = supv::NONE; + return result::CRC_FAILURE; + } + ExecutionReport report(tmReader); + result = report.parse(); + if (result != OK) { + nextReplyId = supv::NONE; + return result; + } + if (tmReader.getServiceId() == static_cast(supv::tm::TmtcId::EXEC_ACK)) { + result = handleExecutionSuccessReport(report); + } else if (tmReader.getServiceId() == static_cast(supv::tm::TmtcId::EXEC_NAK)) { + handleExecutionFailureReport(report); + } + nextReplyId = supv::NONE; + return result; // TODO: Fix // ExecutionReport exe(data, SIZE_EXE_REPORT); // result = exe.checkSize(); @@ -2082,7 +2094,7 @@ ReturnValue_t PlocSupervisorHandler::eventSubscription() { return result; } -ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(const uint8_t* data) { +ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(ExecutionReport& report) { DeviceCommandId_t commandId = getPendingCommand(); switch (commandId) { case supv::READ_GPIO: { @@ -2130,11 +2142,12 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(const uint8_t* return returnvalue::OK; } -void PlocSupervisorHandler::handleExecutionFailureReport(uint16_t statusCode) { +void PlocSupervisorHandler::handleExecutionFailureReport(ExecutionReport& report) { using namespace supv; DeviceCommandId_t commandId = getPendingCommand(); + report.printStatusInformation(); if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { - triggerEvent(SUPV_EXE_FAILURE, commandId, static_cast(statusCode)); + triggerEvent(SUPV_EXE_FAILURE, commandId, static_cast(report.getStatusCode())); } sendFailureReport(EXE_REPORT, result::RECEIVED_EXE_FAILURE); disableExeReportReply(); diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 4221bf4d..898ef13c 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -13,6 +13,8 @@ #include "fsfw_hal/linux/uart/UartComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" +using supv::ExecutionReport; + /** * @brief This is the device handler for the supervisor of the PLOC which is programmed by * Thales. @@ -378,8 +380,8 @@ class PlocSupervisorHandler : public DeviceHandlerBase { supv::UpdateParams& params); ReturnValue_t eventSubscription(); - ReturnValue_t handleExecutionSuccessReport(const uint8_t* data); - void handleExecutionFailureReport(uint16_t statusCode); + ReturnValue_t handleExecutionSuccessReport(ExecutionReport& report); + void handleExecutionFailureReport(ExecutionReport& report); void printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId); }; From fd5cc19231c8e02ff025d6f79110330fbd46afbe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 20:04:57 +0100 Subject: [PATCH 115/244] remove non-working commands --- .../PlocSupervisorDefinitions.h | 2 + linux/devices/ploc/PlocSupervisorHandler.cpp | 195 ++++++------------ linux/devices/ploc/PlocSupervisorHandler.h | 1 - 3 files changed, 65 insertions(+), 133 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 4b482a2b..7facd798 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -1248,6 +1248,8 @@ class ExecutionReport : public VerificationReport { public: ExecutionReport(TmBase& readerBase) : VerificationReport(readerBase) {} + TmBase& getReader() { return readerBase; } + ReturnValue_t parse() override { if (readerBase.getServiceId() != static_cast(tm::TmtcId::EXEC_ACK) and readerBase.getServiceId() != static_cast(tm::TmtcId::EXEC_NAK)) { diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 5374bc84..3fed970e 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -443,55 +443,39 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d } void PlocSupervisorHandler::fillCommandAndReplyMap() { + // Command only insertInCommandMap(GET_HK_REPORT); insertInCommandMap(START_MPSOC); insertInCommandMap(SHUTDOWN_MPSOC); - this->insertInCommandMap(SEL_MPSOC_BOOT_IMAGE); - this->insertInCommandMap(SET_BOOT_TIMEOUT); - this->insertInCommandMap(SET_MAX_RESTART_TRIES); - this->insertInCommandMap(RESET_MPSOC); - this->insertInCommandMap(SET_TIME_REF); - this->insertInCommandMap(DISABLE_PERIOIC_HK_TRANSMISSION); - this->insertInCommandMap(GET_BOOT_STATUS_REPORT); - this->insertInCommandMap(ENABLE_LATCHUP_ALERT); - this->insertInCommandMap(DISABLE_LATCHUP_ALERT); - this->insertInCommandMap(SET_ALERT_LIMIT); - this->insertInCommandMap(SET_ADC_ENABLED_CHANNELS); - this->insertInCommandMap(SET_ADC_WINDOW_AND_STRIDE); - this->insertInCommandMap(SET_ADC_THRESHOLD); - this->insertInCommandMap(GET_LATCHUP_STATUS_REPORT); - this->insertInCommandMap(COPY_ADC_DATA_TO_MRAM); - this->insertInCommandMap(REQUEST_ADC_REPORT); - this->insertInCommandMap(RUN_AUTO_EM_TESTS); - this->insertInCommandMap(WIPE_MRAM); - this->insertInCommandMap(SET_GPIO); - this->insertInCommandMap(READ_GPIO); - this->insertInCommandMap(RESTART_SUPERVISOR); - this->insertInCommandMap(FACTORY_RESET_CLEAR_ALL); - this->insertInCommandMap(FACTORY_RESET_CLEAR_MIRROR); - this->insertInCommandMap(FACTORY_RESET_CLEAR_CIRCULAR); - this->insertInCommandMap(START_MPSOC_QUIET); - this->insertInCommandMap(SET_SHUTDOWN_TIMEOUT); - this->insertInCommandMap(FACTORY_FLASH); - this->insertInCommandMap(ENABLE_AUTO_TM); - this->insertInCommandMap(DISABLE_AUTO_TM); - this->insertInCommandMap(LOGGING_REQUEST_COUNTERS); - this->insertInCommandMap(LOGGING_CLEAR_COUNTERS); - this->insertInCommandMap(LOGGING_SET_TOPIC); - this->insertInCommandMap(RESET_PL); - this->insertInCommandMap(ENABLE_NVMS); - this->insertInCommandAndReplyMap(FIRST_MRAM_DUMP, 0, nullptr, 0, false, false, FIRST_MRAM_DUMP, - &mramDumpTimeout); - this->insertInCommandAndReplyMap(CONSECUTIVE_MRAM_DUMP, 0, nullptr, 0, false, false, - CONSECUTIVE_MRAM_DUMP, &mramDumpTimeout); - this->insertInReplyMap(ACK_REPORT, 3, nullptr, SIZE_ACK_REPORT, false, + insertInCommandMap(SEL_MPSOC_BOOT_IMAGE); + insertInCommandMap(SET_BOOT_TIMEOUT); + insertInCommandMap(SET_MAX_RESTART_TRIES); + insertInCommandMap(RESET_MPSOC); + insertInCommandMap(SET_TIME_REF); + insertInCommandMap(DISABLE_PERIOIC_HK_TRANSMISSION); + insertInCommandMap(GET_BOOT_STATUS_REPORT); + insertInCommandMap(ENABLE_LATCHUP_ALERT); + insertInCommandMap(DISABLE_LATCHUP_ALERT); + insertInCommandMap(SET_ALERT_LIMIT); + insertInCommandMap(GET_LATCHUP_STATUS_REPORT); + insertInCommandMap(RUN_AUTO_EM_TESTS); + insertInCommandMap(SET_GPIO); + insertInCommandMap(READ_GPIO); + insertInCommandMap(SET_SHUTDOWN_TIMEOUT); + insertInCommandMap(FACTORY_FLASH); + insertInCommandMap(RESET_PL); + + // ACK replies, use countdown for them + insertInReplyMap(ACK_REPORT, 0, nullptr, SIZE_ACK_REPORT, false, &acknowledgementReportTimeout); - this->insertInReplyMap(EXE_REPORT, 0, nullptr, SIZE_EXE_REPORT, false, &executionReportTimeout); - this->insertInReplyMap(HK_REPORT, 3, &hkset, SIZE_HK_REPORT); - this->insertInReplyMap(BOOT_STATUS_REPORT, 3, &bootStatusReport, SIZE_BOOT_STATUS_REPORT); - this->insertInReplyMap(LATCHUP_REPORT, 3, &latchupStatusReport, SIZE_LATCHUP_STATUS_REPORT); - this->insertInReplyMap(LOGGING_REPORT, 3, &loggingReport, SIZE_LOGGING_REPORT); - this->insertInReplyMap(ADC_REPORT, 3, &adcReport, SIZE_ADC_REPORT); + insertInReplyMap(EXE_REPORT, 0, nullptr, SIZE_EXE_REPORT, false, &executionReportTimeout); + + // TM replies + insertInReplyMap(HK_REPORT, 3, &hkset, SIZE_HK_REPORT); + insertInReplyMap(BOOT_STATUS_REPORT, 3, &bootStatusReport, SIZE_BOOT_STATUS_REPORT); + insertInReplyMap(LATCHUP_REPORT, 3, &latchupStatusReport, SIZE_LATCHUP_STATUS_REPORT); + insertInReplyMap(LOGGING_REPORT, 3, &loggingReport, SIZE_LOGGING_REPORT); + insertInReplyMap(ADC_REPORT, 3, &adcReport, SIZE_ADC_REPORT); } ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::iterator command, @@ -723,18 +707,10 @@ ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id, result = handleLatchupStatusReport(packet); break; } - // case (LOGGING_REPORT): { - // result = handleLoggingReport(packet); - // break; - // } case (ADC_REPORT): { result = handleAdcReport(packet); break; } - case (FIRST_MRAM_DUMP): - case (CONSECUTIVE_MRAM_DUMP): - result = handleMramDumpPacket(id); - break; case (EXE_REPORT): { result = handleExecutionReport(packet); break; @@ -749,22 +725,6 @@ ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id, return result; } -ReturnValue_t PlocSupervisorHandler::getSwitches(const uint8_t** switches, - uint8_t* numberOfSwitches) { - if (powerSwitch == power::NO_SWITCH) { - return DeviceHandlerBase::NO_SWITCH; - } - *numberOfSwitches = 1; - *switches = &powerSwitch; - return returnvalue::OK; -} - -void PlocSupervisorHandler::setNormalDatapoolEntriesInvalid() {} - -uint32_t PlocSupervisorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { - return 7000; -} - ReturnValue_t PlocSupervisorHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(supv::NUM_TMS, new PoolEntry({0})); @@ -975,40 +935,6 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data) } nextReplyId = supv::NONE; return result; - // TODO: Fix - // ExecutionReport exe(data, SIZE_EXE_REPORT); - // result = exe.checkSize(); - // if (result != returnvalue::OK) { - // return result; - // } - // - // result = exe.checkCrc(); - // if (result != returnvalue::OK) { - // sif::error << "PlocSupervisorHandler::handleExecutionReport: CRC failure" << std::endl; - // nextReplyId = supv::NONE; - // return result; - // } - // - // result = exe.checkApid(); - // - // switch (result) { - // case (returnvalue::OK): { - // handleExecutionSuccessReport(data); - // break; - // } - // case (SupvReturnValuesIF::RECEIVED_EXE_FAILURE): { - // handleExecutionFailureReport(exe.getStatusCode()); - // result = returnvalue::OK; - // break; - // } - // default: { - // sif::error << "PlocSupervisorHandler::handleExecutionReport: Unknown APID" << std::endl; - // result = returnvalue::FAILED; - // break; - // } - // } - // nextReplyId = supv::NONE; - return result; } ReturnValue_t PlocSupervisorHandler::handleHkReport(const uint8_t* data) { @@ -2096,38 +2022,29 @@ ReturnValue_t PlocSupervisorHandler::eventSubscription() { ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(ExecutionReport& report) { DeviceCommandId_t commandId = getPendingCommand(); + ReturnValue_t result = OK; switch (commandId) { case supv::READ_GPIO: { // TODO: Fix - // supv::ExecutionReport exe(data, supv::SIZE_EXE_REPORT); - // if (exe.isNull()) { - // return returnvalue::FAILED; - // } - // ReturnValue_t result = exe.checkSize(); - // if (result != returnvalue::OK) { - // return result; - // } - // uint16_t gpioState = exe.getStatusCode(); - //#if OBSW_DEBUG_PLOC_SUPERVISOR == 1 - // sif::info << "PlocSupervisorHandler: Read GPIO TM, State: " << gpioState << std::endl; - //#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ - // DeviceCommandMap::iterator iter = deviceCommandMap.find(commandId); - // if (iter->second.sendReplyTo == NO_COMMAND_ID) { - // return returnvalue::OK; - // } - // uint8_t data[sizeof(gpioState)]; - // size_t size = 0; - // result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState), - // SerializeIF::Endianness::BIG); - // if (result != returnvalue::OK) { - // sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << - // std::endl; - // } - // result = actionHelper.reportData(iter->second.sendReplyTo, commandId, data, - // sizeof(data)); if (result != returnvalue::OK) { - // sif::warning << "PlocSupervisorHandler: Read GPIO, failed to report data" << - // std::endl; - // } + uint16_t gpioState = report.getStatusCode(); +#if OBSW_DEBUG_PLOC_SUPERVISOR == 1 + sif::info << "PlocSupervisorHandler: Read GPIO TM, State: " << gpioState << std::endl; +#endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ + DeviceCommandMap::iterator iter = deviceCommandMap.find(commandId); + if (iter->second.sendReplyTo == NO_COMMAND_ID) { + return returnvalue::OK; + } + uint8_t data[sizeof(gpioState)]; + size_t size = 0; + result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState), + SerializeIF::Endianness::BIG); + if (result != returnvalue::OK) { + sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << std::endl; + } + result = actionHelper.reportData(iter->second.sendReplyTo, commandId, data, sizeof(data)); + if (result != returnvalue::OK) { + sif::warning << "PlocSupervisorHandler: Read GPIO, failed to report data" << std::endl; + } break; } case supv::SET_TIME_REF: { @@ -2179,3 +2096,17 @@ void PlocSupervisorHandler::printAckFailureInfo(uint16_t statusCode, DeviceComma break; } } + +ReturnValue_t PlocSupervisorHandler::getSwitches(const uint8_t** switches, + uint8_t* numberOfSwitches) { + if (powerSwitch == power::NO_SWITCH) { + return DeviceHandlerBase::NO_SWITCH; + } + *numberOfSwitches = 1; + *switches = &powerSwitch; + return returnvalue::OK; +} + +uint32_t PlocSupervisorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { + return 7000; +} diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 898ef13c..178875d6 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -50,7 +50,6 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) override; ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override; - void setNormalDatapoolEntriesInvalid() override; uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) override; From 52f15906d8f607e5cf1d1f269425d0c67026d635 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 20:14:42 +0100 Subject: [PATCH 116/244] kick out more commands --- linux/devices/ploc/PlocSupervisorHandler.cpp | 32 ++++++-------------- 1 file changed, 10 insertions(+), 22 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 3fed970e..24d3f198 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -139,18 +139,6 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } - // case LOGGING_REQUEST_EVENT_BUFFERS: { - // if (size > config::MAX_PATH_SIZE) { - // return result::FILENAME_TOO_LONG; - // } - // result = supvHelper->startEventBufferRequest( - // std::string(reinterpret_cast(data), size)); - // if (result != returnvalue::OK) { - // return result; - // } - // plocSupvHelperExecuting = true; - // return EXECUTION_FINISHED; - // } default: break; } @@ -568,25 +556,25 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite case SET_ADC_ENABLED_CHANNELS: case SET_ADC_WINDOW_AND_STRIDE: case SET_ADC_THRESHOLD: - case COPY_ADC_DATA_TO_MRAM: + //case COPY_ADC_DATA_TO_MRAM: case RUN_AUTO_EM_TESTS: - case WIPE_MRAM: + //case WIPE_MRAM: case SET_GPIO: case READ_GPIO: - case RESTART_SUPERVISOR: - case FACTORY_RESET_CLEAR_ALL: - case FACTORY_RESET_CLEAR_MIRROR: - case FACTORY_RESET_CLEAR_CIRCULAR: + //case RESTART_SUPERVISOR: + //case FACTORY_RESET_CLEAR_ALL: + //case FACTORY_RESET_CLEAR_MIRROR: + //case FACTORY_RESET_CLEAR_CIRCULAR: case DISABLE_PERIOIC_HK_TRANSMISSION: - case START_MPSOC_QUIET: + //case START_MPSOC_QUIET: case SET_SHUTDOWN_TIMEOUT: case FACTORY_FLASH: case ENABLE_AUTO_TM: case DISABLE_AUTO_TM: - case LOGGING_CLEAR_COUNTERS: - case LOGGING_SET_TOPIC: + //case LOGGING_CLEAR_COUNTERS: + //case LOGGING_SET_TOPIC: case RESET_PL: - case ENABLE_NVMS: + //case ENABLE_NVMS: enabledReplies = 2; break; default: From d3da5bd2d86ac3a648b0ef553d42a5d0de502c3b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 20:37:57 +0100 Subject: [PATCH 117/244] add factory reset cmd --- .../PlocSupervisorDefinitions.h | 63 ++++++++++++------- linux/devices/ploc/PlocSupervisorHandler.cpp | 44 +++++++++---- linux/devices/ploc/PlocSupervisorHandler.h | 2 + 3 files changed, 73 insertions(+), 36 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 7facd798..fa4c728b 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -102,10 +102,8 @@ static const DeviceCommandId_t FIRST_MRAM_DUMP = 30; static const DeviceCommandId_t SET_GPIO = 34; static const DeviceCommandId_t READ_GPIO = 35; static const DeviceCommandId_t RESTART_SUPERVISOR = 36; -static const DeviceCommandId_t FACTORY_RESET_CLEAR_ALL = 37; static const DeviceCommandId_t LOGGING_REQUEST_COUNTERS = 38; -static const DeviceCommandId_t FACTORY_RESET_CLEAR_MIRROR = 40; -static const DeviceCommandId_t FACTORY_RESET_CLEAR_CIRCULAR = 41; +static constexpr DeviceCommandId_t FACTORY_RESET = 39; static const DeviceCommandId_t CONSECUTIVE_MRAM_DUMP = 43; static const DeviceCommandId_t START_MPSOC_QUIET = 45; static const DeviceCommandId_t SET_SHUTDOWN_TIMEOUT = 46; @@ -144,27 +142,6 @@ static const uint16_t SIZE_LATCHUP_STATUS_REPORT = 31; static const uint16_t SIZE_LOGGING_REPORT = 73; static const uint16_t SIZE_ADC_REPORT = 72; -/** - * SpacePacket apids of telemetry packets - */ -// static const uint16_t APID_ACK_SUCCESS = 0x200; -// static const uint16_t APID_ACK_FAILURE = 0x201; -// static const uint16_t APID_EXE_SUCCESS = 0x202; -// static const uint16_t APID_EXE_FAILURE = 0x203; -// static const uint16_t APID_HK_REPORT = 0x204; -// static const uint16_t APID_BOOT_STATUS_REPORT = 0x205; -// static const uint16_t APID_UPDATE_STATUS_REPORT = 0x206; -// static const uint16_t APID_ADC_REPORT = 0x207; -// static const uint16_t APID_LATCHUP_STATUS_REPORT = 0x208; -// static const uint16_t APID_SOC_SYSMON = 0x209; -// static const uint16_t APID_MRAM_DUMP_TM = 0x20A; -// static const uint16_t APID_SRAM = 0x20B; -// static const uint16_t APID_NOR_DATA = 0x20C; -// static const uint16_t APID_DATA_LOGGER_DATA = 0x20D; - -/** - * APIDs of telecommand packets - */ // 2 bits APID SRC, 00 for OBC, 2 bits APID DEST, 01 for SUPV, 7 bits CMD ID -> Mask 0x080 static constexpr uint16_t APID_TC_SUPV_MASK = 0x080; @@ -292,6 +269,21 @@ static constexpr size_t MIN_PAYLOAD_LEN = SECONDARY_HEADER_LEN + CRC_LEN; static constexpr size_t MIN_TMTC_LEN = ccsds::HEADER_LEN + MIN_PAYLOAD_LEN; static constexpr size_t PAYLOAD_OFFSET = ccsds::HEADER_LEN + SECONDARY_HEADER_LEN; +enum class FactoryResetSelect : uint8_t { + EVENT_BUF = 0x00, + ADC_BUF = 0x01, + SYS_CFG = 0x02, + DEBUG_CFG = 0x03, + BOOT_MAN_CFG = 0x04, + DATA_LOGGER_CFG = 0x05, + DATA_LOGGER_OP_DATA = 0x06, + LATCHUP_MON_CFG = 0x07, + ADC_MON_CFG = 0x08, + WDOG_MAN_CFG = 0x09, + HK_CFG = 0x0A, + MEM_MAN_CFG = 0xB9 +}; + struct UpdateParams { std::string file; uint8_t memId; @@ -646,6 +638,29 @@ class SetBootTimeout : public TcBase { } }; +class FactoryReset : public TcBase { + public: + FactoryReset(TcParams params) + : TcBase(params, Apid::DATA_LOGGER, + static_cast(tc::DataLoggerServiceId::FACTORY_RESET), 0) {} + + ReturnValue_t buildPacket(std::optional op) { + if (op) { + setLenFromPayloadLen(1); + } + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + if (op) { + payloadStart[0] = op.value(); + } + return calcAndSetCrc(); + } + + private: +}; + /** * @brief This class can be used to generate the space packet to set the maximum boot tries. */ diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 24d3f198..8a3613ad 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -310,6 +310,10 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d result = returnvalue::OK; break; } + case FACTORY_RESET: { + result = prepareFactoryResetCmd(commandData, commandDataLen); + break; + } case READ_GPIO: { prepareReadGpioCmd(commandData); result = returnvalue::OK; @@ -449,13 +453,13 @@ void PlocSupervisorHandler::fillCommandAndReplyMap() { insertInCommandMap(RUN_AUTO_EM_TESTS); insertInCommandMap(SET_GPIO); insertInCommandMap(READ_GPIO); + insertInCommandMap(FACTORY_RESET); insertInCommandMap(SET_SHUTDOWN_TIMEOUT); insertInCommandMap(FACTORY_FLASH); insertInCommandMap(RESET_PL); // ACK replies, use countdown for them - insertInReplyMap(ACK_REPORT, 0, nullptr, SIZE_ACK_REPORT, false, - &acknowledgementReportTimeout); + insertInReplyMap(ACK_REPORT, 0, nullptr, SIZE_ACK_REPORT, false, &acknowledgementReportTimeout); insertInReplyMap(EXE_REPORT, 0, nullptr, SIZE_EXE_REPORT, false, &executionReportTimeout); // TM replies @@ -556,25 +560,26 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite case SET_ADC_ENABLED_CHANNELS: case SET_ADC_WINDOW_AND_STRIDE: case SET_ADC_THRESHOLD: - //case COPY_ADC_DATA_TO_MRAM: + // case COPY_ADC_DATA_TO_MRAM: case RUN_AUTO_EM_TESTS: - //case WIPE_MRAM: + // case WIPE_MRAM: case SET_GPIO: + case FACTORY_RESET: case READ_GPIO: - //case RESTART_SUPERVISOR: - //case FACTORY_RESET_CLEAR_ALL: - //case FACTORY_RESET_CLEAR_MIRROR: - //case FACTORY_RESET_CLEAR_CIRCULAR: + // case RESTART_SUPERVISOR: + // case FACTORY_RESET_CLEAR_ALL: + // case FACTORY_RESET_CLEAR_MIRROR: + // case FACTORY_RESET_CLEAR_CIRCULAR: case DISABLE_PERIOIC_HK_TRANSMISSION: - //case START_MPSOC_QUIET: + // case START_MPSOC_QUIET: case SET_SHUTDOWN_TIMEOUT: case FACTORY_FLASH: case ENABLE_AUTO_TM: case DISABLE_AUTO_TM: - //case LOGGING_CLEAR_COUNTERS: - //case LOGGING_SET_TOPIC: + // case LOGGING_CLEAR_COUNTERS: + // case LOGGING_SET_TOPIC: case RESET_PL: - //case ENABLE_NVMS: + // case ENABLE_NVMS: enabledReplies = 2; break; default: @@ -1565,6 +1570,21 @@ ReturnValue_t PlocSupervisorHandler::prepareReadGpioCmd(const uint8_t* commandDa return returnvalue::OK; } +ReturnValue_t PlocSupervisorHandler::prepareFactoryResetCmd(const uint8_t* commandData, + size_t len) { + FactoryReset resetCmd(spParams); + std::optional op; + if (len > 0) { + op = commandData[0]; + } + ReturnValue_t result = resetCmd.buildPacket(op); + if (result != returnvalue::OK) { + return result; + } + finishTcPrep(resetCmd.getFullPacketLen()); + return returnvalue::OK; +} + void PlocSupervisorHandler::finishTcPrep(size_t packetLen) { nextReplyId = supv::ACK_REPORT; rawPacket = commandBuffer; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 178875d6..99b222c6 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -264,6 +264,8 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t prepareRestartTriesCmd(const uint8_t* commandData); + ReturnValue_t prepareFactoryResetCmd(const uint8_t* commandData, size_t len); + /** * @brief This function fills the command buffer with the packet to enable or disable the * watchdogs on the PLOC. From e2a0db8fcc15380a145e779fb1dbf7918990be11 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 20:41:20 +0100 Subject: [PATCH 118/244] command and function graveyard --- linux/devices/ploc/PlocSupervisorHandler.cpp | 329 ++++++++++--------- 1 file changed, 165 insertions(+), 164 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 8a3613ad..5ea9d539 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -262,49 +262,15 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d result = prepareSetAlertLimitCmd(commandData); break; } - // case SET_ADC_ENABLED_CHANNELS: { - // prepareSetAdcEnabledChannelsCmd(commandData); - // result = returnvalue::OK; - // break; - // } - // case SET_ADC_WINDOW_AND_STRIDE: { - // prepareSetAdcWindowAndStrideCmd(commandData); - // result = returnvalue::OK; - // break; - // } - // case SET_ADC_THRESHOLD: { - // prepareSetAdcThresholdCmd(commandData); - // result = returnvalue::OK; - // break; - // } case GET_LATCHUP_STATUS_REPORT: { prepareEmptyCmd(Apid::LATCHUP_MON, static_cast(tc::LatchupMonId::GET_STATUS_REPORT)); result = returnvalue::OK; break; } - // I think this is disabled right now according to the TC excel table - // case COPY_ADC_DATA_TO_MRAM: { - // prepareEmptyCmd(APID_COPY_ADC_DATA_TO_MRAM); - // result = returnvalue::OK; - // break; - // } - // case REQUEST_ADC_REPORT: { - // prepareEmptyCmd(APID_REQUEST_ADC_REPORT); - // result = returnvalue::OK; - // break; - // } case RUN_AUTO_EM_TESTS: { result = prepareRunAutoEmTest(commandData); break; } - // case WIPE_MRAM: { - // result = prepareWipeMramCmd(commandData); - // break; - // } - // case FIRST_MRAM_DUMP: - // case CONSECUTIVE_MRAM_DUMP: - // result = prepareDumpMramCmd(commandData); - // break; case SET_GPIO: { prepareSetGpioCmd(commandData); result = returnvalue::OK; @@ -319,6 +285,25 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d result = returnvalue::OK; break; } + case SET_SHUTDOWN_TIMEOUT: { + prepareSetShutdownTimeoutCmd(commandData); + result = returnvalue::OK; + break; + } + case FACTORY_FLASH: { + prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::FACTORY_FLASH)); + result = returnvalue::OK; + break; + } + case RESET_PL: { + prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::RESET_PL)); + result = returnvalue::OK; + break; + } + // case ENABLE_NVMS: { + // result = prepareEnableNvmsCommand(commandData); + // break; + // } // case RESTART_SUPERVISOR: { // prepareEmptyCmd(APID_RESTART_SUPERVISOR); // result = returnvalue::OK; @@ -357,16 +342,22 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // result = returnvalue::OK; // break; // } - case SET_SHUTDOWN_TIMEOUT: { - prepareSetShutdownTimeoutCmd(commandData); - result = returnvalue::OK; - break; - } - case FACTORY_FLASH: { - prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::FACTORY_FLASH)); - result = returnvalue::OK; - break; - } + // case SET_ADC_ENABLED_CHANNELS: { + + // prepareSetAdcEnabledChannelsCmd(commandData); + // result = returnvalue::OK; + // break; + // } + // case SET_ADC_WINDOW_AND_STRIDE: { + // prepareSetAdcWindowAndStrideCmd(commandData); + // result = returnvalue::OK; + // break; + // } + // case SET_ADC_THRESHOLD: { + // prepareSetAdcThresholdCmd(commandData); + // result = returnvalue::OK; + // break; + // } // case ENABLE_AUTO_TM: { // EnableAutoTm packet(spParams); // result = packet.buildPacket(); @@ -416,15 +407,25 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // finishTcPrep(packet.getFullPacketLen()); // break; // } - case RESET_PL: { - prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::RESET_PL)); - result = returnvalue::OK; - break; - } - // case ENABLE_NVMS: { - // result = prepareEnableNvmsCommand(commandData); + // I think this is disabled right now according to the TC excel table + // case COPY_ADC_DATA_TO_MRAM: { + // prepareEmptyCmd(APID_COPY_ADC_DATA_TO_MRAM); + // result = returnvalue::OK; // break; // } + // case REQUEST_ADC_REPORT: { + // prepareEmptyCmd(APID_REQUEST_ADC_REPORT); + // result = returnvalue::OK; + // break; + // } + // case WIPE_MRAM: { + // result = prepareWipeMramCmd(commandData); + // break; + // } + // case FIRST_MRAM_DUMP: + // case CONSECUTIVE_MRAM_DUMP: + // result = prepareDumpMramCmd(commandData); + // break; default: sif::debug << "PlocSupervisorHandler::buildCommandFromCommand: Command not implemented" << std::endl; @@ -1503,48 +1504,6 @@ ReturnValue_t PlocSupervisorHandler::prepareRunAutoEmTest(const uint8_t* command return returnvalue::OK; } -// ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandData) { -// uint32_t start = 0; -// uint32_t stop = 0; -// size_t size = sizeof(start) + sizeof(stop); -// SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); -// SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); -// if ((stop - start) <= 0) { -// return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; -// } -// supv::MramCmd packet(spParams); -// ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE); -// if (result != returnvalue::OK) { -// return result; -// } -// finishTcPrep(packet.getFullPacketLen()); -// return returnvalue::OK; -// } - -// ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandData) { -// uint32_t start = 0; -// uint32_t stop = 0; -// size_t size = sizeof(start) + sizeof(stop); -// SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); -// SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); -// if ((stop - start) <= 0) { -// return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; -// } -// supv::MramCmd packet(spParams); -// ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::DUMP); -// if (result != returnvalue::OK) { -// return result; -// } -// expectedMramDumpPackets = (stop - start) / supv::MAX_DATA_CAPACITY; -// if ((stop - start) % supv::MAX_DATA_CAPACITY) { -// expectedMramDumpPackets++; -// } -// receivedMramDumpPackets = 0; -// -// finishTcPrep(packet.getFullPacketLen()); -// return returnvalue::OK; -// } - ReturnValue_t PlocSupervisorHandler::prepareSetGpioCmd(const uint8_t* commandData) { uint8_t port = *commandData; uint8_t pin = *(commandData + 1); @@ -1611,33 +1570,6 @@ ReturnValue_t PlocSupervisorHandler::prepareSetShutdownTimeoutCmd(const uint8_t* return returnvalue::OK; } -// ReturnValue_t PlocSupervisorHandler::prepareLoggingRequest(const uint8_t* commandData, -// size_t commandDataLen) { -// using namespace supv; -// RequestLoggingData::Sa sa = static_cast(*commandData); -// uint8_t tpc = *(commandData + 1); -// RequestLoggingData packet(spParams); -// ReturnValue_t result = packet.buildPacket(sa, tpc); -// if (result != returnvalue::OK) { -// return result; -// } -// finishTcPrep(packet.getFullPacketLen()); -// return returnvalue::OK; -// } - -// ReturnValue_t PlocSupervisorHandler::prepareEnableNvmsCommand(const uint8_t* commandData) { -// using namespace supv; -// uint8_t nvm01 = *(commandData); -// uint8_t nvm3 = *(commandData + 1); -// EnableNvms packet(spParams); -// ReturnValue_t result = packet.buildPacket(nvm01, nvm3); -// if (result != returnvalue::OK) { -// return result; -// } -// finishTcPrep(packet.getFullPacketLen()); -// return returnvalue::OK; -// } - void PlocSupervisorHandler::disableAllReplies() { using namespace supv; DeviceReplyMap::iterator iter; @@ -1726,41 +1658,6 @@ void PlocSupervisorHandler::disableExeReportReply() { info->command->second.expectedReplies = 1; } -// ReturnValue_t PlocSupervisorHandler::parseMramPackets(const uint8_t* packet, size_t -// remainingSize, -// size_t* foundLen) { -// ReturnValue_t result = IGNORE_FULL_PACKET; -// uint16_t packetLen = 0; -// *foundLen = 0; -// -// for (size_t idx = 0; idx < remainingSize; idx++) { -// std::memcpy(spacePacketBuffer + bufferTop, packet + idx, 1); -// bufferTop += 1; -// *foundLen += 1; -// if (bufferTop >= ccsds::HEADER_LEN) { -// packetLen = readSpacePacketLength(spacePacketBuffer); -// } -// -// if (bufferTop == ccsds::HEADER_LEN + packetLen + 1) { -// packetInBuffer = true; -// bufferTop = 0; -// return checkMramPacketApid(); -// } -// -// if (bufferTop == supv::MAX_PACKET_SIZE) { -// *foundLen = remainingSize; -// disableAllReplies(); -// bufferTop = 0; -// sif::info << "PlocSupervisorHandler::parseMramPackets: Can not find MRAM packet in space " -// "packet buffer" -// << std::endl; -// return result::MRAM_PACKET_PARSING_FAILURE; -// } -// } -// -// return result; -// } - ReturnValue_t PlocSupervisorHandler::handleMramDumpPacket(DeviceCommandId_t id) { ReturnValue_t result = returnvalue::FAILED; @@ -1837,15 +1734,6 @@ void PlocSupervisorHandler::increaseExpectedMramReplies(DeviceCommandId_t id) { return; } -// ReturnValue_t PlocSupervisorHandler::checkMramPacketApid() { -// uint16_t apid = (spacePacketBuffer[0] << 8 | spacePacketBuffer[1]) & supv::APID_MASK; -// TODO: Fix -// if (apid != supv::APID_MRAM_DUMP_TM) { -// return result::NO_MRAM_PACKET; -// } -// return APERIODIC_REPLY; -//} - ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) { #ifdef XIPHOS_Q7S if (not sdcMan->getActiveSdCard()) { @@ -2118,3 +2006,116 @@ ReturnValue_t PlocSupervisorHandler::getSwitches(const uint8_t** switches, uint32_t PlocSupervisorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 7000; } + +// ReturnValue_t PlocSupervisorHandler::checkMramPacketApid() { +// uint16_t apid = (spacePacketBuffer[0] << 8 | spacePacketBuffer[1]) & supv::APID_MASK; +// TODO: Fix +// if (apid != supv::APID_MRAM_DUMP_TM) { +// return result::NO_MRAM_PACKET; +// } +// return APERIODIC_REPLY; +//} + +// ReturnValue_t PlocSupervisorHandler::parseMramPackets(const uint8_t* packet, size_t +// remainingSize, +// size_t* foundLen) { +// ReturnValue_t result = IGNORE_FULL_PACKET; +// uint16_t packetLen = 0; +// *foundLen = 0; +// +// for (size_t idx = 0; idx < remainingSize; idx++) { +// std::memcpy(spacePacketBuffer + bufferTop, packet + idx, 1); +// bufferTop += 1; +// *foundLen += 1; +// if (bufferTop >= ccsds::HEADER_LEN) { +// packetLen = readSpacePacketLength(spacePacketBuffer); +// } +// +// if (bufferTop == ccsds::HEADER_LEN + packetLen + 1) { +// packetInBuffer = true; +// bufferTop = 0; +// return checkMramPacketApid(); +// } +// +// if (bufferTop == supv::MAX_PACKET_SIZE) { +// *foundLen = remainingSize; +// disableAllReplies(); +// bufferTop = 0; +// sif::info << "PlocSupervisorHandler::parseMramPackets: Can not find MRAM packet in space " +// "packet buffer" +// << std::endl; +// return result::MRAM_PACKET_PARSING_FAILURE; +// } +// } +// +// return result; +// } + +// ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandData) { +// uint32_t start = 0; +// uint32_t stop = 0; +// size_t size = sizeof(start) + sizeof(stop); +// SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); +// SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); +// if ((stop - start) <= 0) { +// return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; +// } +// supv::MramCmd packet(spParams); +// ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE); +// if (result != returnvalue::OK) { +// return result; +// } +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } + +// ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandData) { +// uint32_t start = 0; +// uint32_t stop = 0; +// size_t size = sizeof(start) + sizeof(stop); +// SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); +// SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); +// if ((stop - start) <= 0) { +// return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; +// } +// supv::MramCmd packet(spParams); +// ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::DUMP); +// if (result != returnvalue::OK) { +// return result; +// } +// expectedMramDumpPackets = (stop - start) / supv::MAX_DATA_CAPACITY; +// if ((stop - start) % supv::MAX_DATA_CAPACITY) { +// expectedMramDumpPackets++; +// } +// receivedMramDumpPackets = 0; +// +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } + +// ReturnValue_t PlocSupervisorHandler::prepareLoggingRequest(const uint8_t* commandData, +// size_t commandDataLen) { +// using namespace supv; +// RequestLoggingData::Sa sa = static_cast(*commandData); +// uint8_t tpc = *(commandData + 1); +// RequestLoggingData packet(spParams); +// ReturnValue_t result = packet.buildPacket(sa, tpc); +// if (result != returnvalue::OK) { +// return result; +// } +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } + +// ReturnValue_t PlocSupervisorHandler::prepareEnableNvmsCommand(const uint8_t* commandData) { +// using namespace supv; +// uint8_t nvm01 = *(commandData); +// uint8_t nvm3 = *(commandData + 1); +// EnableNvms packet(spParams); +// ReturnValue_t result = packet.buildPacket(nvm01, nvm3); +// if (result != returnvalue::OK) { +// return result; +// } +// finishTcPrep(packet.getFullPacketLen()); +// return returnvalue::OK; +// } From c42eae9c1735ba4fd73af0161ff43e6d973e8986 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 9 Nov 2022 19:24:48 +0100 Subject: [PATCH 119/244] re-enabled some commands --- .../PlocSupervisorDefinitions.h | 62 +++++++++---------- linux/devices/ploc/PlocSupervisorHandler.cpp | 52 ++++++++-------- linux/devices/ploc/PlocSupervisorHandler.h | 2 +- 3 files changed, 57 insertions(+), 59 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index fa4c728b..8a642647 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -882,6 +882,37 @@ class RunAutoEmTests : public TcBase { void initPacket(uint8_t test) { payloadStart[0] = test; } }; +/** + * @brief This class packages the space packet to enable or disable ADC channels. + */ +class SetAdcEnabledChannels : public TcBase { + public: + /** + * @brief Constructor + * + * @param ch Defines channels to be enabled or disabled. + */ + SetAdcEnabledChannels(TcParams params) + : TcBase(params, Apid::ADC_MON, static_cast(tc::AdcMonId::SET_ENABLED_CHANNELS), 2) { + } + + ReturnValue_t buildPacket(uint16_t ch) { + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + initPacket(ch); + return calcAndSetCrc(); + } + + private: + void initPacket(uint16_t ch) { + size_t serializedSize = 0; + SerializeAdapter::serialize(&ch, &payloadStart, &serializedSize, sizeof(ch), + SerializeIF::Endianness::BIG); + } +}; + /** * @brief This class packages the space packet change the state of a GPIO. This command is only * required for ground testing. @@ -2000,37 +2031,6 @@ class DisableAutoTm : public TcBase { static const uint8_t DISABLE = 0; }; -/** - * @brief This class packages the space packet to enable or disable ADC channels. - */ -class SetAdcEnabledChannels : public TcBase { - public: - /** - * @brief Constructor - * - * @param ch Defines channels to be enabled or disabled. - */ - SetAdcEnabledChannels(TcParams params) - : TcBase(params, Apid::ADC_MON, static_cast(tc::AdcMonId::SET_ENABLED_CHANNELS), 2) { - } - - ReturnValue_t buildPacket(uint16_t ch) { - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - initPacket(ch); - return calcAndSetCrc(); - } - - private: - void initPacket(uint16_t ch) { - size_t serializedSize = 0; - SerializeAdapter::serialize(&ch, &payloadStart, &serializedSize, sizeof(ch), - SerializeIF::Endianness::BIG); - } -}; - /** * @brief This class creates the space packet to request the logging data from the supervisor */ diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 5ea9d539..dbb86ac2 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -299,6 +299,21 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d prepareEmptyCmd(Apid::BOOT_MAN, static_cast(tc::BootManId::RESET_PL)); result = returnvalue::OK; break; + } + case SET_ADC_ENABLED_CHANNELS: { + prepareSetAdcEnabledChannelsCmd(commandData); + result = returnvalue::OK; + break; + } + case SET_ADC_WINDOW_AND_STRIDE: { + prepareSetAdcWindowAndStrideCmd(commandData); + result = returnvalue::OK; + break; + } + case SET_ADC_THRESHOLD: { + prepareSetAdcThresholdCmd(commandData); + result = returnvalue::OK; + break; } // case ENABLE_NVMS: { // result = prepareEnableNvmsCommand(commandData); @@ -342,22 +357,6 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // result = returnvalue::OK; // break; // } - // case SET_ADC_ENABLED_CHANNELS: { - - // prepareSetAdcEnabledChannelsCmd(commandData); - // result = returnvalue::OK; - // break; - // } - // case SET_ADC_WINDOW_AND_STRIDE: { - // prepareSetAdcWindowAndStrideCmd(commandData); - // result = returnvalue::OK; - // break; - // } - // case SET_ADC_THRESHOLD: { - // prepareSetAdcThresholdCmd(commandData); - // result = returnvalue::OK; - // break; - // } // case ENABLE_AUTO_TM: { // EnableAutoTm packet(spParams); // result = packet.buildPacket(); @@ -1452,17 +1451,16 @@ ReturnValue_t PlocSupervisorHandler::prepareSetAlertLimitCmd(const uint8_t* comm return returnvalue::OK; } -// ReturnValue_t PlocSupervisorHandler::prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData) -// { -// uint16_t ch = *(commandData) << 8 | *(commandData + 1); -// supv::SetAdcEnabledChannels packet(spParams); -// ReturnValue_t result = packet.buildPacket(ch); -// if (result != returnvalue::OK) { -// return result; -// } -// finishTcPrep(packet.getFullPacketLen()); -// return returnvalue::OK; -// } +ReturnValue_t PlocSupervisorHandler::prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData) { + uint16_t ch = *(commandData) << 8 | *(commandData + 1); + supv::SetAdcEnabledChannels packet(spParams); + ReturnValue_t result = packet.buildPacket(ch); + if (result != returnvalue::OK) { + return result; + } + finishTcPrep(packet.getFullPacketLen()); + return returnvalue::OK; +} ReturnValue_t PlocSupervisorHandler::prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData) { uint8_t offset = 0; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 99b222c6..9480667c 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -281,7 +281,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t prepareLatchupConfigCmd(const uint8_t* commandData, DeviceCommandId_t deviceCommand); ReturnValue_t prepareSetAlertLimitCmd(const uint8_t* commandData); - // ReturnValue_t prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData); + ReturnValue_t prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData); ReturnValue_t prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData); ReturnValue_t prepareSetAdcThresholdCmd(const uint8_t* commandData); ReturnValue_t prepareRunAutoEmTest(const uint8_t* commandData); From 9e006d93fe2e86961bbc744e1e1ae5850b546d5b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 9 Nov 2022 19:27:18 +0100 Subject: [PATCH 120/244] move other function to graveyard --- linux/devices/ploc/PlocSupervisorHandler.cpp | 72 ++++++++++---------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index dbb86ac2..86547b73 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -1156,42 +1156,6 @@ ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* da return result; } -// ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) { -// ReturnValue_t result = returnvalue::OK; -// -// result = verifyPacket(data, supv::SIZE_LOGGING_REPORT); -// -// if (result == SupvReturnValuesIF::CRC_FAILURE) { -// sif::warning << "PlocSupervisorHandler::handleLoggingReport: Logging report has " -// << "invalid crc" << std::endl; -// return result; -// } -// -// const uint8_t* dataField = data + supv::PAYLOAD_OFFSET + sizeof(supv::RequestLoggingData::Sa); -// result = loggingReport.read(); -// if (result != returnvalue::OK) { -// return result; -// } -// loggingReport.setValidityBufferGeneration(false); -// size_t size = loggingReport.getSerializedSize(); -// result = loggingReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG); -// if (result != returnvalue::OK) { -// sif::warning << "PlocSupervisorHandler::handleLoggingReport: Deserialization failed" -// << std::endl; -// } -// loggingReport.setValidityBufferGeneration(true); -// loggingReport.setValidity(true, true); -// result = loggingReport.commit(); -// if (result != returnvalue::OK) { -// return result; -// } -//#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1 -// loggingReport.printSet(); -//#endif -// nextReplyId = supv::EXE_REPORT; -// return result; -// } - ReturnValue_t PlocSupervisorHandler::handleAdcReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; @@ -2117,3 +2081,39 @@ uint32_t PlocSupervisorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t mod // finishTcPrep(packet.getFullPacketLen()); // return returnvalue::OK; // } + +// ReturnValue_t PlocSupervisorHandler::handleLoggingReport(const uint8_t* data) { +// ReturnValue_t result = returnvalue::OK; +// +// result = verifyPacket(data, supv::SIZE_LOGGING_REPORT); +// +// if (result == SupvReturnValuesIF::CRC_FAILURE) { +// sif::warning << "PlocSupervisorHandler::handleLoggingReport: Logging report has " +// << "invalid crc" << std::endl; +// return result; +// } +// +// const uint8_t* dataField = data + supv::PAYLOAD_OFFSET + sizeof(supv::RequestLoggingData::Sa); +// result = loggingReport.read(); +// if (result != returnvalue::OK) { +// return result; +// } +// loggingReport.setValidityBufferGeneration(false); +// size_t size = loggingReport.getSerializedSize(); +// result = loggingReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG); +// if (result != returnvalue::OK) { +// sif::warning << "PlocSupervisorHandler::handleLoggingReport: Deserialization failed" +// << std::endl; +// } +// loggingReport.setValidityBufferGeneration(true); +// loggingReport.setValidity(true, true); +// result = loggingReport.commit(); +// if (result != returnvalue::OK) { +// return result; +// } +//#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1 +// loggingReport.printSet(); +//#endif +// nextReplyId = supv::EXE_REPORT; +// return result; +// } From b88b4cc06d005fe52d8dfd12ab68acaaa14e29aa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 9 Nov 2022 19:47:42 +0100 Subject: [PATCH 121/244] tweaks and fixes for UART man --- linux/devices/ploc/PlocSupvUartMan.cpp | 105 +++++++++---------------- 1 file changed, 35 insertions(+), 70 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 622acc89..9630d674 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -65,7 +65,7 @@ ReturnValue_t PlocSupvHelper::initializeInterface(CookieIF* cookie) { // Use non-canonical mode and clear echo flag tty.c_lflag &= ~(ICANON | ECHO); - // Non-blocking mode, 0.5 seconds timeout + // Blocking mode, 0.5 seconds timeout tty.c_cc[VTIME] = 5; tty.c_cc[VMIN] = 0; @@ -311,12 +311,7 @@ void PlocSupvHelper::executeFullCheckMemoryCommand() { return; } sif::info << "PLOC SUPV Mem Check: Memory Check" << std::endl; - result = handleCheckMemoryCommand(); - if (result == returnvalue::OK) { - triggerEvent(SUPV_MEM_CHECK_OK, result); - } else { - triggerEvent(SUPV_MEM_CHECK_FAIL, result); - } + handleCheckMemoryCommand(); } ReturnValue_t PlocSupvHelper::executeUpdate() { @@ -434,6 +429,7 @@ ReturnValue_t PlocSupvHelper::writeUpdatePackets() { #if OBSW_DEBUG_PLOC_SUPERVISOR == 1 progressPrinter.print(update.bytesWritten); #endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ + TaskFactory::delayTask(1); } return result; } @@ -552,7 +548,11 @@ ReturnValue_t PlocSupvHelper::handlePacketTransmissionNoReply(supv::TcBase& pack } Countdown countdown(timeoutExecutionReport); bool ackReceived = false; + bool packetWasHandled = false; while (true) { + while (result != NO_PACKET_FOUND) { + result = tryHdlcParsing(); + } if (not decodedQueue.empty()) { size_t packetLen = 0; decodedQueue.retrieve(&packetLen); @@ -569,6 +569,7 @@ ReturnValue_t PlocSupvHelper::handlePacketTransmissionNoReply(supv::TcBase& pack retval = handleAckReception(packet, serviceId, packetLen); if (retval == 1) { ackReceived = true; + packetWasHandled = true; } else if (retval == -1) { return returnvalue::FAILED; } @@ -580,7 +581,8 @@ ReturnValue_t PlocSupvHelper::handlePacketTransmissionNoReply(supv::TcBase& pack return returnvalue::FAILED; } } - } else { + } + if (not packetWasHandled) { pushIpcData(decodedBuf.data(), packetLen); decodedRingBuf.deleteData(packetLen); } @@ -601,7 +603,7 @@ int PlocSupvHelper::handleAckReception(supv::TcBase& tc, uint8_t serviceId, size ReturnValue_t result = ackReport.parse(); if (result != returnvalue::OK) { triggerEvent(ACK_RECEPTION_FAILURE); - return returnvalue::FAILED; + return -1; } if (ackReport.getRefApid() == tc.getApid() and ackReport.getRefServiceId() == tc.getServiceId()) { @@ -631,7 +633,7 @@ int PlocSupvHelper::handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, s ReturnValue_t result = exeReport.parse(); if (result != returnvalue::OK) { triggerEvent(EXE_RECEPTION_FAILURE); - return returnvalue::FAILED; + return -1; } if (exeReport.getRefApid() == tc.getApid() and exeReport.getRefServiceId() == tc.getServiceId()) { @@ -729,7 +731,12 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { Countdown countdown(timeout::CRC_EXECUTION_TIMEOUT); bool ackReceived = false; bool checkReplyReceived = false; + bool packetWasHandled = false; + bool exeReceived = false; while (true) { + while (result != NO_PACKET_FOUND) { + result = tryHdlcParsing(); + } if (not decodedQueue.empty()) { size_t packetLen = 0; decodedQueue.retrieve(&packetLen); @@ -739,19 +746,23 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { if (result != returnvalue::OK) { continue; } + packetWasHandled = false; if (tmReader.getApid() == Apid::TMTC_MAN) { uint8_t serviceId = tmReader.getServiceId(); int retval = 0; if (not ackReceived) { retval = handleAckReception(packet, serviceId, packetLen); if (retval == 1) { + packetWasHandled = true; ackReceived = true; } else if (retval == -1) { return returnvalue::FAILED; } - } else if (checkReplyReceived) { + } else { retval = handleExeAckReception(packet, serviceId, packetLen); if (retval == 1) { + packetWasHandled = true; + exeReceived = true; break; } else if (retval == -1) { return returnvalue::FAILED; @@ -764,86 +775,40 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { if (result != returnvalue::OK) { return result; } + packetWasHandled = true; + checkReplyReceived = true; if (update.crcShouldBeChecked) { result = report.verifyCrc(update.crc); - if (result != returnvalue::OK) { + if (result == returnvalue::OK) { + triggerEvent(SUPV_MEM_CHECK_OK, result); + return result; + } else { sif::warning + << "PlocSupvHelper::handleCheckMemoryCommand: CRC failure. Expected CRC 0x" << std::setfill('0') << std::hex << std::setw(4) << static_cast(update.crc) << " but received CRC 0x" << std::setw(4) << report.getCrc() << std::dec << std::endl; - return result; + triggerEvent(SUPV_MEM_CHECK_FAIL, result); } } - checkReplyReceived = true; } - } else { + } + if (not packetWasHandled) { pushIpcData(decodedBuf.data(), packetLen); decodedRingBuf.deleteData(packetLen); } } else { TaskFactory::delayTask(50); } + if (ackReceived and exeReceived and checkReplyReceived) { + break; + } if (countdown.hasTimedOut()) { return result::NO_REPLY_TIMEOUT; } } return returnvalue::OK; - // result = handleAck(); - // if (result != returnvalue::OK) { - // return result; - // } - // - // bool exeAlreadyHandled = false; - // uint32_t timeout = std::max(CRC_EXECUTION_TIMEOUT, supv::timeout::UPDATE_STATUS_REPORT); - // result = handleTmReception(ccsds::HEADER_LEN, tmBuf.data(), timeout); - // ploc::SpTmReader spReader(tmBuf.data(), tmBuf.size()); - // if (spReader.getApid() == supv::APID_EXE_FAILURE) { - // exeAlreadyHandled = true; - // result = handleRemainingExeReport(spReader); - // } else if (spReader.getApid() == supv::APID_UPDATE_STATUS_REPORT) { - // size_t remBytes = spReader.getPacketDataLen() + 1; - // result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN, - // supv::timeout::UPDATE_STATUS_REPORT); - // if (result != returnvalue::OK) { - // sif::warning - // << "PlocSupvHelper::handleCheckMemoryCommand: Failed to receive update status report" - // << std::endl; - // return result; - // } - // result = updateStatusReport.checkCrc(); - // if (result != returnvalue::OK) { - // sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC check failed" << std::endl; - // return result; - // } - // // Copy into other buffer because data will be overwritten when reading execution report - // std::memcpy(statusReportBuf.data(), tmBuf.data(), updateStatusReport.getNominalSize()); - // } - // - // if (not exeAlreadyHandled) { - // result = handleExe(CRC_EXECUTION_TIMEOUT); - // if (result != returnvalue::OK) { - // return result; - // } - // } - // - // // Now process the status report - // updateStatusReport.setData(statusReportBuf.data(), statusReportBuf.size()); - // result = updateStatusReport.parseDataField(); - // if (result != returnvalue::OK) { - // return result; - // } - // if (update.crcShouldBeChecked) { - // result = updateStatusReport.verifycrc(update.crc); - // if (result != returnvalue::OK) { - // sif::warning << "PlocSupvHelper::handleCheckMemoryCommand: CRC failure. Expected CRC 0x" - // << std::setfill('0') << std::hex << std::setw(4) - // << static_cast(update.crc) << " but received CRC 0x" << - // std::setw(4) - // << updateStatusReport.getCrc() << std::dec << std::endl; - // return result; - // } - // } return result; } From 3be45b32ef166456b6d1c2c4073388a2198957c8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 11:20:27 +0100 Subject: [PATCH 122/244] renaming --- linux/devices/ploc/PlocSupvUartMan.cpp | 12 ++++++------ linux/devices/ploc/PlocSupvUartMan.h | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 9630d674..a60618e1 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -109,7 +109,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { putTaskToSleep = true; break; } - case InternalState::LONGER_REQUEST: { + case InternalState::DEDICATED_REQUEST: { if (handleRunningLongerRequest() == REQUEST_DONE) { MutexGuard mg(lock); state = InternalState::DEFAULT; @@ -219,7 +219,7 @@ ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { update.packetNum = 1; update.deleteMemory = params.deleteMemory; update.sequenceCount = params.seqCount; - state = InternalState::LONGER_REQUEST; + state = InternalState::DEDICATED_REQUEST; request = Request::UPDATE; } return result; @@ -243,7 +243,7 @@ ReturnValue_t PlocSupvHelper::performMemCheck(std::string file, uint8_t memoryId MutexGuard mg(lock); update.file = file; update.fullFileSize = getFileSize(file); - state = InternalState::LONGER_REQUEST; + state = InternalState::DEDICATED_REQUEST; request = Request::CHECK_MEMORY; update.memoryId = memoryId; update.startAddress = startAddress; @@ -261,7 +261,7 @@ ReturnValue_t PlocSupvHelper::initiateUpdateContinuation() { return HasActionsIF::IS_BUSY; } MutexGuard mg(lock); - state = InternalState::LONGER_REQUEST; + state = InternalState::DEDICATED_REQUEST; request = Request::CONTINUE_UPDATE; return returnvalue::OK; } @@ -884,7 +884,7 @@ ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendD return FAILED; } lock->lockMutex(); - if (state == InternalState::SLEEPING or state == InternalState::LONGER_REQUEST) { + if (state == InternalState::SLEEPING or state == InternalState::DEDICATED_REQUEST) { lock->unlockMutex(); return FAILED; } @@ -979,7 +979,7 @@ ReturnValue_t PlocSupvHelper::tryHdlcParsing() { ReturnValue_t result = parseRecRingBufForHdlc(bytesRead); if (result == returnvalue::OK) { // Packet found, advance read pointer. - if (state == InternalState::LONGER_REQUEST) { + if (state == InternalState::DEDICATED_REQUEST) { decodedRingBuf.writeData(decodedBuf.data(), bytesRead); decodedQueue.insert(bytesRead); } else { diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index f8f175d5..30d5fe1f 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -214,7 +214,7 @@ class PlocSupvHelper : public DeviceCommunicationIF, EventBufferRequest eventBufferReq; - enum class InternalState { SLEEPING, DEFAULT, LONGER_REQUEST, GO_TO_SLEEP }; + enum class InternalState { SLEEPING, DEFAULT, DEDICATED_REQUEST, GO_TO_SLEEP }; enum class Request { DEFAULT, From a2b0e53301967c4036fe5053b9848978197c7640 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 13:08:17 +0100 Subject: [PATCH 123/244] bugfix SW update --- linux/devices/ploc/PlocSupvUartMan.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index a60618e1..d0c96e93 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -416,12 +416,14 @@ ReturnValue_t PlocSupvHelper::writeUpdatePackets() { update.bytesWritten); return result; } - result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); + result = handlePacketTransmissionNoReply(packet); + //result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); if (result != returnvalue::OK) { triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount), update.bytesWritten); return result; } + update.sequenceCount++; update.packetNum += 1; update.bytesWritten += dataLength; From 09ccb5ddf723974d4590ccabcc74b151b041e487 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 13:50:18 +0100 Subject: [PATCH 124/244] re-enable wipe cmd --- .../PlocSupervisorDefinitions.h | 100 +++++++++--------- linux/devices/ploc/PlocSupervisorHandler.cpp | 46 ++++---- linux/devices/ploc/PlocSupervisorHandler.h | 2 +- linux/devices/ploc/PlocSupvUartMan.cpp | 2 +- 4 files changed, 75 insertions(+), 75 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 8a642647..6901d8fe 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -1116,6 +1116,56 @@ class WriteMemory : public TcBase { } }; +/** + * @brief This class packages the space packet to wipe or dump parts of the MRAM. + */ +class MramCmd : public TcBase { + public: + enum class MramAction { WIPE, DUMP }; + + /** + * @brief Constructor + * + * @param start Start address of the MRAM section to wipe or dump + * @param stop End address of the MRAM section to wipe or dump + * @param action Dump or wipe MRAM + * + * @note The content at the stop address is excluded from the dump or wipe operation. + */ + MramCmd(TcParams params) : TcBase(params, Apid::DATA_LOGGER) { setLenFromPayloadLen(6); } + + ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { + if (action == MramAction::WIPE) { + setServiceId(static_cast(tc::DataLoggerServiceId::WIPE_MRAM)); + } else if (action == MramAction::DUMP) { + setServiceId(static_cast(tc::DataLoggerServiceId::DUMP_MRAM)); + } else { + sif::debug << "WipeMram: Invalid action specified"; + } + auto res = checkSizeAndSerializeHeader(); + if (res != returnvalue::OK) { + return res; + } + initPacket(start, stop); + return calcAndSetCrc(); + } + + private: + uint32_t start = 0; + uint32_t stop = 0; + + void initPacket(uint32_t start, uint32_t stop) { + uint8_t concatBuffer[6]; + concatBuffer[0] = static_cast(start >> 16); + concatBuffer[1] = static_cast(start >> 8); + concatBuffer[2] = static_cast(start); + concatBuffer[3] = static_cast(stop >> 16); + concatBuffer[4] = static_cast(stop >> 8); + concatBuffer[5] = static_cast(stop); + std::memcpy(payloadStart, concatBuffer, sizeof(concatBuffer)); + } +}; + /** * @brief This class can be used to package erase memory command */ @@ -1931,56 +1981,6 @@ class EnableNvms : public TcBase { } }; -/** - * @brief This class packages the space packet to wipe or dump parts of the MRAM. - */ -class MramCmd : public TcBase { - public: - enum class MramAction { WIPE, DUMP }; - - /** - * @brief Constructor - * - * @param start Start address of the MRAM section to wipe or dump - * @param stop End address of the MRAM section to wipe or dump - * @param action Dump or wipe MRAM - * - * @note The content at the stop address is excluded from the dump or wipe operation. - */ - MramCmd(TcParams params) : TcBase(params, Apid::DATA_LOGGER) { setLenFromPayloadLen(6); } - - ReturnValue_t buildPacket(uint32_t start, uint32_t stop, MramAction action) { - if (action == MramAction::WIPE) { - setServiceId(static_cast(tc::DataLoggerServiceId::WIPE_MRAM)); - } else if (action == MramAction::DUMP) { - setServiceId(static_cast(tc::DataLoggerServiceId::DUMP_MRAM)); - } else { - sif::debug << "WipeMram: Invalid action specified"; - } - auto res = checkSizeAndSerializeHeader(); - if (res != returnvalue::OK) { - return res; - } - initPacket(start, stop); - return calcAndSetCrc(); - } - - private: - uint32_t start = 0; - uint32_t stop = 0; - - void initPacket(uint32_t start, uint32_t stop) { - uint8_t concatBuffer[6]; - concatBuffer[0] = static_cast(start >> 16); - concatBuffer[1] = static_cast(start >> 8); - concatBuffer[2] = static_cast(start); - concatBuffer[3] = static_cast(stop >> 16); - concatBuffer[4] = static_cast(stop >> 8); - concatBuffer[5] = static_cast(stop); - std::memcpy(payloadStart, concatBuffer, sizeof(concatBuffer)); - } -}; - /** * @brief This class creates the space packet to enable the auto TM generation */ diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 86547b73..b6783449 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -417,10 +417,10 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // result = returnvalue::OK; // break; // } - // case WIPE_MRAM: { - // result = prepareWipeMramCmd(commandData); - // break; - // } + case WIPE_MRAM: { + result = prepareWipeMramCmd(commandData); + break; + } // case FIRST_MRAM_DUMP: // case CONSECUTIVE_MRAM_DUMP: // result = prepareDumpMramCmd(commandData); @@ -562,7 +562,7 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite case SET_ADC_THRESHOLD: // case COPY_ADC_DATA_TO_MRAM: case RUN_AUTO_EM_TESTS: - // case WIPE_MRAM: + case WIPE_MRAM: case SET_GPIO: case FACTORY_RESET: case READ_GPIO: @@ -1725,6 +1725,24 @@ ReturnValue_t PlocSupervisorHandler::handleMramDumpFile(DeviceCommandId_t id) { return returnvalue::OK; } +ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandData) { + uint32_t start = 0; + uint32_t stop = 0; + size_t size = sizeof(start) + sizeof(stop); + SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); + SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); + if ((stop - start) <= 0) { + return result::INVALID_MRAM_ADDRESSES; + } + supv::MramCmd packet(spParams); + ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE); + if (result != returnvalue::OK) { + return result; + } + finishTcPrep(packet.getFullPacketLen()); + return returnvalue::OK; +} + uint16_t PlocSupervisorHandler::readSpacePacketLength(uint8_t* spacePacket) { return spacePacket[4] << 8 | spacePacket[5]; } @@ -2013,24 +2031,6 @@ uint32_t PlocSupervisorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t mod // return result; // } -// ReturnValue_t PlocSupervisorHandler::prepareWipeMramCmd(const uint8_t* commandData) { -// uint32_t start = 0; -// uint32_t stop = 0; -// size_t size = sizeof(start) + sizeof(stop); -// SerializeAdapter::deSerialize(&start, &commandData, &size, SerializeIF::Endianness::BIG); -// SerializeAdapter::deSerialize(&stop, &commandData, &size, SerializeIF::Endianness::BIG); -// if ((stop - start) <= 0) { -// return SupvReturnValuesIF::INVALID_MRAM_ADDRESSES; -// } -// supv::MramCmd packet(spParams); -// ReturnValue_t result = packet.buildPacket(start, stop, supv::MramCmd::MramAction::WIPE); -// if (result != returnvalue::OK) { -// return result; -// } -// finishTcPrep(packet.getFullPacketLen()); -// return returnvalue::OK; -// } - // ReturnValue_t PlocSupervisorHandler::prepareDumpMramCmd(const uint8_t* commandData) { // uint32_t start = 0; // uint32_t stop = 0; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 9480667c..174ffa9e 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -285,7 +285,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { ReturnValue_t prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData); ReturnValue_t prepareSetAdcThresholdCmd(const uint8_t* commandData); ReturnValue_t prepareRunAutoEmTest(const uint8_t* commandData); - // ReturnValue_t prepareWipeMramCmd(const uint8_t* commandData); + ReturnValue_t prepareWipeMramCmd(const uint8_t* commandData); // ReturnValue_t prepareDumpMramCmd(const uint8_t* commandData); ReturnValue_t prepareSetGpioCmd(const uint8_t* commandData); ReturnValue_t prepareReadGpioCmd(const uint8_t* commandData); diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index d0c96e93..3bb6e764 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -417,7 +417,7 @@ ReturnValue_t PlocSupvHelper::writeUpdatePackets() { return result; } result = handlePacketTransmissionNoReply(packet); - //result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); + // result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); if (result != returnvalue::OK) { triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount), update.bytesWritten); From e7005c18a4a3d58c93590fb7c032ec9840cb4e5d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 15:58:48 +0100 Subject: [PATCH 125/244] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 177c39dd..194b3e10 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 177c39dd53198a1b05e2f40fc3c5e88e7f7c2e0b +Subproject commit 194b3e100a036b1fef22566693549b327d48c6a5 From 5ed0ade358447e54b9f3362258b8fb084fb5e6cb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 16:23:51 +0100 Subject: [PATCH 126/244] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 194b3e10..2a203ae1 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 194b3e100a036b1fef22566693549b327d48c6a5 +Subproject commit 2a203ae13d0b872189c0a79147c92312a44c08b1 From ca9ef281996a88f0fa45ada816fcfe420087660f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 10 Nov 2022 16:31:59 +0100 Subject: [PATCH 127/244] dummy stuff --- dummies/GpsDummy.cpp | 4 ++-- dummies/SusDummy.cpp | 22 ++++++++++++---------- dummies/SusDummy.h | 2 +- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/dummies/GpsDummy.cpp b/dummies/GpsDummy.cpp index d1f526d2..893e8464 100644 --- a/dummies/GpsDummy.cpp +++ b/dummies/GpsDummy.cpp @@ -37,8 +37,8 @@ uint32_t GpsDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t GpsDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry({0.0})); - localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry({0.0})); + localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry({0.0}, 1)); + localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry({0.0}, 1)); localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry({0.0})); localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry({7684.2})); localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry({0})); diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 706f08f0..46b2ec68 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -1,5 +1,7 @@ + #include "SusDummy.h" +#include #include #include @@ -28,7 +30,6 @@ ReturnValue_t SusDummy::initialize() { return result; } } - return returnvalue::OK; } @@ -37,29 +38,30 @@ ReturnValue_t SusDummy::handleCommandMessage(CommandMessage* message) { } void SusDummy::performControlOperation() { - iteration++; - value = sin(iteration / 80. * M_PI + 10) * 10 - 10; + // value = sin(iteration / 80. * M_PI + 10) * 10 - 10; susSet.read(); -// susSet.temperatureCelcius = value; -// if ((iteration % 100) < 20) { -// susSet.setValidity(false, true); -// } else { -// susSet.setValidity(true, true); -// } + // susSet.temperatureCelcius = value; + // if ((iteration % 100) < 20) { + // susSet.setValidity(false, true); + // } else { + // susSet.setValidity(true, true); + // } susSet.channels[0] = 3913; susSet.channels[1] = 3912; susSet.channels[2] = 3799; susSet.channels[3] = 3797; susSet.channels[4] = 4056; + susSet.setValidity(true, true); susSet.commit(); + iteration++; } ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, - new PoolEntry({0, 0, 0, 0, 0, 0},true)); + new PoolEntry({0, 0, 0, 0, 0, 0}, true)); return returnvalue::OK; } diff --git a/dummies/SusDummy.h b/dummies/SusDummy.h index bdabaafc..3e24f368 100644 --- a/dummies/SusDummy.h +++ b/dummies/SusDummy.h @@ -24,4 +24,4 @@ class SusDummy : public ExtendedControllerBase { int iteration = 0; float value = 0; SUS::SusDataset susSet; -}; \ No newline at end of file +}; From ec7ce5ddb6b3b7d21e157c9316e42dbcbffc97d9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 17:32:11 +0100 Subject: [PATCH 128/244] ploc components now compile for hosted (linux) --- CMakeLists.txt | 6 ++- bsp_hosted/ObjectFactory.cpp | 44 ++++++++++++++++++- bsp_linux_board/CMakeLists.txt | 1 + .../fsfwconfig/CMakeLists.txt | 0 .../fsfwconfig/FSFWConfig.h.in | 0 .../fsfwconfig/events/subsystemIdRanges.h | 0 .../fsfwconfig/events/translateEvents.cpp | 0 .../fsfwconfig/events/translateEvents.h | 0 .../fsfwconfig/ipc/MissionMessageTypes.cpp | 0 .../fsfwconfig/ipc/MissionMessageTypes.h | 0 .../fsfwconfig/objects/systemObjectList.h | 2 - .../fsfwconfig/objects/translateObjects.cpp | 0 .../fsfwconfig/objects/translateObjects.h | 0 .../pollingSequenceFactory.cpp | 0 .../pollingsequence/pollingSequenceFactory.h | 0 .../fsfwconfig/returnvalues/classIds.h | 0 bsp_q7s/core/ObjectFactory.cpp | 1 - common/config/devConf.h | 3 +- .../config}/devices/addresses.h | 0 common/config/eive/objects.h | 4 ++ fsfw | 2 +- linux/CMakeLists.txt | 1 - linux/InitMission.cpp | 1 + linux/ObjectFactory.cpp | 2 +- linux/boardtest/SpiTestClass.cpp | 28 +++++++----- linux/boardtest/UartTestClass.cpp | 3 +- linux/boardtest/UartTestClass.h | 2 +- linux/devices/ScexUartReader.cpp | 2 +- .../devicedefinitions/MPSoCReturnValuesIF.h | 1 + .../PlocSupervisorDefinitions.h | 4 +- linux/devices/ploc/PlocMPSoCHandler.cpp | 2 +- linux/devices/ploc/PlocMPSoCHandler.h | 5 ++- linux/devices/ploc/PlocMPSoCHelper.cpp | 2 +- linux/devices/ploc/PlocMPSoCHelper.h | 5 ++- linux/devices/ploc/PlocMemoryDumper.h | 7 ++- linux/devices/ploc/PlocSupervisorHandler.cpp | 4 +- linux/devices/ploc/PlocSupervisorHandler.h | 13 +++--- linux/devices/ploc/PlocSupvUartMan.cpp | 3 +- linux/devices/ploc/PlocSupvUartMan.h | 2 +- .../devices/startracker/ArcsecDatalinkLayer.h | 1 + .../devices/startracker/ArcsecJsonParamBase.h | 1 + .../startracker/StarTrackerHandler.cpp | 2 +- linux/devices/startracker/StrHelper.cpp | 2 +- linux/devices/startracker/StrHelper.h | 5 ++- linux/fsfwconfig/devices/addresses.cpp | 1 - 45 files changed, 116 insertions(+), 46 deletions(-) rename {linux => bsp_linux_board}/fsfwconfig/CMakeLists.txt (100%) rename {linux => bsp_linux_board}/fsfwconfig/FSFWConfig.h.in (100%) rename {linux => bsp_linux_board}/fsfwconfig/events/subsystemIdRanges.h (100%) rename {linux => bsp_linux_board}/fsfwconfig/events/translateEvents.cpp (100%) rename {linux => bsp_linux_board}/fsfwconfig/events/translateEvents.h (100%) rename {linux => bsp_linux_board}/fsfwconfig/ipc/MissionMessageTypes.cpp (100%) rename {linux => bsp_linux_board}/fsfwconfig/ipc/MissionMessageTypes.h (100%) rename {linux => bsp_linux_board}/fsfwconfig/objects/systemObjectList.h (96%) rename {linux => bsp_linux_board}/fsfwconfig/objects/translateObjects.cpp (100%) rename {linux => bsp_linux_board}/fsfwconfig/objects/translateObjects.h (100%) rename {linux => bsp_linux_board}/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp (100%) rename {linux => bsp_linux_board}/fsfwconfig/pollingsequence/pollingSequenceFactory.h (100%) rename {linux => bsp_linux_board}/fsfwconfig/returnvalues/classIds.h (100%) rename {linux/fsfwconfig => common/config}/devices/addresses.h (100%) delete mode 100644 linux/fsfwconfig/devices/addresses.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 644b8bcd..f940ea0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -233,7 +233,11 @@ set(LIB_ARCSEC_PATH ${THIRD_PARTY_FOLDER}/arcsec_star_tracker) set(LIB_JSON_PATH ${THIRD_PARTY_FOLDER}/json) set(FSFW_WARNING_SHADOW_LOCAL_GCC OFF) -set(EIVE_ADD_LINUX_FILES False) +set(EIVE_ADD_LINUX_FILES OFF) + +if(UNIX) + set(EIVE_ADD_LINUX_FILES ON) +endif() # Analyse different OS and architecture/target options, determine BSP_PATH, # display information about compiler etc. diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index a3c94fe1..7e7bda14 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -6,11 +6,13 @@ #include #include #include -#include #include "OBSWConfig.h" +#include "devConf.h" #include "eive/definitions.h" +#include "fsfw/platform.h" #include "fsfw_tests/integration/task/TestTask.h" +#include "tmtc/pusIds.h" #if OBSW_USE_TMTC_TCP_BRIDGE == 0 #include "fsfw/osal/common/UdpTcPollingTask.h" @@ -47,6 +49,19 @@ #include "dummies/helpers.h" #include "mission/utility/GlobalConfigHandler.h" +#ifdef PLATFORM_UNIX +#include +#include + +#include "devices/gpioIds.h" +#include "fsfw_hal/linux/gpio/Gpio.h" +#include "linux/devices/ploc/PlocMPSoCHandler.h" +#include "linux/devices/ploc/PlocMPSoCHelper.h" +#include "linux/devices/ploc/PlocSupervisorHandler.h" +#include "linux/devices/ploc/PlocSupvUartMan.h" +#include "test/gpio/DummyGpioIF.h" +#endif + void Factory::setStaticFrameworkObjectIds() { PusServiceBase::PUS_DISTRIBUTOR = objects::PUS_PACKET_DISTRIBUTOR; PusServiceBase::PACKET_DESTINATION = objects::TM_FUNNEL; @@ -63,6 +78,33 @@ void ObjectFactory::produce(void* args) { CfdpTmFunnel* cfdpFunnel; ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel); + DummyGpioIF* dummyGpioIF = new DummyGpioIF(); + static_cast(dummyGpioIF); +#ifdef PLATFORM_UNIX + new SerialComIF(objects::UART_COM_IF); +#if OBSW_ADD_PLOC_MPSOC == 1 + std::string mpscoDev = ""; + auto mpsocCookie = new UartCookie(objects::PLOC_MPSOC_HANDLER, mpscoDev, uart::PLOC_MPSOC_BAUD, + mpsoc::MAX_REPLY_SIZE, UartModes::NON_CANONICAL); + mpsocCookie->setNoFixedSizeReply(); + auto plocMpsocHelper = new PlocMPSoCHelper(objects::PLOC_MPSOC_HELPER); + new PlocMPSoCHandler(objects::PLOC_MPSOC_HANDLER, objects::UART_COM_IF, mpsocCookie, + plocMpsocHelper, Gpio(gpioIds::ENABLE_MPSOC_UART, dummyGpioIF), + objects::PLOC_SUPERVISOR_HANDLER); +#endif /* OBSW_ADD_PLOC_MPSOC == 1 */ +#if OBSW_ADD_PLOC_SUPERVISOR == 1 + std::string plocSupvString = ""; + auto supervisorCookie = + new UartCookie(objects::PLOC_SUPERVISOR_HANDLER, plocSupvString, uart::PLOC_SUPV_BAUD, + supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); + supervisorCookie->setNoFixedSizeReply(); + auto supvHelper = new PlocSupvHelper(objects::PLOC_SUPERVISOR_HELPER); + new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, objects::PLOC_SUPERVISOR_HELPER, + supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, dummyGpioIF), + pcdu::PDU1_CH6_PLOC_12V, supvHelper); +#endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ +#endif + dummy::DummyCfg cfg; dummy::createDummies(cfg); new TemperatureSensorsDummy(); diff --git a/bsp_linux_board/CMakeLists.txt b/bsp_linux_board/CMakeLists.txt index c1817ac1..39f06401 100644 --- a/bsp_linux_board/CMakeLists.txt +++ b/bsp_linux_board/CMakeLists.txt @@ -3,3 +3,4 @@ target_sources(${OBSW_NAME} PUBLIC InitMission.cpp main.cpp gpioInit.cpp add_subdirectory(boardconfig) add_subdirectory(boardtest) +add_subdirectory(fsfwconfig) diff --git a/linux/fsfwconfig/CMakeLists.txt b/bsp_linux_board/fsfwconfig/CMakeLists.txt similarity index 100% rename from linux/fsfwconfig/CMakeLists.txt rename to bsp_linux_board/fsfwconfig/CMakeLists.txt diff --git a/linux/fsfwconfig/FSFWConfig.h.in b/bsp_linux_board/fsfwconfig/FSFWConfig.h.in similarity index 100% rename from linux/fsfwconfig/FSFWConfig.h.in rename to bsp_linux_board/fsfwconfig/FSFWConfig.h.in diff --git a/linux/fsfwconfig/events/subsystemIdRanges.h b/bsp_linux_board/fsfwconfig/events/subsystemIdRanges.h similarity index 100% rename from linux/fsfwconfig/events/subsystemIdRanges.h rename to bsp_linux_board/fsfwconfig/events/subsystemIdRanges.h diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/bsp_linux_board/fsfwconfig/events/translateEvents.cpp similarity index 100% rename from linux/fsfwconfig/events/translateEvents.cpp rename to bsp_linux_board/fsfwconfig/events/translateEvents.cpp diff --git a/linux/fsfwconfig/events/translateEvents.h b/bsp_linux_board/fsfwconfig/events/translateEvents.h similarity index 100% rename from linux/fsfwconfig/events/translateEvents.h rename to bsp_linux_board/fsfwconfig/events/translateEvents.h diff --git a/linux/fsfwconfig/ipc/MissionMessageTypes.cpp b/bsp_linux_board/fsfwconfig/ipc/MissionMessageTypes.cpp similarity index 100% rename from linux/fsfwconfig/ipc/MissionMessageTypes.cpp rename to bsp_linux_board/fsfwconfig/ipc/MissionMessageTypes.cpp diff --git a/linux/fsfwconfig/ipc/MissionMessageTypes.h b/bsp_linux_board/fsfwconfig/ipc/MissionMessageTypes.h similarity index 100% rename from linux/fsfwconfig/ipc/MissionMessageTypes.h rename to bsp_linux_board/fsfwconfig/ipc/MissionMessageTypes.h diff --git a/linux/fsfwconfig/objects/systemObjectList.h b/bsp_linux_board/fsfwconfig/objects/systemObjectList.h similarity index 96% rename from linux/fsfwconfig/objects/systemObjectList.h rename to bsp_linux_board/fsfwconfig/objects/systemObjectList.h index dba8c630..8f36b013 100644 --- a/linux/fsfwconfig/objects/systemObjectList.h +++ b/bsp_linux_board/fsfwconfig/objects/systemObjectList.h @@ -45,10 +45,8 @@ enum sourceObjects : uint32_t { ARDUINO_COM_IF = 0x49000000, CSP_COM_IF = 0x49050001, I2C_COM_IF = 0x49040002, - UART_COM_IF = 0x49030003, SPI_MAIN_COM_IF = 0x49020004, GPIO_IF = 0x49010005, - SCEX_UART_READER = 0x49010006, /* Custom device handler */ SPI_RW_COM_IF = 0x49020005, diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/bsp_linux_board/fsfwconfig/objects/translateObjects.cpp similarity index 100% rename from linux/fsfwconfig/objects/translateObjects.cpp rename to bsp_linux_board/fsfwconfig/objects/translateObjects.cpp diff --git a/linux/fsfwconfig/objects/translateObjects.h b/bsp_linux_board/fsfwconfig/objects/translateObjects.h similarity index 100% rename from linux/fsfwconfig/objects/translateObjects.h rename to bsp_linux_board/fsfwconfig/objects/translateObjects.h diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/bsp_linux_board/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp similarity index 100% rename from linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp rename to bsp_linux_board/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h b/bsp_linux_board/fsfwconfig/pollingsequence/pollingSequenceFactory.h similarity index 100% rename from linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h rename to bsp_linux_board/fsfwconfig/pollingsequence/pollingSequenceFactory.h diff --git a/linux/fsfwconfig/returnvalues/classIds.h b/bsp_linux_board/fsfwconfig/returnvalues/classIds.h similarity index 100% rename from linux/fsfwconfig/returnvalues/classIds.h rename to bsp_linux_board/fsfwconfig/returnvalues/classIds.h diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index bc46efb5..718594e1 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -593,7 +593,6 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwit auto* camSwitcher = new CamSwitcher(objects::CAM_SWITCHER, pwrSwitch, pcdu::PDU2_CH8_PAYLOAD_CAMERA); camSwitcher->connectModeTreeParent(satsystem::pl::SUBSYSTEM); - // camSwitcher-> #if OBSW_ADD_PLOC_MPSOC == 1 consumer << "0x" << std::hex << objects::PLOC_MPSOC_HANDLER; auto gpioConfigMPSoC = new GpiodRegularByLineName(q7s::gpioNames::ENABLE_MPSOC_UART, diff --git a/common/config/devConf.h b/common/config/devConf.h index 88e78e1e..5f8164f6 100644 --- a/common/config/devConf.h +++ b/common/config/devConf.h @@ -1,11 +1,12 @@ #ifndef COMMON_CONFIG_DEVCONF_H_ #define COMMON_CONFIG_DEVCONF_H_ +#include + #include #include "fsfw/timemanager/clockDefinitions.h" #include "fsfw_hal/linux/spi/spiDefinitions.h" -#include "fsfw_hal/linux/uart/UartCookie.h" /** * SPI configuration will be contained here to let the device handlers remain independent diff --git a/linux/fsfwconfig/devices/addresses.h b/common/config/devices/addresses.h similarity index 100% rename from linux/fsfwconfig/devices/addresses.h rename to common/config/devices/addresses.h diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h index 5b9ec8a0..e5192a82 100644 --- a/common/config/eive/objects.h +++ b/common/config/eive/objects.h @@ -16,6 +16,10 @@ enum commonObjects : uint32_t { PDEC_HANDLER = 0x50000700, CCSDS_HANDLER = 0x50000800, + /* 0x49 ('I') for Communication Interfaces **/ + UART_COM_IF = 0x49030003, + SCEX_UART_READER = 0x49010006, + /* 0x43 ('C') for Controllers */ THERMAL_CONTROLLER = 0x43400001, ACS_CONTROLLER = 0x43000002, diff --git a/fsfw b/fsfw index 2a203ae1..39946bff 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 2a203ae13d0b872189c0a79147c92312a44c08b1 +Subproject commit 39946bff58db7c5ac9016ca3156abb059560d9cb diff --git a/linux/CMakeLists.txt b/linux/CMakeLists.txt index d7e6ca93..7b99da46 100644 --- a/linux/CMakeLists.txt +++ b/linux/CMakeLists.txt @@ -3,7 +3,6 @@ add_subdirectory(utility) add_subdirectory(callbacks) add_subdirectory(boardtest) add_subdirectory(devices) -add_subdirectory(fsfwconfig) add_subdirectory(ipcore) target_sources(${OBSW_NAME} PUBLIC ObjectFactory.cpp InitMission.cpp) diff --git a/linux/InitMission.cpp b/linux/InitMission.cpp index 5bc8698c..4b7dc1f1 100644 --- a/linux/InitMission.cpp +++ b/linux/InitMission.cpp @@ -6,6 +6,7 @@ #include "OBSWConfig.h" #include "ObjectFactory.h" +#include "eive/objects.h" void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, PeriodicTaskIF*& scexReaderTask) { diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp index 2fe31372..21f884f9 100644 --- a/linux/ObjectFactory.cpp +++ b/linux/ObjectFactory.cpp @@ -5,9 +5,9 @@ #include #include #include +#include #include #include -#include #include #include #include diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index dc5a5e3b..ea314b4b 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -333,14 +333,12 @@ void SpiTestClass::performPeriodicMax1227Test() { } void SpiTestClass::performMax1227Test() { + std::string deviceName = ""; #ifdef XIPHOS_Q7S std::string deviceName = q7s::SPI_DEFAULT_DEV; #elif defined(RASPBERRY_PI) - std::string deviceName = ""; #elif defined(EGSE) - std::string deviceName = ""; #elif defined(TE0720_1CFA) - std::string deviceName = ""; #endif int fd = 0; UnixFileGuard fileHelper(deviceName, &fd, O_RDWR, "SpiComIF::initializeInterface"); @@ -381,8 +379,9 @@ void SpiTestClass::max1227RadSensorTest(int fd) { DiffSel::NONE_0); spiTransferStruct[0].len = 1; transfer(fd, gpioIds::CS_RAD_SENSOR); - max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 7, spiTransferStruct[0].len); - size_t tmpLen = spiTransferStruct[0].len; + size_t tmpSz = spiTransferStruct[0].len; + max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 7, tmpSz); + size_t tmpLen = tmpSz; spiTransferStruct[0].len = 1; transfer(fd, gpioIds::CS_RAD_SENSOR); std::memcpy(sendBuffer.data(), sendBuffer.data() + 1, tmpLen - 1); @@ -403,7 +402,8 @@ void SpiTestClass::max1227RadSensorTest(int fd) { for (int idx = 0; idx < 8; idx++) { sif::info << "ADC raw " << idx << ": " << adcRaw[idx] << std::endl; } - max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data(), spiTransferStruct[0].len); + tmpSz = spiTransferStruct[0].len; + max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data(), tmpSz); spiTransferStruct[0].len = 1; transfer(fd, gpioIds::CS_RAD_SENSOR); usleep(65); @@ -467,7 +467,8 @@ void SpiTestClass::max1227SusTest(int fd, SusTestCfg &cfg) { spiTransferStruct[0].len = 1; transfer(fd, cfg.gpioId); - max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 5, spiTransferStruct[0].len); + size_t tmpSz = spiTransferStruct[0].len; + max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 5, tmpSz); transfer(fd, cfg.gpioId); uint16_t adcRaw[6] = {}; adcRaw[0] = (recvBuffer[1] << 8) | recvBuffer[2]; @@ -552,10 +553,11 @@ void SpiTestClass::max1227PlPcduTest(int fd) { spiTransferStruct[0].len = 1; transfer(fd, gpioIds::PLPCDU_ADC_CS); uint8_t n = 11; - max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, spiTransferStruct[0].len); - size_t dummy = 0; + size_t tmpSz = spiTransferStruct[0].len; + max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, tmpSz); max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data() + spiTransferStruct[0].len, - dummy); + tmpSz); + spiTransferStruct[0].len = tmpSz; // + 1 to account for temp conversion byte spiTransferStruct[0].len += 1; transfer(fd, gpioIds::PLPCDU_ADC_CS); @@ -585,9 +587,11 @@ void SpiTestClass::max1227PlPcduTest(int fd) { spiTransferStruct[0].len = 1; transfer(fd, gpioIds::PLPCDU_ADC_CS); uint8_t n = 11; - max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, spiTransferStruct[0].len); + size_t tmpLen = spiTransferStruct[0].len; + max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, tmpLen); max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data() + spiTransferStruct[0].len, - spiTransferStruct[0].len); + tmpLen); + spiTransferStruct[0].len = tmpLen; transfer(fd, gpioIds::PLPCDU_ADC_CS); uint16_t adcRaw[n + 1] = {}; for (uint8_t idx = 0; idx < n + 1; idx++) { diff --git a/linux/boardtest/UartTestClass.cpp b/linux/boardtest/UartTestClass.cpp index 4f476d4b..1f707c8f 100644 --- a/linux/boardtest/UartTestClass.cpp +++ b/linux/boardtest/UartTestClass.cpp @@ -3,7 +3,7 @@ #include // Error integer and strerror() function #include // Contains file controls like O_RDWR #include -#include +#include #include #include #include @@ -13,6 +13,7 @@ #include #include "OBSWConfig.h" +#include "eive/objects.h" #include "fsfw/globalfunctions/CRC.h" #include "fsfw/globalfunctions/DleEncoder.h" #include "fsfw/globalfunctions/arrayprinter.h" diff --git a/linux/boardtest/UartTestClass.h b/linux/boardtest/UartTestClass.h index 14d74322..6304724b 100644 --- a/linux/boardtest/UartTestClass.h +++ b/linux/boardtest/UartTestClass.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include // Contains POSIX terminal control definitions #include diff --git a/linux/devices/ScexUartReader.cpp b/linux/devices/ScexUartReader.cpp index ca797e74..4606c921 100644 --- a/linux/devices/ScexUartReader.cpp +++ b/linux/devices/ScexUartReader.cpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include // write(), read(), close() #include // Error integer and strerror() function diff --git a/linux/devices/devicedefinitions/MPSoCReturnValuesIF.h b/linux/devices/devicedefinitions/MPSoCReturnValuesIF.h index 8a1085ca..032211e0 100644 --- a/linux/devices/devicedefinitions/MPSoCReturnValuesIF.h +++ b/linux/devices/devicedefinitions/MPSoCReturnValuesIF.h @@ -1,6 +1,7 @@ #ifndef MPSOC_RETURN_VALUES_IF_H_ #define MPSOC_RETURN_VALUES_IF_H_ +#include "eive/resultClassIds.h" #include "fsfw/returnvalues/returnvalue.h" class MPSoCReturnValuesIF { diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 6901d8fe..512a4950 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -8,8 +8,10 @@ #include #include #include -#include +#include + +#include "eive/resultClassIds.h" #include "mission/devices/devicedefinitions/SpBase.h" namespace supv { diff --git a/linux/devices/ploc/PlocMPSoCHandler.cpp b/linux/devices/ploc/PlocMPSoCHandler.cpp index a192365f..28513314 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.cpp +++ b/linux/devices/ploc/PlocMPSoCHandler.cpp @@ -31,7 +31,7 @@ ReturnValue_t PlocMPSoCHandler::initialize() { if (result != returnvalue::OK) { return result; } - uartComIf = dynamic_cast(communicationInterface); + uartComIf = dynamic_cast(communicationInterface); if (uartComIf == nullptr) { sif::warning << "PlocMPSoCHandler::initialize: Invalid uart com if" << std::endl; return ObjectManagerIF::CHILD_INIT_FAILED; diff --git a/linux/devices/ploc/PlocMPSoCHandler.h b/linux/devices/ploc/PlocMPSoCHandler.h index fca65c7b..bd996006 100644 --- a/linux/devices/ploc/PlocMPSoCHandler.h +++ b/linux/devices/ploc/PlocMPSoCHandler.h @@ -1,6 +1,8 @@ #ifndef BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ #define BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ +#include + #include #include "PlocMPSoCHelper.h" @@ -10,7 +12,6 @@ #include "fsfw/ipc/QueueFactory.h" #include "fsfw/tmtcservices/SourceSequenceCounter.h" #include "fsfw_hal/linux/gpio/Gpio.h" -#include "fsfw_hal/linux/uart/UartComIF.h" #include "linux/devices/devicedefinitions/MPSoCReturnValuesIF.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" @@ -123,7 +124,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF { */ DeviceCommandId_t nextReplyId = mpsoc::NONE; - UartComIF* uartComIf = nullptr; + SerialComIF* uartComIf = nullptr; PlocMPSoCHelper* plocMPSoCHelper = nullptr; Gpio uartIsolatorSwitch; diff --git a/linux/devices/ploc/PlocMPSoCHelper.cpp b/linux/devices/ploc/PlocMPSoCHelper.cpp index 980b8889..fcd5178b 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.cpp +++ b/linux/devices/ploc/PlocMPSoCHelper.cpp @@ -57,7 +57,7 @@ ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) { } ReturnValue_t PlocMPSoCHelper::setComIF(DeviceCommunicationIF* communicationInterface_) { - uartComIF = dynamic_cast(communicationInterface_); + uartComIF = dynamic_cast(communicationInterface_); if (uartComIF == nullptr) { sif::warning << "PlocMPSoCHelper::initialize: Invalid uart com if" << std::endl; return returnvalue::FAILED; diff --git a/linux/devices/ploc/PlocMPSoCHelper.h b/linux/devices/ploc/PlocMPSoCHelper.h index b669b51b..84a9f47f 100644 --- a/linux/devices/ploc/PlocMPSoCHelper.h +++ b/linux/devices/ploc/PlocMPSoCHelper.h @@ -1,6 +1,8 @@ #ifndef BSP_Q7S_DEVICES_PLOCMPSOCHELPER_H_ #define BSP_Q7S_DEVICES_PLOCMPSOCHELPER_H_ +#include + #include #include "fsfw/devicehandlers/CookieIF.h" @@ -9,7 +11,6 @@ #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tmtcservices/SourceSequenceCounter.h" -#include "fsfw_hal/linux/uart/UartComIF.h" #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/SdCardManager.h" @@ -136,7 +137,7 @@ class PlocMPSoCHelper : public SystemObject, public ExecutableObjectIF { * Communication interface of MPSoC responsible for low level access. Must be set by the * MPSoC Handler. */ - UartComIF* uartComIF = nullptr; + SerialComIF* uartComIF = nullptr; // Communication cookie. Must be set by the MPSoC Handler CookieIF* comCookie = nullptr; // Sequence count, must be set by Ploc MPSoC Handler diff --git a/linux/devices/ploc/PlocMemoryDumper.h b/linux/devices/ploc/PlocMemoryDumper.h index da72c558..4a4e05bf 100644 --- a/linux/devices/ploc/PlocMemoryDumper.h +++ b/linux/devices/ploc/PlocMemoryDumper.h @@ -5,7 +5,12 @@ #include #include "OBSWConfig.h" + +#ifdef XIPHOS_Q7S #include "bsp_q7s/fs/SdCardManager.h" +#endif + +#include "eive/eventSubsystemIds.h" #include "fsfw/action/ActionHelper.h" #include "fsfw/action/CommandActionHelper.h" #include "fsfw/action/CommandsActionsIF.h" @@ -13,7 +18,7 @@ #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" -#include "linux/fsfwconfig/objects/systemObjectList.h" +#include "objects/systemObjectList.h" /** * @brief Because the buffer of the linux tty driver is limited to 2 x 65535 bytes, this class is diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index b6783449..ddfd3733 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -49,7 +49,7 @@ ReturnValue_t PlocSupervisorHandler::initialize() { if (result != returnvalue::OK) { return result; } -#ifndef TE0720_1CFA +#ifdef XIPHOS_Q7S sdcMan = SdCardManager::instance(); #endif /* TE0720_1CFA */ if (supvHelper == nullptr) { @@ -1761,7 +1761,7 @@ ReturnValue_t PlocSupervisorHandler::createMramDumpFile() { std::string filename = "mram-dump--" + timeStamp + ".bin"; -#ifndef TE0720_1CFA +#ifdef XIPHOS_Q7S std::string currentMountPrefix = sdcMan->getCurrentMountPrefix(); #else std::string currentMountPrefix("/mnt/sd0"); diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 174ffa9e..a5b0a2ae 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -1,18 +1,21 @@ #ifndef MISSION_DEVICES_PLOCSUPERVISORHANDLER_H_ #define MISSION_DEVICES_PLOCSUPERVISORHANDLER_H_ +#include #include #include "OBSWConfig.h" -#include "bsp_q7s/fs/SdCardManager.h" #include "devices/powerSwitcherList.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/timemanager/Countdown.h" #include "fsfw_hal/linux/gpio/Gpio.h" #include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" -#include "fsfw_hal/linux/uart/UartComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" +#ifdef XIPHOS_Q7S +#include "bsp_q7s/fs/SdCardManager.h" +#endif + using supv::ExecutionReport; /** @@ -114,7 +117,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { */ DeviceCommandId_t nextReplyId = supv::NONE; - UartComIF* uartComIf = nullptr; + SerialComIF* uartComIf = nullptr; LinuxLibgpioIF* gpioComIF = nullptr; Gpio uartIsolatorSwitch; @@ -141,9 +144,9 @@ class PlocSupervisorHandler : public DeviceHandlerBase { /** This buffer is used to concatenate space packets received in two different read steps */ uint8_t spacePacketBuffer[supv::MAX_PACKET_SIZE]; -#ifndef TE0720_1CFA +#ifdef XIPHOS_Q7S SdCardManager* sdcMan = nullptr; -#endif /* BOARD_TE0720 == 0 */ +#endif // Path to supervisor specific files on SD card std::string supervisorFilePath = "ploc/supervisor"; diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 3bb6e764..26da089b 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -16,9 +16,10 @@ #include "bsp_q7s/fs/SdCardManager.h" #endif +#include + #include "fsfw/tasks/TaskFactory.h" #include "fsfw/timemanager/Countdown.h" -#include "fsfw_hal/linux/uart/helper.h" #include "mission/utility/Filenaming.h" #include "mission/utility/ProgressPrinter.h" #include "mission/utility/Timestamp.h" diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 30d5fe1f..d10a5373 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -2,6 +2,7 @@ #define BSP_Q7S_DEVICES_PLOCSUPVHELPER_H_ #include +#include #include #include @@ -13,7 +14,6 @@ #include "fsfw/osal/linux/BinarySemaphore.h" #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" -#include "fsfw_hal/linux/uart/UartComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" #ifdef XIPHOS_Q7S diff --git a/linux/devices/startracker/ArcsecDatalinkLayer.h b/linux/devices/startracker/ArcsecDatalinkLayer.h index ec8caed9..5681d3ca 100644 --- a/linux/devices/startracker/ArcsecDatalinkLayer.h +++ b/linux/devices/startracker/ArcsecDatalinkLayer.h @@ -1,6 +1,7 @@ #ifndef BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_ #define BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_ +#include "eive/resultClassIds.h" #include "fsfw/returnvalues/returnvalue.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" diff --git a/linux/devices/startracker/ArcsecJsonParamBase.h b/linux/devices/startracker/ArcsecJsonParamBase.h index 75b160b2..a7a4f421 100644 --- a/linux/devices/startracker/ArcsecJsonParamBase.h +++ b/linux/devices/startracker/ArcsecJsonParamBase.h @@ -5,6 +5,7 @@ #include #include +#include "eive/resultClassIds.h" #include "fsfw/returnvalues/returnvalue.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" diff --git a/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index ff632c99..bede0a45 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -1591,7 +1591,7 @@ void StarTrackerHandler::preparePowerRequest() { void StarTrackerHandler::prepareSwitchToBootloaderCmd() { uint32_t length = 0; - struct RebootActionRequest rebootReq; + struct RebootActionRequest rebootReq {}; arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length); dataLinkLayer.encodeFrame(commandBuffer, length); rawPacket = dataLinkLayer.getEncodedFrame(); diff --git a/linux/devices/startracker/StrHelper.cpp b/linux/devices/startracker/StrHelper.cpp index 1f6472bb..dcf56b59 100644 --- a/linux/devices/startracker/StrHelper.cpp +++ b/linux/devices/startracker/StrHelper.cpp @@ -85,7 +85,7 @@ ReturnValue_t StrHelper::performOperation(uint8_t operationCode) { } ReturnValue_t StrHelper::setComIF(DeviceCommunicationIF* communicationInterface_) { - uartComIF = dynamic_cast(communicationInterface_); + uartComIF = dynamic_cast(communicationInterface_); if (uartComIF == nullptr) { sif::warning << "StrHelper::initialize: Invalid uart com if" << std::endl; return returnvalue::FAILED; diff --git a/linux/devices/startracker/StrHelper.h b/linux/devices/startracker/StrHelper.h index a9c80d77..ab0498d9 100644 --- a/linux/devices/startracker/StrHelper.h +++ b/linux/devices/startracker/StrHelper.h @@ -10,12 +10,13 @@ #include "bsp_q7s/fs/SdCardManager.h" #endif +#include + #include "fsfw/devicehandlers/CookieIF.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/osal/linux/BinarySemaphore.h" #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/tasks/ExecutableObjectIF.h" -#include "fsfw_hal/linux/uart/UartComIF.h" extern "C" { #include "thirdparty/arcsec_star_tracker/client/generated/actionreq.h" @@ -255,7 +256,7 @@ class StrHelper : public SystemObject, public ExecutableObjectIF { * UART communication object responsible for low level access of star tracker * Must be set by star tracker handler */ - UartComIF* uartComIF = nullptr; + SerialComIF* uartComIF = nullptr; // Communication cookie. Must be set by the star tracker handler CookieIF* comCookie = nullptr; diff --git a/linux/fsfwconfig/devices/addresses.cpp b/linux/fsfwconfig/devices/addresses.cpp deleted file mode 100644 index 580818e0..00000000 --- a/linux/fsfwconfig/devices/addresses.cpp +++ /dev/null @@ -1 +0,0 @@ -#include "addresses.h" From 02df949a4806916fb247f8910c02ad9882459fc0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 17:42:19 +0100 Subject: [PATCH 129/244] this seems to work --- CMakeLists.txt | 7 +++---- bsp_q7s/core/ObjectFactory.cpp | 10 +++++----- bsp_q7s/core/ObjectFactory.h | 6 +++--- bsp_q7s/fmObjectFactory.cpp | 2 +- linux/CMakeLists.txt | 4 ++++ linux/boardtest/SpiTestClass.cpp | 2 +- {bsp_linux_board => linux}/fsfwconfig/CMakeLists.txt | 0 {bsp_linux_board => linux}/fsfwconfig/FSFWConfig.h.in | 0 .../fsfwconfig/events/subsystemIdRanges.h | 0 .../fsfwconfig/events/translateEvents.cpp | 0 .../fsfwconfig/events/translateEvents.h | 0 .../fsfwconfig/ipc/MissionMessageTypes.cpp | 0 .../fsfwconfig/ipc/MissionMessageTypes.h | 0 .../fsfwconfig/objects/systemObjectList.h | 0 .../fsfwconfig/objects/translateObjects.cpp | 0 .../fsfwconfig/objects/translateObjects.h | 0 .../pollingsequence/pollingSequenceFactory.cpp | 0 .../pollingsequence/pollingSequenceFactory.h | 0 .../fsfwconfig/returnvalues/classIds.h | 0 19 files changed, 17 insertions(+), 14 deletions(-) rename {bsp_linux_board => linux}/fsfwconfig/CMakeLists.txt (100%) rename {bsp_linux_board => linux}/fsfwconfig/FSFWConfig.h.in (100%) rename {bsp_linux_board => linux}/fsfwconfig/events/subsystemIdRanges.h (100%) rename {bsp_linux_board => linux}/fsfwconfig/events/translateEvents.cpp (100%) rename {bsp_linux_board => linux}/fsfwconfig/events/translateEvents.h (100%) rename {bsp_linux_board => linux}/fsfwconfig/ipc/MissionMessageTypes.cpp (100%) rename {bsp_linux_board => linux}/fsfwconfig/ipc/MissionMessageTypes.h (100%) rename {bsp_linux_board => linux}/fsfwconfig/objects/systemObjectList.h (100%) rename {bsp_linux_board => linux}/fsfwconfig/objects/translateObjects.cpp (100%) rename {bsp_linux_board => linux}/fsfwconfig/objects/translateObjects.h (100%) rename {bsp_linux_board => linux}/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp (100%) rename {bsp_linux_board => linux}/fsfwconfig/pollingsequence/pollingSequenceFactory.h (100%) rename {bsp_linux_board => linux}/fsfwconfig/returnvalues/classIds.h (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index f940ea0f..97b7601c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -235,10 +235,6 @@ set(LIB_JSON_PATH ${THIRD_PARTY_FOLDER}/json) set(FSFW_WARNING_SHADOW_LOCAL_GCC OFF) set(EIVE_ADD_LINUX_FILES OFF) -if(UNIX) - set(EIVE_ADD_LINUX_FILES ON) -endif() - # Analyse different OS and architecture/target options, determine BSP_PATH, # display information about compiler etc. pre_source_hw_os_config() @@ -257,12 +253,15 @@ if(TGT_BSP) set(FSFW_CONFIG_PATH "linux/fsfwconfig") if(NOT BUILD_Q7S_SIMPLE_MODE) set(EIVE_ADD_LINUX_FILES TRUE) + set(EIVE_ADD_LINUX_FSFWCONFIG TRUE) set(ADD_GOMSPACE_CSP TRUE) set(ADD_GOMSPACE_CLIENTS TRUE) set(FSFW_HAL_ADD_LINUX ON) set(FSFW_HAL_LINUX_ADD_LIBGPIOD ON) set(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS ON) endif() + elseif(UNIX) + set(EIVE_ADD_LINUX_FILES ON) endif() if(TGT_BSP MATCHES "arm/raspberrypi") diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 718594e1..07eb0d22 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -71,8 +71,8 @@ #include "fsfw_hal/linux/i2c/I2cCookie.h" #include "fsfw_hal/linux/spi/SpiComIF.h" #include "fsfw_hal/linux/spi/SpiCookie.h" -#include "fsfw_hal/linux/uart/UartComIF.h" -#include "fsfw_hal/linux/uart/UartCookie.h" +#include "fsfw_hal/linux/serial/SerialComIF.h" +#include "fsfw_hal/linux/serial/SerialCookie.h" #include "mission/core/GenericFactory.h" #include "mission/devices/ACUHandler.h" #include "mission/devices/BpxBatteryHandler.h" @@ -137,7 +137,7 @@ void ObjectFactory::createTmpComponents() { (void)tmp1075Handler_2; } -void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, UartComIF** uartComIF, +void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, SerialComIF** uartComIF, SpiComIF** spiMainComIF, I2cComIF** i2cComIF, SpiComIF** spiRWComIF) { if (gpioComIF == nullptr or uartComIF == nullptr or spiMainComIF == nullptr or @@ -150,7 +150,7 @@ void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, Ua /* Communication interfaces */ new CspComIF(objects::CSP_COM_IF); *i2cComIF = new I2cComIF(objects::I2C_COM_IF); - *uartComIF = new UartComIF(objects::UART_COM_IF); + *uartComIF = new SerialComIF(objects::UART_COM_IF); *spiMainComIF = new SpiComIF(objects::SPI_MAIN_COM_IF, q7s::SPI_DEFAULT_DEV, **gpioComIF); *spiRWComIF = new SpiComIF(objects::SPI_RW_COM_IF, q7s::SPI_RW_DEV, **gpioComIF); } @@ -230,7 +230,7 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF) return returnvalue::OK; } -void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComIF* uartComIF, +void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF* pwrSwitcher) { using namespace gpio; GpioCookie* gpioCookieAcsBoard = new GpioCookie(); diff --git a/bsp_q7s/core/ObjectFactory.h b/bsp_q7s/core/ObjectFactory.h index 40a4e2ed..c67353c4 100644 --- a/bsp_q7s/core/ObjectFactory.h +++ b/bsp_q7s/core/ObjectFactory.h @@ -9,7 +9,7 @@ #include class LinuxLibgpioIF; -class UartComIF; +class SerialComIF; class SpiComIF; class I2cComIF; class PowerSwitchIF; @@ -22,7 +22,7 @@ namespace ObjectFactory { void setStatics(); void produce(void* args); -void createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, UartComIF** uartComIF, +void createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, SerialComIF** uartComIF, SpiComIF** spiMainComIF, I2cComIF** i2cComIF, SpiComIF** spiRwComIF); void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher); @@ -30,7 +30,7 @@ void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF, PowerSwitchIF* pwrSwitcher); void createTmpComponents(); ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF); -void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComIF* uartComIF, +void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF, PowerSwitchIF* pwrSwitcher); void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable); void createImtqComponents(PowerSwitchIF* pwrSwitcher); diff --git a/bsp_q7s/fmObjectFactory.cpp b/bsp_q7s/fmObjectFactory.cpp index 6ac2498a..c56dcf8d 100644 --- a/bsp_q7s/fmObjectFactory.cpp +++ b/bsp_q7s/fmObjectFactory.cpp @@ -21,7 +21,7 @@ void ObjectFactory::produce(void* args) { ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel); LinuxLibgpioIF* gpioComIF = nullptr; - UartComIF* uartComIF = nullptr; + SerialComIF* uartComIF = nullptr; SpiComIF* spiMainComIF = nullptr; I2cComIF* i2cComIF = nullptr; PowerSwitchIF* pwrSwitcher = nullptr; diff --git a/linux/CMakeLists.txt b/linux/CMakeLists.txt index 7b99da46..f8c8935f 100644 --- a/linux/CMakeLists.txt +++ b/linux/CMakeLists.txt @@ -5,4 +5,8 @@ add_subdirectory(boardtest) add_subdirectory(devices) add_subdirectory(ipcore) +if(EIVE_ADD_LINUX_FSFWCONFIG) + add_subdirectory(fsfwconfig) +endif() + target_sources(${OBSW_NAME} PUBLIC ObjectFactory.cpp InitMission.cpp) diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index ea314b4b..575bd6cb 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -335,7 +335,7 @@ void SpiTestClass::performPeriodicMax1227Test() { void SpiTestClass::performMax1227Test() { std::string deviceName = ""; #ifdef XIPHOS_Q7S - std::string deviceName = q7s::SPI_DEFAULT_DEV; + deviceName = q7s::SPI_DEFAULT_DEV; #elif defined(RASPBERRY_PI) #elif defined(EGSE) #elif defined(TE0720_1CFA) diff --git a/bsp_linux_board/fsfwconfig/CMakeLists.txt b/linux/fsfwconfig/CMakeLists.txt similarity index 100% rename from bsp_linux_board/fsfwconfig/CMakeLists.txt rename to linux/fsfwconfig/CMakeLists.txt diff --git a/bsp_linux_board/fsfwconfig/FSFWConfig.h.in b/linux/fsfwconfig/FSFWConfig.h.in similarity index 100% rename from bsp_linux_board/fsfwconfig/FSFWConfig.h.in rename to linux/fsfwconfig/FSFWConfig.h.in diff --git a/bsp_linux_board/fsfwconfig/events/subsystemIdRanges.h b/linux/fsfwconfig/events/subsystemIdRanges.h similarity index 100% rename from bsp_linux_board/fsfwconfig/events/subsystemIdRanges.h rename to linux/fsfwconfig/events/subsystemIdRanges.h diff --git a/bsp_linux_board/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp similarity index 100% rename from bsp_linux_board/fsfwconfig/events/translateEvents.cpp rename to linux/fsfwconfig/events/translateEvents.cpp diff --git a/bsp_linux_board/fsfwconfig/events/translateEvents.h b/linux/fsfwconfig/events/translateEvents.h similarity index 100% rename from bsp_linux_board/fsfwconfig/events/translateEvents.h rename to linux/fsfwconfig/events/translateEvents.h diff --git a/bsp_linux_board/fsfwconfig/ipc/MissionMessageTypes.cpp b/linux/fsfwconfig/ipc/MissionMessageTypes.cpp similarity index 100% rename from bsp_linux_board/fsfwconfig/ipc/MissionMessageTypes.cpp rename to linux/fsfwconfig/ipc/MissionMessageTypes.cpp diff --git a/bsp_linux_board/fsfwconfig/ipc/MissionMessageTypes.h b/linux/fsfwconfig/ipc/MissionMessageTypes.h similarity index 100% rename from bsp_linux_board/fsfwconfig/ipc/MissionMessageTypes.h rename to linux/fsfwconfig/ipc/MissionMessageTypes.h diff --git a/bsp_linux_board/fsfwconfig/objects/systemObjectList.h b/linux/fsfwconfig/objects/systemObjectList.h similarity index 100% rename from bsp_linux_board/fsfwconfig/objects/systemObjectList.h rename to linux/fsfwconfig/objects/systemObjectList.h diff --git a/bsp_linux_board/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp similarity index 100% rename from bsp_linux_board/fsfwconfig/objects/translateObjects.cpp rename to linux/fsfwconfig/objects/translateObjects.cpp diff --git a/bsp_linux_board/fsfwconfig/objects/translateObjects.h b/linux/fsfwconfig/objects/translateObjects.h similarity index 100% rename from bsp_linux_board/fsfwconfig/objects/translateObjects.h rename to linux/fsfwconfig/objects/translateObjects.h diff --git a/bsp_linux_board/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp similarity index 100% rename from bsp_linux_board/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp rename to linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp diff --git a/bsp_linux_board/fsfwconfig/pollingsequence/pollingSequenceFactory.h b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h similarity index 100% rename from bsp_linux_board/fsfwconfig/pollingsequence/pollingSequenceFactory.h rename to linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h diff --git a/bsp_linux_board/fsfwconfig/returnvalues/classIds.h b/linux/fsfwconfig/returnvalues/classIds.h similarity index 100% rename from bsp_linux_board/fsfwconfig/returnvalues/classIds.h rename to linux/fsfwconfig/returnvalues/classIds.h From a74ddbbb9f4b8d8680cee5ce6bac3dad84c12110 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 17:46:57 +0100 Subject: [PATCH 130/244] small bugfix --- linux/boardtest/SpiTestClass.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index 575bd6cb..91e8893c 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -555,9 +555,10 @@ void SpiTestClass::max1227PlPcduTest(int fd) { uint8_t n = 11; size_t tmpSz = spiTransferStruct[0].len; max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, tmpSz); - max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data() + spiTransferStruct[0].len, - tmpSz); spiTransferStruct[0].len = tmpSz; + size_t dummy = 0; + max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data() + spiTransferStruct[0].len, + dummy); // + 1 to account for temp conversion byte spiTransferStruct[0].len += 1; transfer(fd, gpioIds::PLPCDU_ADC_CS); From 4c5515c05c04d0fd88493b1ab9a9a2c5f5b57fdc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 11 Nov 2022 11:06:08 +0100 Subject: [PATCH 131/244] update status code handling --- .../PlocSupervisorDefinitions.h | 614 +++++++++--------- linux/devices/ploc/PlocSupervisorHandler.cpp | 42 +- 2 files changed, 318 insertions(+), 338 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 512a4950..c74e81ff 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -235,6 +235,81 @@ enum class LatchupMonId : uint8_t { LATCHUP_STATUS_REPORT = 0x01 }; } // namespace tm +enum class GeneralStatusCode : uint32_t { + OK = 0x000, + NAK = 0x001, + INIT_ERROR = 0x002, + BAD_PARAM = 0x003, + NOT_INITIALIZED = 0x004, + BAD_PERIPH_ID = 0x005, + TIMEOUT = 0x006, + RX_ERROR = 0x007, + TX_ERROR = 0x008, + ARB_LOST = 0x009, + BUSY = 0x00A, + NOT_IMPL = 0x00B, + ALIGNMENT_ERROR = 0x00C, + PERIPH_ERROR = 0x00D, + FAILED_LATCH = 0x00E, + GPIO_HIGH = 0x00F, + GPIO_LOW = 0x010, + TEST_PASSED = 0x011, + TEST_FAILED = 0x012, + BAD_NOF_PARAMS = 0x013, + NULL_POINTER = 0x014, + TASK_CREATION_ERROR = 0x015, + CORRUPTED_MRAM_VAL = 0x016, + BUF_EMPTY = 0x017 +}; + +enum class BootManStatusCode : uint32_t { + NOTHING_TODO = 0x100, + POWER_FAULT = 0x101, + INVALID_LENGTH = 0x102, + OUT_OF_RANGE = 0x103, + OUT_OF_HEAP_MEMORY = 0x104, + INVALID_STATE_TRANSITION = 0x105, + MPSOC_ALREADY_BOOTING = 0x106, + MPSOC_ALREADY_OPERATIONAL = 0x107, + MPSOC_BOOT_FAILED = 0x108, +}; + +enum class MemManStatusCode : uint32_t { + SP_NOT_AVAILABLE = 0x200, + SP_DATA_INSUFFICIENT = 0x201, + SP_MEMORY_ID_INVALID = 0x202, + MPSOC_NOT_IN_RESET = 0x203, + FLASH_INIT_FAILED = 0x204, + FLASH_ERASE_FAILED = 0x205, + FLASH_WRITE_FAILED = 0x206, + FLASH_VERIFY_FAILED = 0x207, + CANNOT_ACCESS_TM = 0x208, + CANNOT_SEND_TM = 0x209, +}; + +enum class PowerManStatusCode : uint32_t { + PG_LOW = 0x300, + PG_5V_LOW = 0x301, + PG_0V85_LOW = 0x302, + PG_1V8_LOW = 0x303, + PG_MISC_LOW = 0x304, + PG_3V3_LOW = 0x305, + PG_MB_VAIO_LOW = 0x306, + PG_MB_MPSOCIO_LOW = 0x307 +}; + +enum class TmtcManStatusCode : uint32_t { + BUF_FULL = 0x600, + WRONG_APID = 0x601, + WRONG_SERVICE_ID = 0x602, + TC_DELIVERY_ACCEPTED = 0x603, + TC_DELIVERY_REJECTED = 0x0604, + TC_PACKET_LEN_INCORRECT = 0x605, + BAD_CRC = 0x606, + BAD_DEST = 0x607, + BAD_SP_HEADER = 0x608 +}; + static const uint16_t APID_MASK = 0x3FF; static const uint16_t SEQUENCE_COUNT_MASK = 0xFFF; @@ -1265,6 +1340,236 @@ class VerificationReport { uint32_t getStatusCode() const { return statusCode; } + virtual void printStatusInformation(const char* prefix) { + bool codeHandled = true; + if (statusCode < 0x100) { + GeneralStatusCode code = static_cast(getStatusCode()); + switch (code) { + case GeneralStatusCode::OK: { + sif::warning << prefix << "Ok" << std::endl; + break; + } + case GeneralStatusCode::INIT_ERROR: { + sif::warning << prefix << "Init error" << std::endl; + break; + } + case GeneralStatusCode::BAD_PARAM: { + sif::warning << prefix << "Bad param" << std::endl; + break; + } + case GeneralStatusCode::NOT_INITIALIZED: { + sif::warning << prefix << "Not initialized" << std::endl; + break; + } + case GeneralStatusCode::BAD_PERIPH_ID: { + sif::warning << prefix << "Bad periph ID" << std::endl; + break; + } + case GeneralStatusCode::TIMEOUT: { + sif::warning << prefix << "Timeout" << std::endl; + break; + } + case GeneralStatusCode::RX_ERROR: { + sif::warning << prefix << "RX error" << std::endl; + break; + } + case GeneralStatusCode::TX_ERROR: { + sif::warning << prefix << "TX error" << std::endl; + break; + } + case GeneralStatusCode::BUF_EMPTY: { + sif::warning << prefix << "Buf empty" << std::endl; + break; + } + case GeneralStatusCode::NAK: { + sif::warning << prefix << "Nak, default error code" << std::endl; + break; + } + case GeneralStatusCode::ARB_LOST: { + sif::warning << prefix << "Arb lost" << std::endl; + break; + } + case GeneralStatusCode::BUSY: { + sif::warning << prefix << "Busy" << std::endl; + break; + } + case GeneralStatusCode::NOT_IMPL: { + sif::warning << prefix << "Not implemented" << std::endl; + break; + } + case GeneralStatusCode::ALIGNMENT_ERROR: { + sif::warning << prefix << "Alignment error" << std::endl; + break; + } + case GeneralStatusCode::PERIPH_ERROR: { + sif::warning << prefix << "Periph error" << std::endl; + break; + } + case GeneralStatusCode::FAILED_LATCH: { + sif::warning << prefix << "Failed latch" << std::endl; + break; + } + case GeneralStatusCode::GPIO_HIGH: { + sif::warning << prefix << "GPIO high" << std::endl; + break; + } + case GeneralStatusCode::GPIO_LOW: { + sif::warning << prefix << "GPIO low" << std::endl; + break; + } + case GeneralStatusCode::TEST_PASSED: { + sif::warning << prefix << "Test passed" << std::endl; + break; + } + case GeneralStatusCode::TEST_FAILED: { + sif::warning << prefix << "Test failed" << std::endl; + break; + } + default: { + codeHandled = false; + break; + } + } + } else if (statusCode < 0x200 and statusCode > 0x100) { + BootManStatusCode code = static_cast(statusCode); + switch (code) { + case BootManStatusCode::NOTHING_TODO: { + sif::warning << prefix << "Nothing to do" << std::endl; + break; + } + case BootManStatusCode::POWER_FAULT: { + sif::warning << prefix << "Power fault" << std::endl; + break; + } + case BootManStatusCode::INVALID_LENGTH: { + sif::warning << prefix << "Invalid length" << std::endl; + break; + } + case BootManStatusCode::OUT_OF_RANGE: { + sif::warning << prefix << "Out of range, lenght check of parameter failed" << std::endl; + break; + } + case BootManStatusCode::OUT_OF_HEAP_MEMORY: { + sif::warning << prefix << "Out of heap memory" << std::endl; + break; + } + case BootManStatusCode::INVALID_STATE_TRANSITION: { + sif::warning << prefix << "Invalid state transition" << std::endl; + break; + } + case BootManStatusCode::MPSOC_ALREADY_BOOTING: { + sif::warning << prefix << "MPSoC already booting" << std::endl; + break; + } + case BootManStatusCode::MPSOC_ALREADY_OPERATIONAL: { + sif::warning << prefix << "MPSoC already operational" << std::endl; + break; + } + case BootManStatusCode::MPSOC_BOOT_FAILED: { + sif::warning << prefix << "MPSoC boot failed" << std::endl; + break; + } + default: { + codeHandled = true; + break; + } + } + } else if (statusCode < 0x300 and statusCode > 0x200) { + MemManStatusCode code = static_cast(statusCode); + switch (code) { + case MemManStatusCode::SP_NOT_AVAILABLE: { + sif::warning << prefix << "SP not available" << std::endl; + break; + } + case MemManStatusCode::SP_DATA_INSUFFICIENT: { + sif::warning << prefix << "SP data insufficient" << std::endl; + break; + } + case MemManStatusCode::SP_MEMORY_ID_INVALID: { + sif::warning << prefix << "SP data insufficient" << std::endl; + break; + } + case MemManStatusCode::MPSOC_NOT_IN_RESET: { + sif::warning << prefix << "MPSoC not in reset" << std::endl; + break; + } + case MemManStatusCode::FLASH_INIT_FAILED: { + sif::warning << prefix << "Flash init failed" << std::endl; + break; + } + case MemManStatusCode::FLASH_ERASE_FAILED: { + sif::warning << prefix << "Flash erase failed" << std::endl; + break; + } + case MemManStatusCode::FLASH_WRITE_FAILED: { + sif::warning << prefix << "Flash write failed" << std::endl; + break; + } + case MemManStatusCode::FLASH_VERIFY_FAILED: { + sif::warning << prefix << "Flash verify failed" << std::endl; + break; + } + case MemManStatusCode::CANNOT_ACCESS_TM: { + sif::warning << prefix << "Can not access tm" << std::endl; + break; + } + case MemManStatusCode::CANNOT_SEND_TM: { + sif::warning << prefix << "Can not access tm" << std::endl; + break; + } + default: { + codeHandled = true; + break; + } + } + } else if (statusCode < 0x400 and statusCode > 0x300) { + PowerManStatusCode code = static_cast(statusCode); + switch (code) { + case PowerManStatusCode::PG_LOW: { + sif::warning << prefix << "PG low" << std::endl; + break; + } + case PowerManStatusCode::PG_5V_LOW: { + sif::warning << prefix << "PG 5V low" << std::endl; + break; + } + case PowerManStatusCode::PG_0V85_LOW: { + sif::warning << prefix << "PG 0V85 low" << std::endl; + break; + } + case PowerManStatusCode::PG_1V8_LOW: { + sif::warning << prefix << "PG 1V8 low" << std::endl; + break; + } + case PowerManStatusCode::PG_MISC_LOW: { + sif::warning << prefix << "PG misc low" << std::endl; + break; + } + case PowerManStatusCode::PG_3V3_LOW: { + sif::warning << prefix << "PG 3V3 low" << std::endl; + break; + } + case PowerManStatusCode::PG_MB_VAIO_LOW: { + sif::warning << prefix << "PG mb vaio low" << std::endl; + break; + } + case PowerManStatusCode::PG_MB_MPSOCIO_LOW: { + sif::warning << prefix << "PG mb mpsocio low" << std::endl; + break; + } + default: { + codeHandled = true; + break; + } + } + } + if (not codeHandled) { + sif::warning << prefix << "Invalid or unimplemented status code: 0x" << std::hex + << std::setfill('0') << std::setw(4) << static_cast(statusCode) + << std::dec << std::endl; + } + } + protected: TmBase& readerBase; uint8_t refApid = 0; @@ -1287,59 +1592,11 @@ class AcknowledgmentReport : public VerificationReport { } void printStatusInformation() { - StatusCode statusCode = static_cast(getStatusCode()); - const char* prefix = "Supervisor acknowledgment report status: "; - switch (statusCode) { - case StatusCode::OK: { - sif::warning << prefix << "Ok" << std::endl; - break; - } - case StatusCode::BAD_PARAM: { - sif::warning << prefix << "Bad param" << std::endl; - break; - } - case StatusCode::TIMEOUT: { - sif::warning << prefix << "Timeout" << std::endl; - break; - } - case StatusCode::RX_ERROR: { - sif::warning << prefix << "RX error" << std::endl; - break; - } - case StatusCode::TX_ERROR: { - sif::warning << prefix << "TX error" << std::endl; - break; - } - case StatusCode::HEADER_EMPTY: { - sif::warning << prefix << "Header empty" << std::endl; - break; - } - case StatusCode::DEFAULT_NAK: { - sif::warning << prefix << "Default code for NAK" << std::endl; - break; - } - case StatusCode::ROUTE_PACKET: { - sif::warning << prefix << "Route packet error" << std::endl; - break; - } - default: - sif::warning << "AcknowledgmentReport::printStatusInformation: Invalid status code: 0x" - << std::hex << static_cast(statusCode) << std::endl; - break; - } + VerificationReport::printStatusInformation(STATUS_PRINTOUT_PREFIX); } private: - enum class StatusCode : uint16_t { - OK = 0x0, - BAD_PARAM = 0x1, - TIMEOUT = 0x2, - RX_ERROR = 0x3, - TX_ERROR = 0x4, - HEADER_EMPTY = 0x5, - DEFAULT_NAK = 0x6, - ROUTE_PACKET = 0x7 - }; + static constexpr char STATUS_PRINTOUT_PREFIX[] = "SUPV NAK Status: "; }; class ExecutionReport : public VerificationReport { @@ -1355,264 +1612,13 @@ class ExecutionReport : public VerificationReport { } return VerificationReport::parse(); } + void printStatusInformation() { - StatusCode statusCode = static_cast(getStatusCode()); - switch (statusCode) { - case StatusCode::OK: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Ok" << std::endl; - break; - } - case StatusCode::INIT_ERROR: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Init error" << std::endl; - break; - } - case StatusCode::BAD_PARAM: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Bad param" << std::endl; - break; - } - case StatusCode::NOT_INITIALIZED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Not initialized" << std::endl; - break; - } - case StatusCode::BAD_PERIPH_ID: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Bad periph ID" << std::endl; - break; - } - case StatusCode::TIMEOUT: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Timeout" << std::endl; - break; - } - case StatusCode::RX_ERROR: { - sif::warning << STATUS_PRINTOUT_PREFIX << "RX error" << std::endl; - break; - } - case StatusCode::TX_ERROR: { - sif::warning << STATUS_PRINTOUT_PREFIX << "TX error" << std::endl; - break; - } - case StatusCode::BUF_EMPTY: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Buf empty" << std::endl; - break; - } - case StatusCode::BUF_FULL: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Buf full" << std::endl; - break; - } - case StatusCode::NAK: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Nak, default error code" << std::endl; - break; - } - case StatusCode::ARB_LOST: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Arb lost" << std::endl; - break; - } - case StatusCode::BUSY: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Busy" << std::endl; - break; - } - case StatusCode::NOT_IMPLEMENTED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Not implemented" << std::endl; - break; - } - case StatusCode::ALIGNEMENT_ERROR: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Alignment error" << std::endl; - break; - } - case StatusCode::PERIPH_ERR: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Periph error" << std::endl; - break; - } - case StatusCode::FAILED_LATCH: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Failed latch" << std::endl; - break; - } - case StatusCode::GPIO_HIGH: { - sif::warning << STATUS_PRINTOUT_PREFIX << "GPIO high" << std::endl; - break; - } - case StatusCode::GPIO_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "GPIO low" << std::endl; - break; - } - case StatusCode::TEST_PASSED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Test passed" << std::endl; - break; - } - case StatusCode::TEST_FAILED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Test failed" << std::endl; - break; - } - case StatusCode::NOTHING_TODO: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Nothing todo, not an error but a warning" - << std::endl; - break; - } - case StatusCode::POWER_FAULT: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Power fault" << std::endl; - break; - } - case StatusCode::INVALID_LENGTH: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Invalid length" << std::endl; - break; - } - case StatusCode::OUT_OF_RANGE: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Out of range, lenght check of parameter failed" - << std::endl; - break; - } - case StatusCode::OUT_OF_HEAP_MEMORY: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Out of heap memory" << std::endl; - break; - } - case StatusCode::INVALID_STATE_TRANSITION: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Invalid state transition" << std::endl; - break; - } - case StatusCode::MPSOC_ALREADY_BOOTING: { - sif::warning << STATUS_PRINTOUT_PREFIX << "MPSoC already booting" << std::endl; - break; - } - case StatusCode::MPSOC_ALREADY_OPERATIONAL: { - sif::warning << STATUS_PRINTOUT_PREFIX << "MPSoC already operational" << std::endl; - break; - } - case StatusCode::MPSOC_BOOT_FAILED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "MPSoC boot failed" << std::endl; - break; - } - case StatusCode::SP_NOT_AVAILABLE: { - sif::warning << STATUS_PRINTOUT_PREFIX << "SP not available" << std::endl; - break; - } - case StatusCode::SP_DATA_INSUFFICIENT: { - sif::warning << STATUS_PRINTOUT_PREFIX << "SP data insufficient" << std::endl; - break; - } - case StatusCode::SP_MEMORY_ID_INVALID: { - sif::warning << STATUS_PRINTOUT_PREFIX << "SP data insufficient" << std::endl; - break; - } - case StatusCode::MPSOC_NOT_IN_RESET: { - sif::warning << STATUS_PRINTOUT_PREFIX << "MPSoC not in reset" << std::endl; - break; - } - case StatusCode::FLASH_INIT_FAILED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Flash init failed" << std::endl; - break; - } - case StatusCode::FLASH_ERASE_FAILED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Flash erase failed" << std::endl; - break; - } - case StatusCode::FLASH_WRITE_FAILED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Flash write failed" << std::endl; - break; - } - case StatusCode::FLASH_VERIFY_FAILED: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Flash verify failed" << std::endl; - break; - } - case StatusCode::CANNOT_ACCESS_TM: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Can not access tm" << std::endl; - break; - } - case StatusCode::CANNOT_SEND_TM: { - sif::warning << STATUS_PRINTOUT_PREFIX << "Can not access tm" << std::endl; - break; - } - case StatusCode::PG_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "PG low" << std::endl; - break; - } - case StatusCode::PG_5V_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "PG 5V low" << std::endl; - break; - } - case StatusCode::PG_0V85_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "PG 0V85 low" << std::endl; - break; - } - case StatusCode::PG_1V8_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "PG 1V8 low" << std::endl; - break; - } - case StatusCode::PG_MISC_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "PG misc low" << std::endl; - break; - } - case StatusCode::PG_3V3_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "PG 3V3 low" << std::endl; - break; - } - case StatusCode::PG_MB_VAIO_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "PG mb vaio low" << std::endl; - break; - } - case StatusCode::PG_MB_MPSOCIO_LOW: { - sif::warning << STATUS_PRINTOUT_PREFIX << "PG mb mpsocio low" << std::endl; - break; - } - default: - sif::warning << "ExecutionReport::printStatusInformation: Invalid status code: 0x" - << std::hex << std::setfill('0') << std::setw(4) - << static_cast(statusCode) << std::dec << std::endl; - break; - } + VerificationReport::printStatusInformation(STATUS_PRINTOUT_PREFIX); } private: - static constexpr char STATUS_PRINTOUT_PREFIX[] = "Supervisor execution failure report status: "; - - enum class StatusCode : uint16_t { - OK = 0x0, - INIT_ERROR = 0x1, - BAD_PARAM = 0x2, - NOT_INITIALIZED = 0x3, - BAD_PERIPH_ID = 0x4, - TIMEOUT = 0x5, - RX_ERROR = 0x6, - TX_ERROR = 0x7, - BUF_EMPTY = 0x8, - BUF_FULL = 0x9, - NAK = 0xA, - ARB_LOST = 0xB, - BUSY = 0xC, - NOT_IMPLEMENTED = 0xD, - ALIGNEMENT_ERROR = 0xE, - PERIPH_ERR = 0xF, - FAILED_LATCH = 0x10, - GPIO_HIGH = 0x11, - GPIO_LOW = 0x12, - TEST_PASSED = 0x13, - TEST_FAILED = 0x14, - NOTHING_TODO = 0x100, - POWER_FAULT = 0x101, - INVALID_LENGTH = 0x102, - OUT_OF_RANGE = 0x103, - OUT_OF_HEAP_MEMORY = 0x104, - INVALID_STATE_TRANSITION = 0x105, - MPSOC_ALREADY_BOOTING = 0x106, - MPSOC_ALREADY_OPERATIONAL = 0x107, - MPSOC_BOOT_FAILED = 0x108, - SP_NOT_AVAILABLE = 0x200, - SP_DATA_INSUFFICIENT = 0x201, - SP_MEMORY_ID_INVALID = 0x202, - MPSOC_NOT_IN_RESET = 0x203, - FLASH_INIT_FAILED = 0x204, - FLASH_ERASE_FAILED = 0x205, - FLASH_WRITE_FAILED = 0x206, - FLASH_VERIFY_FAILED = 0x207, - CANNOT_ACCESS_TM = 0x208, - CANNOT_SEND_TM = 0x209, - PG_LOW = 0x300, - PG_5V_LOW = 0x301, - PG_0V85_LOW = 0x302, - PG_1V8_LOW = 0x303, - PG_MISC_LOW = 0x304, - PG_3V3_LOW = 0x305, - PG_MB_VAIO_LOW = 0x306, - PG_MB_MPSOCIO_LOW = 0x307 - }; + static constexpr char STATUS_PRINTOUT_PREFIX[] = "SUPV EXE NAK Status: "; }; class UpdateStatusReport { diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index ddfd3733..f5f333fa 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -314,6 +314,10 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d prepareSetAdcThresholdCmd(commandData); result = returnvalue::OK; break; + } + case WIPE_MRAM: { + result = prepareWipeMramCmd(commandData); + break; } // case ENABLE_NVMS: { // result = prepareEnableNvmsCommand(commandData); @@ -324,33 +328,6 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // result = returnvalue::OK; // break; // } - /* case FACTORY_RESET_CLEAR_ALL: { - FactoryReset packet(spParams); - result = packet.buildPacket(FactoryReset::Op::CLEAR_ALL); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } - case FACTORY_RESET_CLEAR_MIRROR: { - FactoryReset packet(spParams); - result = packet.buildPacket(FactoryReset::Op::MIRROR_ENTRIES); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - } - case FACTORY_RESET_CLEAR_CIRCULAR: { - FactoryReset packet(spParams); - result = packet.buildPacket(FactoryReset::Op::CIRCULAR_ENTRIES); - if (result != returnvalue::OK) { - break; - } - finishTcPrep(packet.getFullPacketLen()); - break; - }*/ // Removed command // case START_MPSOC_QUIET: { // prepareEmptyCmd(APID_START_MPSOC_QUIET); @@ -417,10 +394,6 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d // result = returnvalue::OK; // break; // } - case WIPE_MRAM: { - result = prepareWipeMramCmd(commandData); - break; - } // case FIRST_MRAM_DUMP: // case CONSECUTIVE_MRAM_DUMP: // result = prepareDumpMramCmd(commandData); @@ -443,6 +416,7 @@ void PlocSupervisorHandler::fillCommandAndReplyMap() { insertInCommandMap(SET_BOOT_TIMEOUT); insertInCommandMap(SET_MAX_RESTART_TRIES); insertInCommandMap(RESET_MPSOC); + insertInCommandMap(WIPE_MRAM); insertInCommandMap(SET_TIME_REF); insertInCommandMap(DISABLE_PERIOIC_HK_TRANSMISSION); insertInCommandMap(GET_BOOT_STATUS_REPORT); @@ -456,6 +430,9 @@ void PlocSupervisorHandler::fillCommandAndReplyMap() { insertInCommandMap(FACTORY_RESET); insertInCommandMap(SET_SHUTDOWN_TIMEOUT); insertInCommandMap(FACTORY_FLASH); + insertInCommandMap(SET_ADC_ENABLED_CHANNELS); + insertInCommandMap(SET_ADC_THRESHOLD); + insertInCommandMap(SET_ADC_WINDOW_AND_STRIDE); insertInCommandMap(RESET_PL); // ACK replies, use countdown for them @@ -567,9 +544,6 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite case FACTORY_RESET: case READ_GPIO: // case RESTART_SUPERVISOR: - // case FACTORY_RESET_CLEAR_ALL: - // case FACTORY_RESET_CLEAR_MIRROR: - // case FACTORY_RESET_CLEAR_CIRCULAR: case DISABLE_PERIOIC_HK_TRANSMISSION: // case START_MPSOC_QUIET: case SET_SHUTDOWN_TIMEOUT: From 5690e35a58dd13a05bb2a363c8db804910a72b77 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 09:55:23 +0100 Subject: [PATCH 132/244] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index e03e7f52..00284510 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit e03e7f5260eb92d1903188c5ea14ea01616600ae +Subproject commit 002845108d24abe9fbb26618f004f04ccb160938 From 6c568102337c680fc0e7d6e8266016d1586f9421 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 09:55:38 +0100 Subject: [PATCH 133/244] bump fsfw --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 66a1362e..89285ad5 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 66a1362e7e977e427a66fd3176a9e7b6bc1b5998 +Subproject commit 89285ad511ef11582b67b307c3840a09efd0577a From f6729cb3b45c4c7b3a7ea7b081615bf03a09353f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 13:20:12 +0100 Subject: [PATCH 134/244] bump deps --- fsfw | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fsfw b/fsfw index 00284510..c5f91926 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 002845108d24abe9fbb26618f004f04ccb160938 +Subproject commit c5f91926c95a8db0c7921176aa3f62cc2bebef47 diff --git a/tmtc b/tmtc index 89285ad5..66a1362e 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 89285ad511ef11582b67b307c3840a09efd0577a +Subproject commit 66a1362e7e977e427a66fd3176a9e7b6bc1b5998 From 9da5a55ef31b3bfdde4239f0c94bf148e3c6a57c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 13:32:05 +0100 Subject: [PATCH 135/244] lets see if that works --- CMakeLists.txt | 7 +++++-- bsp_linux_board/CMakeLists.txt | 1 + linux/CMakeLists.txt | 5 ++++- linux/InitMission.cpp | 1 + linux/boardtest/SpiTestClass.cpp | 27 ++++++++++++++++----------- linux/boardtest/UartTestClass.cpp | 3 ++- 6 files changed, 29 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ddf101a4..97b7601c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -233,7 +233,7 @@ set(LIB_ARCSEC_PATH ${THIRD_PARTY_FOLDER}/arcsec_star_tracker) set(LIB_JSON_PATH ${THIRD_PARTY_FOLDER}/json) set(FSFW_WARNING_SHADOW_LOCAL_GCC OFF) -set(EIVE_ADD_LINUX_FILES False) +set(EIVE_ADD_LINUX_FILES OFF) # Analyse different OS and architecture/target options, determine BSP_PATH, # display information about compiler etc. @@ -253,12 +253,15 @@ if(TGT_BSP) set(FSFW_CONFIG_PATH "linux/fsfwconfig") if(NOT BUILD_Q7S_SIMPLE_MODE) set(EIVE_ADD_LINUX_FILES TRUE) + set(EIVE_ADD_LINUX_FSFWCONFIG TRUE) set(ADD_GOMSPACE_CSP TRUE) set(ADD_GOMSPACE_CLIENTS TRUE) set(FSFW_HAL_ADD_LINUX ON) set(FSFW_HAL_LINUX_ADD_LIBGPIOD ON) set(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS ON) endif() + elseif(UNIX) + set(EIVE_ADD_LINUX_FILES ON) endif() if(TGT_BSP MATCHES "arm/raspberrypi") @@ -393,7 +396,7 @@ if(EIVE_ADD_JSON_LIB) add_subdirectory(${LIB_JSON_PATH}) endif() -add_subdirectory(thirdparty/rapidcsv) +add_subdirectory(thirdparty) if(EIVE_ADD_LINUX_FILES) add_subdirectory(${LIB_ARCSEC_PATH}) diff --git a/bsp_linux_board/CMakeLists.txt b/bsp_linux_board/CMakeLists.txt index c1817ac1..39f06401 100644 --- a/bsp_linux_board/CMakeLists.txt +++ b/bsp_linux_board/CMakeLists.txt @@ -3,3 +3,4 @@ target_sources(${OBSW_NAME} PUBLIC InitMission.cpp main.cpp gpioInit.cpp add_subdirectory(boardconfig) add_subdirectory(boardtest) +add_subdirectory(fsfwconfig) diff --git a/linux/CMakeLists.txt b/linux/CMakeLists.txt index d7e6ca93..f8c8935f 100644 --- a/linux/CMakeLists.txt +++ b/linux/CMakeLists.txt @@ -3,7 +3,10 @@ add_subdirectory(utility) add_subdirectory(callbacks) add_subdirectory(boardtest) add_subdirectory(devices) -add_subdirectory(fsfwconfig) add_subdirectory(ipcore) +if(EIVE_ADD_LINUX_FSFWCONFIG) + add_subdirectory(fsfwconfig) +endif() + target_sources(${OBSW_NAME} PUBLIC ObjectFactory.cpp InitMission.cpp) diff --git a/linux/InitMission.cpp b/linux/InitMission.cpp index 5bc8698c..4b7dc1f1 100644 --- a/linux/InitMission.cpp +++ b/linux/InitMission.cpp @@ -6,6 +6,7 @@ #include "OBSWConfig.h" #include "ObjectFactory.h" +#include "eive/objects.h" void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, PeriodicTaskIF*& scexReaderTask) { diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index dc5a5e3b..91e8893c 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -333,14 +333,12 @@ void SpiTestClass::performPeriodicMax1227Test() { } void SpiTestClass::performMax1227Test() { + std::string deviceName = ""; #ifdef XIPHOS_Q7S - std::string deviceName = q7s::SPI_DEFAULT_DEV; + deviceName = q7s::SPI_DEFAULT_DEV; #elif defined(RASPBERRY_PI) - std::string deviceName = ""; #elif defined(EGSE) - std::string deviceName = ""; #elif defined(TE0720_1CFA) - std::string deviceName = ""; #endif int fd = 0; UnixFileGuard fileHelper(deviceName, &fd, O_RDWR, "SpiComIF::initializeInterface"); @@ -381,8 +379,9 @@ void SpiTestClass::max1227RadSensorTest(int fd) { DiffSel::NONE_0); spiTransferStruct[0].len = 1; transfer(fd, gpioIds::CS_RAD_SENSOR); - max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 7, spiTransferStruct[0].len); - size_t tmpLen = spiTransferStruct[0].len; + size_t tmpSz = spiTransferStruct[0].len; + max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 7, tmpSz); + size_t tmpLen = tmpSz; spiTransferStruct[0].len = 1; transfer(fd, gpioIds::CS_RAD_SENSOR); std::memcpy(sendBuffer.data(), sendBuffer.data() + 1, tmpLen - 1); @@ -403,7 +402,8 @@ void SpiTestClass::max1227RadSensorTest(int fd) { for (int idx = 0; idx < 8; idx++) { sif::info << "ADC raw " << idx << ": " << adcRaw[idx] << std::endl; } - max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data(), spiTransferStruct[0].len); + tmpSz = spiTransferStruct[0].len; + max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data(), tmpSz); spiTransferStruct[0].len = 1; transfer(fd, gpioIds::CS_RAD_SENSOR); usleep(65); @@ -467,7 +467,8 @@ void SpiTestClass::max1227SusTest(int fd, SusTestCfg &cfg) { spiTransferStruct[0].len = 1; transfer(fd, cfg.gpioId); - max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 5, spiTransferStruct[0].len); + size_t tmpSz = spiTransferStruct[0].len; + max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 5, tmpSz); transfer(fd, cfg.gpioId); uint16_t adcRaw[6] = {}; adcRaw[0] = (recvBuffer[1] << 8) | recvBuffer[2]; @@ -552,7 +553,9 @@ void SpiTestClass::max1227PlPcduTest(int fd) { spiTransferStruct[0].len = 1; transfer(fd, gpioIds::PLPCDU_ADC_CS); uint8_t n = 11; - max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, spiTransferStruct[0].len); + size_t tmpSz = spiTransferStruct[0].len; + max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, tmpSz); + spiTransferStruct[0].len = tmpSz; size_t dummy = 0; max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data() + spiTransferStruct[0].len, dummy); @@ -585,9 +588,11 @@ void SpiTestClass::max1227PlPcduTest(int fd) { spiTransferStruct[0].len = 1; transfer(fd, gpioIds::PLPCDU_ADC_CS); uint8_t n = 11; - max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, spiTransferStruct[0].len); + size_t tmpLen = spiTransferStruct[0].len; + max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), n, tmpLen); max1227::prepareExternallyClockedTemperatureRead(sendBuffer.data() + spiTransferStruct[0].len, - spiTransferStruct[0].len); + tmpLen); + spiTransferStruct[0].len = tmpLen; transfer(fd, gpioIds::PLPCDU_ADC_CS); uint16_t adcRaw[n + 1] = {}; for (uint8_t idx = 0; idx < n + 1; idx++) { diff --git a/linux/boardtest/UartTestClass.cpp b/linux/boardtest/UartTestClass.cpp index 51ea237f..1f707c8f 100644 --- a/linux/boardtest/UartTestClass.cpp +++ b/linux/boardtest/UartTestClass.cpp @@ -13,6 +13,7 @@ #include #include "OBSWConfig.h" +#include "eive/objects.h" #include "fsfw/globalfunctions/CRC.h" #include "fsfw/globalfunctions/DleEncoder.h" #include "fsfw/globalfunctions/arrayprinter.h" @@ -164,7 +165,7 @@ void UartTestClass::scexInit() { #else std::string devname = "/dev/ul-scex"; #endif - uartCookie = new SerialCookie(this->getObjectId(), devname, UartBaudRate::RATE_57600, 4096); + uartCookie = new UartCookie(this->getObjectId(), devname, UartBaudRate::RATE_57600, 4096); reader->setDebugMode(false); ReturnValue_t result = reader->initializeInterface(uartCookie); if (result != OK) { From 478e3b92374d67279d4092232ca27ef880a271fa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 13:33:53 +0100 Subject: [PATCH 136/244] add thirdparty CMakeLists.txt --- thirdparty/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) create mode 100644 thirdparty/CMakeLists.txt diff --git a/thirdparty/CMakeLists.txt b/thirdparty/CMakeLists.txt new file mode 100644 index 00000000..68ea851d --- /dev/null +++ b/thirdparty/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(rapidcsv) From 3c469e5c7a1ab5a1f9d7f42213d61b03f8fe258a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 13:35:18 +0100 Subject: [PATCH 137/244] uart test class tweak --- linux/boardtest/UartTestClass.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/boardtest/UartTestClass.cpp b/linux/boardtest/UartTestClass.cpp index 1f707c8f..def7da8b 100644 --- a/linux/boardtest/UartTestClass.cpp +++ b/linux/boardtest/UartTestClass.cpp @@ -165,7 +165,7 @@ void UartTestClass::scexInit() { #else std::string devname = "/dev/ul-scex"; #endif - uartCookie = new UartCookie(this->getObjectId(), devname, UartBaudRate::RATE_57600, 4096); + uartCookie = new SerialCookie(this->getObjectId(), devname, UartBaudRate::RATE_57600, 4096); reader->setDebugMode(false); ReturnValue_t result = reader->initializeInterface(uartCookie); if (result != OK) { From 7384c5ce16d4d104b00b7b7e602a7f54cc11ae65 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 13:41:39 +0100 Subject: [PATCH 138/244] update changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 33604f09..acef9e92 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,8 @@ list yields a list of all related PRs for each release. # [unreleased] +- It is now possible to compile Linux components for the hosted build conditionally + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/322 - ACS Subsystem. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/231 - Payload Subsystem. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/231 - Add IRQ mode for PDEC handler. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/310 From cf57be14bc6007428295ce78cd221d2c6a5ede52 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 16:00:32 +0100 Subject: [PATCH 139/244] assign name for hosted ploc supv --- bsp_hosted/ObjectFactory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 7e7bda14..7289c361 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -93,7 +93,7 @@ void ObjectFactory::produce(void* args) { objects::PLOC_SUPERVISOR_HANDLER); #endif /* OBSW_ADD_PLOC_MPSOC == 1 */ #if OBSW_ADD_PLOC_SUPERVISOR == 1 - std::string plocSupvString = ""; + std::string plocSupvString = "/dev/ploc_supv"; auto supervisorCookie = new UartCookie(objects::PLOC_SUPERVISOR_HANDLER, plocSupvString, uart::PLOC_SUPV_BAUD, supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); From 9ee969218cf7ea993fcd96989cbba7bdde3585b4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 16:06:23 +0100 Subject: [PATCH 140/244] add README entry for udevadm usage --- README.md | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/README.md b/README.md index f68eb210..403189d6 100644 --- a/README.md +++ b/README.md @@ -1055,6 +1055,29 @@ Get fill count: xsc_scratch read | wc -c ``` +## Custom device names in Linux with the `udev` module + +You can assign custom device names using the Linux `udev` system. +This works by specifying a rules file inside the `/etc/udev/rules.d` folder +which creates a SYMLINK if certain device properties are true. + +Each rule is a new line inside a rules file. +For example, the rule + +```txt +SUBSYSTEM=="tty", ATTRS{interface}=="Dual RS232-HS", ATTRS{bInterfaceNumber}=="01", SYMLINK+="ploc_supv +``` + +Will create a symlink `/dev/ploc_supv` if a connected USB device has the +same `interface` and `bInterfaceNumber` properties as shown above. + +You can list the properties for a given connected device using `udevadm`. +For example, you can do this for a connected example device `/dev/ttyUSB0` +by using + +```txt +udevadm info -a /dev/ttyUSB0 +``` ## Using `system` when debugging From 7d7efaca426bd0909d891ad82a3d9840873c07df Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 16:44:07 +0100 Subject: [PATCH 141/244] improve hosted SW --- bsp_hosted/OBSWConfig.h.in | 8 ++++++++ bsp_hosted/ObjectFactory.cpp | 6 ++---- bsp_q7s/OBSWConfig.h.in | 7 +++++++ common/config/commonConfig.h.in | 6 ------ dummies/TemperatureSensorsDummy.cpp | 4 ++++ mission/core/GenericFactory.cpp | 12 ++++++------ 6 files changed, 27 insertions(+), 16 deletions(-) diff --git a/bsp_hosted/OBSWConfig.h.in b/bsp_hosted/OBSWConfig.h.in index 9c7a3f03..f234e795 100644 --- a/bsp_hosted/OBSWConfig.h.in +++ b/bsp_hosted/OBSWConfig.h.in @@ -24,6 +24,7 @@ #define OBSW_ADD_GPS_0 0 #define OBSW_ADD_GPS_1 0 #define OBSW_ADD_RW 0 +#define OBSW_DEBUG_TMP1075 0 #define OBSW_ADD_BPX_BATTERY_HANDLER 0 #define OBSW_ADD_RTD_DEVICES 0 #define OBSW_ADD_PL_PCDU 0 @@ -100,6 +101,13 @@ /*******************************************************************/ /** CMake Defines */ /*******************************************************************/ + +// Use TCP instead of UDP for the TMTC bridge. This allows using the TMTC client locally +// because UDP packets are not allowed in the VPN +// This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the +// CCSDS IP Cores. +#define OBSW_USE_TMTC_TCP_BRIDGE 0 + #cmakedefine EIVE_BUILD_GPSD_GPS_HANDLER #cmakedefine LIBGPS_VERSION_MAJOR @LIBGPS_VERSION_MAJOR@ diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 7289c361..01d6da22 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -64,10 +64,10 @@ void Factory::setStaticFrameworkObjectIds() { PusServiceBase::PUS_DISTRIBUTOR = objects::PUS_PACKET_DISTRIBUTOR; - PusServiceBase::PACKET_DESTINATION = objects::TM_FUNNEL; + PusServiceBase::PACKET_DESTINATION = objects::PUS_TM_FUNNEL; CommandingServiceBase::defaultPacketSource = objects::PUS_PACKET_DISTRIBUTOR; - CommandingServiceBase::defaultPacketDestination = objects::TM_FUNNEL; + CommandingServiceBase::defaultPacketDestination = objects::PUS_TM_FUNNEL; VerificationReporter::DEFAULT_RECEIVER = objects::PUS_SERVICE_1_VERIFICATION; } @@ -107,8 +107,6 @@ void ObjectFactory::produce(void* args) { dummy::DummyCfg cfg; dummy::createDummies(cfg); - new TemperatureSensorsDummy(); - new SusDummy(); new ThermalController(objects::THERMAL_CONTROLLER); new TestTask(objects::TEST_TASK); } diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in index 499d8e6b..5cde824d 100644 --- a/bsp_q7s/OBSWConfig.h.in +++ b/bsp_q7s/OBSWConfig.h.in @@ -119,6 +119,13 @@ /*******************************************************************/ /** CMake Defines */ /*******************************************************************/ + +// Use TCP instead of UDP for the TMTC bridge. This allows using the TMTC client locally +// because UDP packets are not allowed in the VPN +// This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the +// CCSDS IP Cores. +#define OBSW_USE_TMTC_TCP_BRIDGE 1 + #cmakedefine EIVE_BUILD_GPSD_GPS_HANDLER #cmakedefine LIBGPS_VERSION_MAJOR @LIBGPS_VERSION_MAJOR@ diff --git a/common/config/commonConfig.h.in b/common/config/commonConfig.h.in index 83e1aaac..4fcc308f 100644 --- a/common/config/commonConfig.h.in +++ b/common/config/commonConfig.h.in @@ -19,12 +19,6 @@ debugging. */ // Disable this for mission code. It allows exchanging TMTC packets via the Ethernet port #define OBSW_ADD_TCPIP_BRIDGE 1 -// Use TCP instead of UDP for the TMTC bridge. This allows using the TMTC client locally -// because UDP packets are not allowed in the VPN -// This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the -// CCSDS IP Cores. -#define OBSW_USE_TMTC_TCP_BRIDGE 1 - #define OBSW_ADD_CFDP_COMPONENTS 1 namespace common { diff --git a/dummies/TemperatureSensorsDummy.cpp b/dummies/TemperatureSensorsDummy.cpp index 52190ee8..e7c75833 100644 --- a/dummies/TemperatureSensorsDummy.cpp +++ b/dummies/TemperatureSensorsDummy.cpp @@ -25,6 +25,10 @@ TemperatureSensorsDummy::TemperatureSensorsDummy() ObjectManager::instance()->insert(objects::RTD_15_IC18_IMTQ, this); ObjectManager::instance()->insert(objects::TMP1075_HANDLER_TCS_0, this); ObjectManager::instance()->insert(objects::TMP1075_HANDLER_TCS_1, this); + + ObjectManager::instance()->insert(objects::TMP1075_HANDLER_PLPCDU_0, this); + ObjectManager::instance()->insert(objects::TMP1075_HANDLER_PLPCDU_1, this); + ObjectManager::instance()->insert(objects::TMP1075_HANDLER_IF_BOARD, this); } ReturnValue_t TemperatureSensorsDummy::initialize() { diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp index 0e1926f4..114451b2 100644 --- a/mission/core/GenericFactory.cpp +++ b/mission/core/GenericFactory.cpp @@ -99,12 +99,12 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun #if OBSW_ADD_TCPIP_BRIDGE == 1 #if OBSW_USE_TMTC_TCP_BRIDGE == 0 - auto tcpIpTmtcBridge = new UdpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR); + auto tmtcBridge = new UdpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR); new UdpTcPollingTask(objects::TMTC_POLLING_TASK, objects::TMTC_BRIDGE); sif::info << "Created UDP server for TMTC commanding with listener port " - << udpBridge->getUdpPort() << std::endl; + << tmtcBridge->getUdpPort() << std::endl; #else - auto tcpIpTmtcBridge = new TcpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR); + auto tmtcBridge = new TcpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR); auto tcpServer = new TcpTmTcServer(objects::TMTC_POLLING_TASK, objects::TMTC_BRIDGE); // TCP is stream based. Use packet ID as start marker when parsing for space packets tcpServer->setSpacePacketParsingOptions({common::PUS_PACKET_ID, common::CFDP_PACKET_ID}); @@ -114,7 +114,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun tcpServer->enableWiretapping(true); #endif /* OBSW_TCP_SERVER_WIRETAPPING == 1 */ #endif /* OBSW_USE_TMTC_TCP_BRIDGE == 0 */ - tcpIpTmtcBridge->setMaxNumberOfPacketsStored(150); + tmtcBridge->setMaxNumberOfPacketsStored(150); #endif /* OBSW_ADD_TCPIP_BRIDGE == 1 */ auto* ccsdsDistrib = @@ -124,8 +124,8 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmStore, 50); *pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, *timeStamper, *tmStore, 80); #if OBSW_ADD_TCPIP_BRIDGE == 1 - (*cfdpFunnel)->addDestination(*tcpIpTmtcBridge, 0); - (*pusFunnel)->addDestination(*tcpIpTmtcBridge, 0); + (*cfdpFunnel)->addDestination(*tmtcBridge, 0); + (*pusFunnel)->addDestination(*tmtcBridge, 0); #endif // Every TM packet goes through this funnel new TmFunnelHandler(objects::TM_FUNNEL, **pusFunnel, **cfdpFunnel); From f96d831cc92d2f55e808063f2786f17162e320c9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 16:46:47 +0100 Subject: [PATCH 142/244] simiplify hosted SW --- bsp_hosted/InitMission.cpp | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/bsp_hosted/InitMission.cpp b/bsp_hosted/InitMission.cpp index 4ee90f70..1301ff51 100644 --- a/bsp_hosted/InitMission.cpp +++ b/bsp_hosted/InitMission.cpp @@ -53,28 +53,25 @@ void initmission::initTasks() { #endif /* TMTC Distribution */ - PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask( - "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); - ReturnValue_t result = tmTcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); + PeriodicTaskIF* tmtcDistributor = factory->createPeriodicTask( + "DIST", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); + ReturnValue_t result = tmtcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } - result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); + result = tmtcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } - result = tmTcDistributor->addComponent(objects::TM_FUNNEL); + result = tmtcDistributor->addComponent(objects::TM_FUNNEL); if (result != returnvalue::OK) { sif::error << "Object add component failed" << std::endl; } - - /* UDP bridge */ - PeriodicTaskIF* tmtcBridgeTask = factory->createPeriodicTask( - "TMTC_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); - result = tmtcBridgeTask->addComponent(objects::TMTC_BRIDGE); + result = tmtcDistributor->addComponent(objects::TMTC_BRIDGE); if (result != returnvalue::OK) { sif::error << "Add component UDP Unix Bridge failed" << std::endl; } + PeriodicTaskIF* tmtcPollingTask = factory->createPeriodicTask( "UDP_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = tmtcPollingTask->addComponent(objects::TMTC_POLLING_TASK); @@ -174,8 +171,7 @@ void initmission::initTasks() { #endif /* OBSW_ADD_TEST_CODE == 1 */ sif::info << "Starting tasks.." << std::endl; - tmTcDistributor->startTask(); - tmtcBridgeTask->startTask(); + tmtcDistributor->startTask(); tmtcPollingTask->startTask(); pusVerification->startTask(); From b9f4a7be0ce186796adfbcf0bbfecce99a233b2d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 16:59:33 +0100 Subject: [PATCH 143/244] dumb undefined reference --- bsp_hosted/InitMission.cpp | 13 +++++++++++++ bsp_hosted/InitMission.h | 5 +---- linux/InitMission.h | 1 + 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/bsp_hosted/InitMission.cpp b/bsp_hosted/InitMission.cpp index 1301ff51..a10d2f79 100644 --- a/bsp_hosted/InitMission.cpp +++ b/bsp_hosted/InitMission.cpp @@ -10,6 +10,7 @@ #include #include #include +#include "linux/InitMission.h" #include @@ -163,6 +164,18 @@ void initmission::initTasks() { sif::error << "Failed to add dummy pst to fixed timeslot task" << std::endl; } +#if OBSW_ADD_PLOC_SUPERVISOR == 1 + PeriodicTaskIF* supvHelperTask = factory->createPeriodicTask( + "PLOC_SUPV_HELPER", 10, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); + result = supvHelperTask->addComponent(objects::PLOC_SUPERVISOR_HELPER); + if (result != returnvalue::OK) { + initmission::printAddObjectError("PLOC_SUPV_HELPER", objects::PLOC_SUPERVISOR_HELPER); + } +#endif /* OBSW_ADD_PLOC_SUPERVISOR */ + + PeriodicTaskIF* plTask = factory->createPeriodicTask( + "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); + scheduling::addMpsocSupvHandlers(plTask); #if OBSW_ADD_TEST_CODE == 1 result = testTask->addComponent(objects::TEST_TASK); if (result != returnvalue::OK) { diff --git a/bsp_hosted/InitMission.h b/bsp_hosted/InitMission.h index 507de592..41da5db2 100644 --- a/bsp_hosted/InitMission.h +++ b/bsp_hosted/InitMission.h @@ -1,9 +1,6 @@ -#ifndef BSP_LINUX_INITMISSION_H_ -#define BSP_LINUX_INITMISSION_H_ +#pragma once namespace initmission { void initMission(); void initTasks(); }; // namespace initmission - -#endif /* BSP_LINUX_INITMISSION_H_ */ diff --git a/linux/InitMission.h b/linux/InitMission.h index dd809ff7..b5ffc415 100644 --- a/linux/InitMission.h +++ b/linux/InitMission.h @@ -1,4 +1,5 @@ #pragma once + #include namespace scheduling { From 99f92a87da7eaf127f21ea9c30729c8c0d91091b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:03:46 +0100 Subject: [PATCH 144/244] rename scheduling file --- bsp_hosted/InitMission.cpp | 37 +++++++++++------------ bsp_hosted/InitMission.h | 2 +- bsp_hosted/main.cpp | 2 +- linux/CMakeLists.txt | 2 +- linux/{InitMission.cpp => scheduling.cpp} | 4 +-- linux/{InitMission.h => scheduling.h} | 0 mission/utility/InitMission.h | 2 +- 7 files changed, 24 insertions(+), 25 deletions(-) rename linux/{InitMission.cpp => scheduling.cpp} (98%) rename linux/{InitMission.h => scheduling.h} (100%) diff --git a/bsp_hosted/InitMission.cpp b/bsp_hosted/InitMission.cpp index a10d2f79..016985f4 100644 --- a/bsp_hosted/InitMission.cpp +++ b/bsp_hosted/InitMission.cpp @@ -10,10 +10,9 @@ #include #include #include -#include "linux/InitMission.h" - #include +#include "../linux/scheduling.h" #include "ObjectFactory.h" #ifdef LINUX @@ -30,7 +29,7 @@ ServiceInterfaceStream sif::error("ERROR", true, false, true); ObjectManagerIF* objectManager = nullptr; -void initmission::initMission() { +void scheduling::initMission() { sif::info << "Building global objects.." << std::endl; /* Instantiate global object manager and also create all objects */ ObjectManager::instance()->setObjectFactoryFunction(ObjectFactory::produce, nullptr); @@ -41,7 +40,7 @@ void initmission::initMission() { initTasks(); } -void initmission::initTasks() { +void scheduling::initTasks() { TaskFactory* factory = TaskFactory::instance(); if (factory == nullptr) { /* Should never happen ! */ @@ -92,69 +91,69 @@ void initmission::initTasks() { "EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = eventHandling->addComponent(objects::EVENT_MANAGER); if (result != returnvalue::OK) { - initmission::printAddObjectError("EVENT_MNGR", objects::EVENT_MANAGER); + scheduling::printAddObjectError("EVENT_MNGR", objects::EVENT_MANAGER); } result = eventHandling->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS5", objects::PUS_SERVICE_5_EVENT_REPORTING); + scheduling::printAddObjectError("PUS5", objects::PUS_SERVICE_5_EVENT_REPORTING); } PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask( "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS); + scheduling::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT); + scheduling::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS3", objects::PUS_SERVICE_3_HOUSEKEEPING); + scheduling::printAddObjectError("PUS3", objects::PUS_SERVICE_3_HOUSEKEEPING); } PeriodicTaskIF* pusMedPrio = factory->createPeriodicTask( "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc); result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT); + scheduling::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT); + scheduling::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS); + scheduling::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS); } PeriodicTaskIF* pusLowPrio = factory->createPeriodicTask( "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST); + scheduling::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST); } PeriodicTaskIF* thermalTask = factory->createPeriodicTask( "THERMAL_CTL_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = thermalTask->addComponent(objects::RTD_0_IC3_PLOC_HEATSPREADER); if (result != returnvalue::OK) { - initmission::printAddObjectError("RTD_0_dummy", objects::RTD_0_IC3_PLOC_HEATSPREADER); + scheduling::printAddObjectError("RTD_0_dummy", objects::RTD_0_IC3_PLOC_HEATSPREADER); } result = thermalTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF); if (result != returnvalue::OK) { - initmission::printAddObjectError("SUS_0_dummy", objects::SUS_0_N_LOC_XFYFZM_PT_XF); + scheduling::printAddObjectError("SUS_0_dummy", objects::SUS_0_N_LOC_XFYFZM_PT_XF); } result = thermalTask->addComponent(objects::CORE_CONTROLLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("Core controller dummy", objects::CORE_CONTROLLER); + scheduling::printAddObjectError("Core controller dummy", objects::CORE_CONTROLLER); } result = thermalTask->addComponent(objects::THERMAL_CONTROLLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("THERMAL_CONTROLLER", objects::THERMAL_CONTROLLER); + scheduling::printAddObjectError("THERMAL_CONTROLLER", objects::THERMAL_CONTROLLER); } FixedTimeslotTaskIF* pstTask = factory->createFixedTimeslotTask( @@ -169,7 +168,7 @@ void initmission::initTasks() { "PLOC_SUPV_HELPER", 10, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = supvHelperTask->addComponent(objects::PLOC_SUPERVISOR_HELPER); if (result != returnvalue::OK) { - initmission::printAddObjectError("PLOC_SUPV_HELPER", objects::PLOC_SUPERVISOR_HELPER); + scheduling::printAddObjectError("PLOC_SUPV_HELPER", objects::PLOC_SUPERVISOR_HELPER); } #endif /* OBSW_ADD_PLOC_SUPERVISOR */ @@ -179,7 +178,7 @@ void initmission::initTasks() { #if OBSW_ADD_TEST_CODE == 1 result = testTask->addComponent(objects::TEST_TASK); if (result != returnvalue::OK) { - initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK); + scheduling::printAddObjectError("TEST_TASK", objects::TEST_TASK); } #endif /* OBSW_ADD_TEST_CODE == 1 */ diff --git a/bsp_hosted/InitMission.h b/bsp_hosted/InitMission.h index 41da5db2..e1ba3a4a 100644 --- a/bsp_hosted/InitMission.h +++ b/bsp_hosted/InitMission.h @@ -1,6 +1,6 @@ #pragma once -namespace initmission { +namespace scheduling { void initMission(); void initTasks(); }; // namespace initmission diff --git a/bsp_hosted/main.cpp b/bsp_hosted/main.cpp index e493c1c9..0e1fc01c 100644 --- a/bsp_hosted/main.cpp +++ b/bsp_hosted/main.cpp @@ -35,7 +35,7 @@ int main(void) { << " BSP HOSTED" << " --" << std::endl; - initmission::initMission(); + scheduling::initMission(); for (;;) { // suspend main thread by sleeping it. diff --git a/linux/CMakeLists.txt b/linux/CMakeLists.txt index f8c8935f..7f6ea0bc 100644 --- a/linux/CMakeLists.txt +++ b/linux/CMakeLists.txt @@ -9,4 +9,4 @@ if(EIVE_ADD_LINUX_FSFWCONFIG) add_subdirectory(fsfwconfig) endif() -target_sources(${OBSW_NAME} PUBLIC ObjectFactory.cpp InitMission.cpp) +target_sources(${OBSW_NAME} PUBLIC ObjectFactory.cpp scheduling.cpp) diff --git a/linux/InitMission.cpp b/linux/scheduling.cpp similarity index 98% rename from linux/InitMission.cpp rename to linux/scheduling.cpp index 4b7dc1f1..9a4c1a93 100644 --- a/linux/InitMission.cpp +++ b/linux/scheduling.cpp @@ -1,4 +1,4 @@ -#include "InitMission.h" +#include "scheduling.h" #include #include @@ -10,7 +10,7 @@ void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, PeriodicTaskIF*& scexReaderTask) { - using namespace initmission; + using namespace scheduling; ReturnValue_t result = returnvalue::OK; #if OBSW_PRINT_MISSED_DEADLINES == 1 void (*missedDeadlineFunc)(void) = TaskFactory::printMissedDeadline; diff --git a/linux/InitMission.h b/linux/scheduling.h similarity index 100% rename from linux/InitMission.h rename to linux/scheduling.h diff --git a/mission/utility/InitMission.h b/mission/utility/InitMission.h index 5529f749..c0663544 100644 --- a/mission/utility/InitMission.h +++ b/mission/utility/InitMission.h @@ -3,7 +3,7 @@ #include #include -namespace initmission { +namespace scheduling { static void printAddObjectError(const char* name, object_id_t objectId) { #if FSFW_CPP_OSTREAM_ENABLED == 1 From 9481cc2eb05c2df35cd4fcc039f09c593e042fff Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:08:10 +0100 Subject: [PATCH 145/244] rename bsp hosted scheduling file --- bsp_hosted/CMakeLists.txt | 2 +- bsp_hosted/main.cpp | 2 +- bsp_hosted/{InitMission.cpp => scheduling.cpp} | 10 +++++----- bsp_hosted/{InitMission.h => scheduling.h} | 0 4 files changed, 7 insertions(+), 7 deletions(-) rename bsp_hosted/{InitMission.cpp => scheduling.cpp} (99%) rename bsp_hosted/{InitMission.h => scheduling.h} (100%) diff --git a/bsp_hosted/CMakeLists.txt b/bsp_hosted/CMakeLists.txt index 2804977d..ec2016b4 100644 --- a/bsp_hosted/CMakeLists.txt +++ b/bsp_hosted/CMakeLists.txt @@ -1,4 +1,4 @@ -target_sources(${OBSW_NAME} PUBLIC InitMission.cpp main.cpp ObjectFactory.cpp) +target_sources(${OBSW_NAME} PUBLIC scheduling.cpp main.cpp ObjectFactory.cpp) add_subdirectory(fsfwconfig) add_subdirectory(boardconfig) diff --git a/bsp_hosted/main.cpp b/bsp_hosted/main.cpp index 0e1fc01c..83f6777b 100644 --- a/bsp_hosted/main.cpp +++ b/bsp_hosted/main.cpp @@ -2,7 +2,6 @@ #include -#include "InitMission.h" #include "commonConfig.h" #include "fsfw/FSFWVersion.h" #include "fsfw/controller/ControllerBase.h" @@ -11,6 +10,7 @@ #include "fsfw/modes/ModeMessage.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/tasks/TaskFactory.h" +#include "scheduling.h" #ifdef WIN32 static const char* COMPILE_PRINTOUT = "Windows"; diff --git a/bsp_hosted/InitMission.cpp b/bsp_hosted/scheduling.cpp similarity index 99% rename from bsp_hosted/InitMission.cpp rename to bsp_hosted/scheduling.cpp index 016985f4..bb50530b 100644 --- a/bsp_hosted/InitMission.cpp +++ b/bsp_hosted/scheduling.cpp @@ -1,6 +1,8 @@ -#include "InitMission.h" +#include "linux/scheduling.h" +#include "scheduling.h" +#include "ObjectFactory.h" +#include "OBSWConfig.h" -#include #include #include #include @@ -10,10 +12,8 @@ #include #include #include -#include -#include "../linux/scheduling.h" -#include "ObjectFactory.h" +#include #ifdef LINUX ServiceInterfaceStream sif::debug("DEBUG"); diff --git a/bsp_hosted/InitMission.h b/bsp_hosted/scheduling.h similarity index 100% rename from bsp_hosted/InitMission.h rename to bsp_hosted/scheduling.h From 23cfe3aa69b575c950b652d7ffb29d38ac30aeb1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:15:14 +0100 Subject: [PATCH 146/244] unbelievable --- bsp_hosted/scheduling.cpp | 2 +- bsp_hosted/scheduling.h | 5 ++++- linux/scheduling.h | 6 ++++-- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/bsp_hosted/scheduling.cpp b/bsp_hosted/scheduling.cpp index bb50530b..20b8ba9c 100644 --- a/bsp_hosted/scheduling.cpp +++ b/bsp_hosted/scheduling.cpp @@ -174,7 +174,7 @@ void scheduling::initTasks() { PeriodicTaskIF* plTask = factory->createPeriodicTask( "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); - scheduling::addMpsocSupvHandlers(plTask); + //scheduling::addMpsocSupvHandlers(plTask); #if OBSW_ADD_TEST_CODE == 1 result = testTask->addComponent(objects::TEST_TASK); if (result != returnvalue::OK) { diff --git a/bsp_hosted/scheduling.h b/bsp_hosted/scheduling.h index e1ba3a4a..fabec0ce 100644 --- a/bsp_hosted/scheduling.h +++ b/bsp_hosted/scheduling.h @@ -1,6 +1,9 @@ -#pragma once +#ifndef _BSP_HOSTED_SCHEDULING_H_ +#define _BSP_HOSTED_SCHEDULING_H_ namespace scheduling { void initMission(); void initTasks(); }; // namespace initmission + +#endif diff --git a/linux/scheduling.h b/linux/scheduling.h index b5ffc415..ef15cd23 100644 --- a/linux/scheduling.h +++ b/linux/scheduling.h @@ -1,4 +1,5 @@ -#pragma once +#ifndef _LINUX_SCHEDULING_H +#define _LINUX_SCHEDULING_H #include @@ -6,5 +7,6 @@ namespace scheduling { void schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, PeriodicTaskIF*& scexReaderTask); void addMpsocSupvHandlers(PeriodicTaskIF* task); - } // namespace scheduling + +#endif From e5ec8a74901f586b569cfd994f3a59c7efbe7c0f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:21:12 +0100 Subject: [PATCH 147/244] now the linux files are compiling --- CMakeLists.txt | 3 +++ bsp_hosted/scheduling.cpp | 9 +++++---- bsp_hosted/scheduling.h | 2 +- linux/devices/ploc/PlocSupvUartMan.cpp | 2 +- mission/utility/InitMission.h | 2 +- 5 files changed, 11 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 97b7601c..0753ea9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -296,6 +296,9 @@ if(TGT_BSP) else() # Required by FSFW library set(FSFW_CONFIG_PATH "${BSP_PATH}/fsfwconfig") + if(UNIX) + set(EIVE_ADD_LINUX_FILES ON) + endif() endif() # Configuration files diff --git a/bsp_hosted/scheduling.cpp b/bsp_hosted/scheduling.cpp index 20b8ba9c..9c239eb3 100644 --- a/bsp_hosted/scheduling.cpp +++ b/bsp_hosted/scheduling.cpp @@ -1,7 +1,4 @@ #include "linux/scheduling.h" -#include "scheduling.h" -#include "ObjectFactory.h" -#include "OBSWConfig.h" #include #include @@ -15,6 +12,10 @@ #include +#include "OBSWConfig.h" +#include "ObjectFactory.h" +#include "scheduling.h" + #ifdef LINUX ServiceInterfaceStream sif::debug("DEBUG"); ServiceInterfaceStream sif::info("INFO"); @@ -174,7 +175,7 @@ void scheduling::initTasks() { PeriodicTaskIF* plTask = factory->createPeriodicTask( "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); - //scheduling::addMpsocSupvHandlers(plTask); + scheduling::addMpsocSupvHandlers(plTask); #if OBSW_ADD_TEST_CODE == 1 result = testTask->addComponent(objects::TEST_TASK); if (result != returnvalue::OK) { diff --git a/bsp_hosted/scheduling.h b/bsp_hosted/scheduling.h index fabec0ce..2f98ade2 100644 --- a/bsp_hosted/scheduling.h +++ b/bsp_hosted/scheduling.h @@ -4,6 +4,6 @@ namespace scheduling { void initMission(); void initTasks(); -}; // namespace initmission +}; // namespace scheduling #endif diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 26da089b..3189cb65 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -42,7 +42,7 @@ PlocSupvHelper::PlocSupvHelper(object_id_t objectId) PlocSupvHelper::~PlocSupvHelper() = default; ReturnValue_t PlocSupvHelper::initializeInterface(CookieIF* cookie) { - UartCookie* uartCookie = dynamic_cast(cookie); + auto* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { return FAILED; } diff --git a/mission/utility/InitMission.h b/mission/utility/InitMission.h index c0663544..85d8ae21 100644 --- a/mission/utility/InitMission.h +++ b/mission/utility/InitMission.h @@ -16,4 +16,4 @@ static void printAddObjectError(const char* name, object_id_t objectId) { #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ } -} // namespace initmission +} // namespace scheduling From 7fb689b45159fbe94c7c2f182eaa223a282d4497 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:22:28 +0100 Subject: [PATCH 148/244] it was not the #pragma once --- bsp_hosted/scheduling.h | 5 +---- linux/scheduling.h | 5 +---- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/bsp_hosted/scheduling.h b/bsp_hosted/scheduling.h index 2f98ade2..2a2f32a0 100644 --- a/bsp_hosted/scheduling.h +++ b/bsp_hosted/scheduling.h @@ -1,9 +1,6 @@ -#ifndef _BSP_HOSTED_SCHEDULING_H_ -#define _BSP_HOSTED_SCHEDULING_H_ +#pragma once namespace scheduling { void initMission(); void initTasks(); }; // namespace scheduling - -#endif diff --git a/linux/scheduling.h b/linux/scheduling.h index ef15cd23..d33e9d1f 100644 --- a/linux/scheduling.h +++ b/linux/scheduling.h @@ -1,5 +1,4 @@ -#ifndef _LINUX_SCHEDULING_H -#define _LINUX_SCHEDULING_H +#pragma once #include @@ -8,5 +7,3 @@ void schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHandler, PeriodicTaskIF*& scexReaderTask); void addMpsocSupvHandlers(PeriodicTaskIF* task); } // namespace scheduling - -#endif From 38e74e6eaf099b2d86dce16148359ae6a22734e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:24:38 +0100 Subject: [PATCH 149/244] rename ploc supv uart man --- bsp_hosted/ObjectFactory.cpp | 2 +- bsp_hosted/scheduling.cpp | 6 ++ linux/devices/ploc/PlocSupervisorHandler.cpp | 20 ++-- linux/devices/ploc/PlocSupervisorHandler.h | 4 +- linux/devices/ploc/PlocSupvUartMan.cpp | 99 ++++++++++---------- linux/devices/ploc/PlocSupvUartMan.h | 10 +- 6 files changed, 74 insertions(+), 67 deletions(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 01d6da22..c384c7ad 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -98,7 +98,7 @@ void ObjectFactory::produce(void* args) { new UartCookie(objects::PLOC_SUPERVISOR_HANDLER, plocSupvString, uart::PLOC_SUPV_BAUD, supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); supervisorCookie->setNoFixedSizeReply(); - auto supvHelper = new PlocSupvHelper(objects::PLOC_SUPERVISOR_HELPER); + auto supvHelper = new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER); new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, objects::PLOC_SUPERVISOR_HELPER, supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, dummyGpioIF), pcdu::PDU1_CH6_PLOC_12V, supvHelper); diff --git a/bsp_hosted/scheduling.cpp b/bsp_hosted/scheduling.cpp index 9c239eb3..d7238a80 100644 --- a/bsp_hosted/scheduling.cpp +++ b/bsp_hosted/scheduling.cpp @@ -195,6 +195,12 @@ void scheduling::initTasks() { pstTask->startTask(); thermalTask->startTask(); +#if OBSW_ADD_PLOC_SUPERVISOR == 1 + supvHelperTask->startTask(); +#endif +#if OBSW_ADD_PLOC_SUPERVISOR == 1 || OBSW_ADD_PLOC_MPSOC == 1 + plTask->startTask(); +#endif #if OBSW_ADD_TEST_CODE == 1 testTask->startTask(); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index f5f333fa..9e9653ed 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -20,7 +20,7 @@ using namespace returnvalue; PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, Gpio uartIsolatorSwitch, power::Switch_t powerSwitch, - PlocSupvHelper* supvHelper) + PlocSupvUartManager* supvHelper) : DeviceHandlerBase(objectId, uartComIFid, comCookie), uartIsolatorSwitch(uartIsolatorSwitch), hkset(this), @@ -796,12 +796,12 @@ void PlocSupervisorHandler::handleEvent(EventMessage* eventMessage) { plocSupvHelperExecuting = false; // After execution of update procedure, PLOC is in a state where it draws approx. 700 mA of // current. To leave this state the shutdown MPSoC command must be sent here. - if (event == PlocSupvHelper::SUPV_UPDATE_FAILED || - event == PlocSupvHelper::SUPV_UPDATE_SUCCESSFUL || - event == PlocSupvHelper::SUPV_CONTINUE_UPDATE_FAILED || - event == PlocSupvHelper::SUPV_CONTINUE_UPDATE_SUCCESSFUL || - event == PlocSupvHelper::SUPV_MEM_CHECK_FAIL || - event == PlocSupvHelper::SUPV_MEM_CHECK_OK) { + if (event == PlocSupvUartManager::SUPV_UPDATE_FAILED || + event == PlocSupvUartManager::SUPV_UPDATE_SUCCESSFUL || + event == PlocSupvUartManager::SUPV_CONTINUE_UPDATE_FAILED || + event == PlocSupvUartManager::SUPV_CONTINUE_UPDATE_SUCCESSFUL || + event == PlocSupvUartManager::SUPV_MEM_CHECK_FAIL || + event == PlocSupvUartManager::SUPV_MEM_CHECK_OK) { result = this->executeAction(supv::SHUTDOWN_MPSOC, NO_COMMANDER, nullptr, 0); if (result != returnvalue::OK) { triggerEvent(SUPV_MPSOC_SHUWDOWN_BUILD_FAILED); @@ -1856,9 +1856,9 @@ ReturnValue_t PlocSupervisorHandler::eventSubscription() { if (result != returnvalue::OK) { return result; } - result = manager->subscribeToEventRange(eventQueue->getId(), - event::getEventId(PlocSupvHelper::SUPV_UPDATE_FAILED), - event::getEventId(PlocSupvHelper::SUPV_MEM_CHECK_FAIL)); + result = manager->subscribeToEventRange( + eventQueue->getId(), event::getEventId(PlocSupvUartManager::SUPV_UPDATE_FAILED), + event::getEventId(PlocSupvUartManager::SUPV_MEM_CHECK_FAIL)); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PlocSupervisorHandler::eventSubscritpion: Failed to subscribe to events from " diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 27f80ae0..a152349c 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -34,7 +34,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { public: PlocSupervisorHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, Gpio uartIsolatorSwitch, power::Switch_t powerSwitch, - PlocSupvHelper* supvHelper); + PlocSupvUartManager* supvHelper); virtual ~PlocSupervisorHandler(); virtual ReturnValue_t initialize() override; @@ -130,7 +130,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { const power::Switch_t powerSwitch = power::NO_SWITCH; supv::TmBase tmReader; - PlocSupvHelper* supvHelper = nullptr; + PlocSupvUartManager* supvHelper = nullptr; MessageQueueIF* eventQueue = nullptr; /** Number of expected replies following the MRAM dump command */ diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 3189cb65..f17033a7 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -27,7 +27,7 @@ using namespace returnvalue; using namespace supv; -PlocSupvHelper::PlocSupvHelper(object_id_t objectId) +PlocSupvUartManager::PlocSupvUartManager(object_id_t objectId) : SystemObject(objectId), recRingBuf(4096, true), decodedRingBuf(1200 * MAX_STORED_DECODED_PACKETS, true), @@ -39,9 +39,9 @@ PlocSupvHelper::PlocSupvHelper(object_id_t objectId) ipcLock = MutexFactory::instance()->createMutex(); } -PlocSupvHelper::~PlocSupvHelper() = default; +PlocSupvUartManager::~PlocSupvUartManager() = default; -ReturnValue_t PlocSupvHelper::initializeInterface(CookieIF* cookie) { +ReturnValue_t PlocSupvUartManager::initializeInterface(CookieIF* cookie) { auto* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { return FAILED; @@ -80,7 +80,7 @@ ReturnValue_t PlocSupvHelper::initializeInterface(CookieIF* cookie) { return OK; } -ReturnValue_t PlocSupvHelper::initialize() { +ReturnValue_t PlocSupvUartManager::initialize() { #ifdef XIPHOS_Q7S sdcMan = SdCardManager::instance(); if (sdcMan == nullptr) { @@ -91,7 +91,7 @@ ReturnValue_t PlocSupvHelper::initialize() { return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { +ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { bool putTaskToSleep = false; while (true) { semaphore->acquire(); @@ -129,7 +129,7 @@ ReturnValue_t PlocSupvHelper::performOperation(uint8_t operationCode) { } } -bool PlocSupvHelper::handleUartReception() { +bool PlocSupvUartManager::handleUartReception() { ReturnValue_t result = OK; ssize_t bytesRead = read(serialPort, reinterpret_cast(recBuf.data()), static_cast(recBuf.size())); @@ -160,8 +160,8 @@ bool PlocSupvHelper::handleUartReception() { return false; } -ReturnValue_t PlocSupvHelper::startUpdate(std::string file, uint8_t memoryId, - uint32_t startAddress) { +ReturnValue_t PlocSupvUartManager::startUpdate(std::string file, uint8_t memoryId, + uint32_t startAddress) { supv::UpdateParams params; params.file = file; params.memId = memoryId; @@ -172,7 +172,7 @@ ReturnValue_t PlocSupvHelper::startUpdate(std::string file, uint8_t memoryId, return performUpdate(params); } -ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { +ReturnValue_t PlocSupvUartManager::performUpdate(const supv::UpdateParams& params) { lock->lockMutex(); InternalState current = state; lock->unlockMutex(); @@ -226,8 +226,8 @@ ReturnValue_t PlocSupvHelper::performUpdate(const supv::UpdateParams& params) { return result; } -ReturnValue_t PlocSupvHelper::performMemCheck(std::string file, uint8_t memoryId, - uint32_t startAddress) { +ReturnValue_t PlocSupvUartManager::performMemCheck(std::string file, uint8_t memoryId, + uint32_t startAddress) { lock->lockMutex(); InternalState current = state; lock->unlockMutex(); @@ -237,9 +237,9 @@ ReturnValue_t PlocSupvHelper::performMemCheck(std::string file, uint8_t memoryId return performMemCheck(file, memoryId, startAddress, getFileSize(update.file), true); } -ReturnValue_t PlocSupvHelper::performMemCheck(std::string file, uint8_t memoryId, - uint32_t startAddress, size_t sizeToCheck, - bool checkCrc) { +ReturnValue_t PlocSupvUartManager::performMemCheck(std::string file, uint8_t memoryId, + uint32_t startAddress, size_t sizeToCheck, + bool checkCrc) { { MutexGuard mg(lock); update.file = file; @@ -254,7 +254,7 @@ ReturnValue_t PlocSupvHelper::performMemCheck(std::string file, uint8_t memoryId return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::initiateUpdateContinuation() { +ReturnValue_t PlocSupvUartManager::initiateUpdateContinuation() { lock->lockMutex(); InternalState current = state; lock->unlockMutex(); @@ -284,12 +284,12 @@ ReturnValue_t PlocSupvHelper::initiateUpdateContinuation() { // return returnvalue::OK; // } -void PlocSupvHelper::stop() { +void PlocSupvUartManager::stop() { MutexGuard mg(lock); state = InternalState::GO_TO_SLEEP; } -void PlocSupvHelper::executeFullCheckMemoryCommand() { +void PlocSupvUartManager::executeFullCheckMemoryCommand() { ReturnValue_t result; if (update.crcShouldBeChecked) { sif::info << "PLOC SUPV Mem Check: Calculating Image CRC" << std::endl; @@ -315,7 +315,7 @@ void PlocSupvHelper::executeFullCheckMemoryCommand() { handleCheckMemoryCommand(); } -ReturnValue_t PlocSupvHelper::executeUpdate() { +ReturnValue_t PlocSupvUartManager::executeUpdate() { ReturnValue_t result = returnvalue::OK; sif::info << "PLOC SUPV Update MPSoC: Calculating Image CRC" << std::endl; result = calcImageCrc(); @@ -342,7 +342,7 @@ ReturnValue_t PlocSupvHelper::executeUpdate() { return updateOperation(); } -ReturnValue_t PlocSupvHelper::continueUpdate() { +ReturnValue_t PlocSupvUartManager::continueUpdate() { ReturnValue_t result = prepareUpdate(); if (result != returnvalue::OK) { return result; @@ -350,7 +350,7 @@ ReturnValue_t PlocSupvHelper::continueUpdate() { return updateOperation(); } -ReturnValue_t PlocSupvHelper::updateOperation() { +ReturnValue_t PlocSupvUartManager::updateOperation() { sif::info << "PlocSupvHelper::performUpdate: Writing Update Packets" << std::endl; auto result = writeUpdatePackets(); if (result != returnvalue::OK) { @@ -360,7 +360,7 @@ ReturnValue_t PlocSupvHelper::updateOperation() { return handleCheckMemoryCommand(); } -ReturnValue_t PlocSupvHelper::writeUpdatePackets() { +ReturnValue_t PlocSupvUartManager::writeUpdatePackets() { ReturnValue_t result = returnvalue::OK; #if OBSW_DEBUG_PLOC_SUPERVISOR == 1 ProgressPrinter progressPrinter("Supervisor update", update.fullFileSize, @@ -437,7 +437,7 @@ ReturnValue_t PlocSupvHelper::writeUpdatePackets() { return result; } -uint32_t PlocSupvHelper::buildProgParams1(uint8_t percent, uint16_t seqCount) { +uint32_t PlocSupvUartManager::buildProgParams1(uint8_t percent, uint16_t seqCount) { return (static_cast(percent) << 24) | static_cast(seqCount); } @@ -482,7 +482,7 @@ uint32_t PlocSupvHelper::buildProgParams1(uint8_t percent, uint16_t seqCount) { // return result; // } -ReturnValue_t PlocSupvHelper::handleRemainingExeReport(ploc::SpTmReader& reader) { +ReturnValue_t PlocSupvUartManager::handleRemainingExeReport(ploc::SpTmReader& reader) { size_t remBytes = reader.getPacketDataLen() + 1; ReturnValue_t result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN); if (result != returnvalue::OK) { @@ -495,7 +495,7 @@ ReturnValue_t PlocSupvHelper::handleRemainingExeReport(ploc::SpTmReader& reader) return result; } -ReturnValue_t PlocSupvHelper::selectMemory() { +ReturnValue_t PlocSupvUartManager::selectMemory() { ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::MPSoCBootSelect packet(spParams); @@ -510,7 +510,7 @@ ReturnValue_t PlocSupvHelper::selectMemory() { return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::prepareUpdate() { +ReturnValue_t PlocSupvUartManager::prepareUpdate() { ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::NoPayloadPacket packet(spParams, Apid::BOOT_MAN, @@ -526,7 +526,7 @@ ReturnValue_t PlocSupvHelper::prepareUpdate() { return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::eraseMemory() { +ReturnValue_t PlocSupvUartManager::eraseMemory() { ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::EraseMemory eraseMemory(spParams); @@ -542,8 +542,8 @@ ReturnValue_t PlocSupvHelper::eraseMemory() { return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::handlePacketTransmissionNoReply(supv::TcBase& packet, - uint32_t timeoutExecutionReport) { +ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( + supv::TcBase& packet, uint32_t timeoutExecutionReport) { ReturnValue_t result = returnvalue::OK; result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); if (result != returnvalue::OK) { @@ -599,7 +599,7 @@ ReturnValue_t PlocSupvHelper::handlePacketTransmissionNoReply(supv::TcBase& pack return returnvalue::OK; } -int PlocSupvHelper::handleAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen) { +int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen) { if (serviceId == static_cast(supv::tm::TmtcId::ACK) or serviceId == static_cast(supv::tm::TmtcId::NAK)) { AcknowledgmentReport ackReport(tmReader); @@ -629,7 +629,8 @@ int PlocSupvHelper::handleAckReception(supv::TcBase& tc, uint8_t serviceId, size return 0; } -int PlocSupvHelper::handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen) { +int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, + size_t packetLen) { if (serviceId == static_cast(supv::tm::TmtcId::EXEC_ACK) or serviceId == static_cast(supv::tm::TmtcId::EXEC_NAK)) { ExecutionReport exeReport(tmReader); @@ -659,7 +660,7 @@ int PlocSupvHelper::handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, s return 0; } -ReturnValue_t PlocSupvHelper::checkReceivedTm() { +ReturnValue_t PlocSupvUartManager::checkReceivedTm() { ReturnValue_t result = tmReader.checkSize(); if (result != returnvalue::OK) { triggerEvent(SUPV_REPLY_SIZE_MISSMATCH, rememberApid); @@ -673,7 +674,7 @@ ReturnValue_t PlocSupvHelper::checkReceivedTm() { return result; } -ReturnValue_t PlocSupvHelper::calcImageCrc() { +ReturnValue_t PlocSupvUartManager::calcImageCrc() { ReturnValue_t result = returnvalue::OK; if (update.fullFileSize == 0) { return returnvalue::FAILED; @@ -719,7 +720,7 @@ ReturnValue_t PlocSupvHelper::calcImageCrc() { return result; } -ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { +ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::CheckMemory packet(spParams); @@ -815,7 +816,7 @@ ReturnValue_t PlocSupvHelper::handleCheckMemoryCommand() { return result; } -uint32_t PlocSupvHelper::getFileSize(std::string filename) { +uint32_t PlocSupvUartManager::getFileSize(std::string filename) { std::ifstream file(filename, std::ifstream::binary); file.seekg(0, file.end); uint32_t size = file.tellg(); @@ -823,7 +824,7 @@ uint32_t PlocSupvHelper::getFileSize(std::string filename) { return size; } -ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reader) { +ReturnValue_t PlocSupvUartManager::handleEventBufferReception(ploc::SpTmReader& reader) { ReturnValue_t result = returnvalue::OK; // TODO: Fix //#ifdef XIPHOS_Q7S @@ -879,10 +880,10 @@ ReturnValue_t PlocSupvHelper::handleEventBufferReception(ploc::SpTmReader& reade return result; } -void PlocSupvHelper::resetSpParams() { spParams.buf = cmdBuf.data(); } +void PlocSupvUartManager::resetSpParams() { spParams.buf = cmdBuf.data(); } -ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendData, - size_t sendLen) { +ReturnValue_t PlocSupvUartManager::sendMessage(CookieIF* cookie, const uint8_t* sendData, + size_t sendLen) { if (sendData == nullptr or sendLen == 0) { return FAILED; } @@ -895,13 +896,13 @@ ReturnValue_t PlocSupvHelper::sendMessage(CookieIF* cookie, const uint8_t* sendD return encodeAndSendPacket(sendData, sendLen); } -ReturnValue_t PlocSupvHelper::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } +ReturnValue_t PlocSupvUartManager::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { +ReturnValue_t PlocSupvUartManager::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::handleRunningLongerRequest() { +ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() { ReturnValue_t result = OK; switch (request) { case Request::UPDATE: { @@ -949,7 +950,7 @@ ReturnValue_t PlocSupvHelper::handleRunningLongerRequest() { return false; } -ReturnValue_t PlocSupvHelper::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) { +ReturnValue_t PlocSupvUartManager::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) { size_t encodedLen = 0; hdlc_add_framing(sendData, sendLen, encodedSendBuf.data(), &encodedLen); size_t bytesWritten = write(serialPort, encodedSendBuf.data(), encodedLen); @@ -961,8 +962,8 @@ ReturnValue_t PlocSupvHelper::encodeAndSendPacket(const uint8_t* sendData, size_ return returnvalue::OK; } -ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, - size_t* size) { +ReturnValue_t PlocSupvUartManager::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, + size_t* size) { MutexGuard mg(ipcLock); if (ipcQueue.empty()) { *size = 0; @@ -977,7 +978,7 @@ ReturnValue_t PlocSupvHelper::readReceivedMessage(CookieIF* cookie, uint8_t** bu return OK; } -ReturnValue_t PlocSupvHelper::tryHdlcParsing() { +ReturnValue_t PlocSupvUartManager::tryHdlcParsing() { size_t bytesRead = 0; ReturnValue_t result = parseRecRingBufForHdlc(bytesRead); if (result == returnvalue::OK) { @@ -1001,7 +1002,7 @@ ReturnValue_t PlocSupvHelper::tryHdlcParsing() { return result; } -ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { +ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize) { size_t availableData = recRingBuf.getAvailableReadData(); if (availableData == 0) { return NO_PACKET_FOUND; @@ -1043,17 +1044,17 @@ ReturnValue_t PlocSupvHelper::parseRecRingBufForHdlc(size_t& readSize) { return NO_PACKET_FOUND; } -void PlocSupvHelper::pushIpcData(const uint8_t* data, size_t len) { +void PlocSupvUartManager::pushIpcData(const uint8_t* data, size_t len) { MutexGuard mg(ipcLock); ipcRingBuf.writeData(data, len); ipcQueue.insert(len); } -uint32_t PlocSupvHelper::buildApidServiceParam1(uint8_t apid, uint8_t serviceId) { +uint32_t PlocSupvUartManager::buildApidServiceParam1(uint8_t apid, uint8_t serviceId) { return (apid << 8) | serviceId; } -void PlocSupvHelper::performUartShutdown() { +void PlocSupvUartManager::performUartShutdown() { tcflush(serialPort, TCIOFLUSH); // Clear ring buffers recRingBuf.clear(); diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 29183406..fcf49949 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -25,9 +25,9 @@ * the supervisor and the OBC. * @author J. Meier */ -class PlocSupvHelper : public DeviceCommunicationIF, - public SystemObject, - public ExecutableObjectIF { +class PlocSupvUartManager : public DeviceCommunicationIF, + public SystemObject, + public ExecutableObjectIF { public: static const uint8_t INTERFACE_ID = CLASS_ID::PLOC_SUPV_HELPER; @@ -115,8 +115,8 @@ class PlocSupvHelper : public DeviceCommunicationIF, //! P1: First byte percent, third and fourth byte Sequence Count, P2: Bytes written static constexpr Event SUPV_UPDATE_PROGRESS = MAKE_EVENT(30, severity::INFO); - PlocSupvHelper(object_id_t objectId); - virtual ~PlocSupvHelper(); + PlocSupvUartManager(object_id_t objectId); + virtual ~PlocSupvUartManager(); ReturnValue_t initialize() override; ReturnValue_t performOperation(uint8_t operationCode = 0) override; From 2b4ec6d2745ffc558332f0685510ea9f057af037 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:40:19 +0100 Subject: [PATCH 150/244] add start method for uart man --- bsp_hosted/ObjectFactory.cpp | 6 +++--- linux/devices/ploc/PlocSupervisorHandler.cpp | 21 ++++++-------------- linux/devices/ploc/PlocSupervisorHandler.h | 4 ++-- linux/devices/ploc/PlocSupvUartMan.cpp | 11 ++++++++++ linux/devices/ploc/PlocSupvUartMan.h | 7 ++++++- 5 files changed, 28 insertions(+), 21 deletions(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index c384c7ad..10c9e46d 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -95,13 +95,13 @@ void ObjectFactory::produce(void* args) { #if OBSW_ADD_PLOC_SUPERVISOR == 1 std::string plocSupvString = "/dev/ploc_supv"; auto supervisorCookie = - new UartCookie(objects::PLOC_SUPERVISOR_HANDLER, plocSupvString, uart::PLOC_SUPV_BAUD, - supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); + new SerialCookie(objects::PLOC_SUPERVISOR_HANDLER, plocSupvString, uart::PLOC_SUPV_BAUD, + supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); supervisorCookie->setNoFixedSizeReply(); auto supvHelper = new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER); new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, objects::PLOC_SUPERVISOR_HELPER, supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, dummyGpioIF), - pcdu::PDU1_CH6_PLOC_12V, supvHelper); + pcdu::PDU1_CH6_PLOC_12V, *supvHelper); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ #endif diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 9e9653ed..f2a7c5fe 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -20,7 +20,7 @@ using namespace returnvalue; PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, Gpio uartIsolatorSwitch, power::Switch_t powerSwitch, - PlocSupvUartManager* supvHelper) + PlocSupvUartManager& supvHelper) : DeviceHandlerBase(objectId, uartComIFid, comCookie), uartIsolatorSwitch(uartIsolatorSwitch), hkset(this), @@ -33,9 +33,6 @@ PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t u if (comCookie == nullptr) { sif::error << "PlocSupervisorHandler: Invalid com cookie" << std::endl; } - if (supvHelper == nullptr) { - sif::error << "PlocSupervisorHandler: Invalid PlocSupvHelper object" << std::endl; - } spParams.buf = commandBuffer; spParams.maxSize = sizeof(commandBuffer); eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5); @@ -52,10 +49,6 @@ ReturnValue_t PlocSupervisorHandler::initialize() { #ifdef XIPHOS_Q7S sdcMan = SdCardManager::instance(); #endif /* TE0720_1CFA */ - if (supvHelper == nullptr) { - sif::warning << "PlocSupervisorHandler::initialize: Invalid supervisor helper" << std::endl; - return ObjectManagerIF::CHILD_INIT_FAILED; - } result = eventSubscription(); if (result != returnvalue::OK) { @@ -87,10 +80,6 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, ReturnValue_t result = returnvalue::OK; switch (actionId) { - case TERMINATE_SUPV_HELPER: { - supvHelper->stop(); - return EXECUTION_FINISHED; - } default: break; } @@ -114,7 +103,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, if (result != returnvalue::OK) { return result; } - result = supvHelper->performUpdate(params); + result = supvHelper.performUpdate(params); if (result != returnvalue::OK) { return result; } @@ -122,7 +111,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, return EXECUTION_FINISHED; } case CONTINUE_UPDATE: { - supvHelper->initiateUpdateContinuation(); + supvHelper.initiateUpdateContinuation(); plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } @@ -135,7 +124,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, if (not std::filesystem::exists(params.file)) { return HasFileSystemIF::FILE_DOES_NOT_EXIST; } - supvHelper->performMemCheck(params.file, params.memId, params.startAddr); + supvHelper.performMemCheck(params.file, params.memId, params.startAddr); plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } @@ -150,6 +139,7 @@ void PlocSupervisorHandler::doStartUp() { switch (startupState) { case StartupState::OFF: { bootTimeout.resetTimer(); + supvHelper.start(); startupState = StartupState::BOOTING; break; } @@ -177,6 +167,7 @@ void PlocSupervisorHandler::doStartUp() { void PlocSupervisorHandler::doShutDown() { setMode(_MODE_POWER_DOWN); + supvHelper.stop(); uartIsolatorSwitch.pullLow(); startupState = StartupState::OFF; } diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index a152349c..c5034c03 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -34,7 +34,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { public: PlocSupervisorHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, Gpio uartIsolatorSwitch, power::Switch_t powerSwitch, - PlocSupvUartManager* supvHelper); + PlocSupvUartManager& supvHelper); virtual ~PlocSupervisorHandler(); virtual ReturnValue_t initialize() override; @@ -130,7 +130,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { const power::Switch_t powerSwitch = power::NO_SWITCH; supv::TmBase tmReader; - PlocSupvUartManager* supvHelper = nullptr; + PlocSupvUartManager& supvHelper; MessageQueueIF* eventQueue = nullptr; /** Number of expected replies following the MRAM dump command */ diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index f17033a7..cdf74027 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -94,6 +94,9 @@ ReturnValue_t PlocSupvUartManager::initialize() { ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { bool putTaskToSleep = false; while (true) { + lock->lockMutex(); + state = InternalState::SLEEPING; + lock->unlockMutex(); semaphore->acquire(); while (true) { putTaskToSleep = handleUartReception(); @@ -289,6 +292,14 @@ void PlocSupvUartManager::stop() { state = InternalState::GO_TO_SLEEP; } +void PlocSupvUartManager::start() { + MutexGuard mg(lock); + if (state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) { + return; + } + semaphore->release(); +} + void PlocSupvUartManager::executeFullCheckMemoryCommand() { ReturnValue_t result; if (update.crcShouldBeChecked) { diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index fcf49949..da8bb176 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -148,10 +148,15 @@ class PlocSupvUartManager : public DeviceCommunicationIF, // ReturnValue_t startEventBufferRequest(std::string path); /** - * @brief Can be used to interrupt a running data transfer. + * @brief Can be used to stop the UART reception and put the task to sleep */ void stop(); + /** + * @brief Can be used to start the UART reception + */ + void start(); + static uint32_t buildProgParams1(uint8_t percent, uint16_t seqCount); static uint32_t buildApidServiceParam1(uint8_t apid, uint8_t serviceId); From c436a2abdae472cf492637341dce08d36378439d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:41:15 +0100 Subject: [PATCH 151/244] rename helper --- linux/devices/ploc/PlocSupervisorHandler.cpp | 12 ++++++------ linux/devices/ploc/PlocSupervisorHandler.h | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index f2a7c5fe..6ea85c6f 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -29,7 +29,7 @@ PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t u loggingReport(this), adcReport(this), powerSwitch(powerSwitch), - supvHelper(supvHelper) { + uartManager(supvHelper) { if (comCookie == nullptr) { sif::error << "PlocSupervisorHandler: Invalid com cookie" << std::endl; } @@ -103,7 +103,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, if (result != returnvalue::OK) { return result; } - result = supvHelper.performUpdate(params); + result = uartManager.performUpdate(params); if (result != returnvalue::OK) { return result; } @@ -111,7 +111,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, return EXECUTION_FINISHED; } case CONTINUE_UPDATE: { - supvHelper.initiateUpdateContinuation(); + uartManager.initiateUpdateContinuation(); plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } @@ -124,7 +124,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, if (not std::filesystem::exists(params.file)) { return HasFileSystemIF::FILE_DOES_NOT_EXIST; } - supvHelper.performMemCheck(params.file, params.memId, params.startAddr); + uartManager.performMemCheck(params.file, params.memId, params.startAddr); plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } @@ -139,7 +139,7 @@ void PlocSupervisorHandler::doStartUp() { switch (startupState) { case StartupState::OFF: { bootTimeout.resetTimer(); - supvHelper.start(); + uartManager.start(); startupState = StartupState::BOOTING; break; } @@ -167,7 +167,7 @@ void PlocSupervisorHandler::doStartUp() { void PlocSupervisorHandler::doShutDown() { setMode(_MODE_POWER_DOWN); - supvHelper.stop(); + uartManager.stop(); uartIsolatorSwitch.pullLow(); startupState = StartupState::OFF; } diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index c5034c03..dd63c9e6 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -130,7 +130,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { const power::Switch_t powerSwitch = power::NO_SWITCH; supv::TmBase tmReader; - PlocSupvUartManager& supvHelper; + PlocSupvUartManager& uartManager; MessageQueueIF* eventQueue = nullptr; /** Number of expected replies following the MRAM dump command */ From 38e0df5d12f6fd1c96bec9f98206385878243157 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:42:53 +0100 Subject: [PATCH 152/244] increase prio a bit --- bsp_hosted/OBSWConfig.h.in | 2 +- bsp_hosted/scheduling.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bsp_hosted/OBSWConfig.h.in b/bsp_hosted/OBSWConfig.h.in index f234e795..9a80286d 100644 --- a/bsp_hosted/OBSWConfig.h.in +++ b/bsp_hosted/OBSWConfig.h.in @@ -15,7 +15,7 @@ #define OBSW_ENABLE_TIMERS 1 #define OBSW_ADD_STAR_TRACKER 0 -#define OBSW_ADD_PLOC_SUPERVISOR 0 +#define OBSW_ADD_PLOC_SUPERVISOR 1 #define OBSW_ADD_PLOC_MPSOC 0 #define OBSW_ADD_SUN_SENSORS 0 #define OBSW_ADD_MGT 0 diff --git a/bsp_hosted/scheduling.cpp b/bsp_hosted/scheduling.cpp index d7238a80..76a8f2e9 100644 --- a/bsp_hosted/scheduling.cpp +++ b/bsp_hosted/scheduling.cpp @@ -166,7 +166,7 @@ void scheduling::initTasks() { #if OBSW_ADD_PLOC_SUPERVISOR == 1 PeriodicTaskIF* supvHelperTask = factory->createPeriodicTask( - "PLOC_SUPV_HELPER", 10, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); + "PLOC_SUPV_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = supvHelperTask->addComponent(objects::PLOC_SUPERVISOR_HELPER); if (result != returnvalue::OK) { scheduling::printAddObjectError("PLOC_SUPV_HELPER", objects::PLOC_SUPERVISOR_HELPER); From 42c52295f72bcdd722599357965154d38ed39d39 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:44:47 +0100 Subject: [PATCH 153/244] re-run generators --- generators/bsp_q7s_events.csv | 61 ++++++++++--------- generators/events/translateEvents.cpp | 19 ++++-- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 19 ++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 6 files changed, 63 insertions(+), 42 deletions(-) diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 7dace6d3..bb762004 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -123,10 +123,13 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 11901;0x2e7d;BOOTING_FIRMWARE_FAILED;LOW;Failed to boot firmware;linux/devices/startracker/StarTrackerHandler.h 11902;0x2e7e;BOOTING_BOOTLOADER_FAILED;LOW;Failed to boot star tracker into bootloader mode;linux/devices/startracker/StarTrackerHandler.h 12001;0x2ee1;SUPV_MEMORY_READ_RPT_CRC_FAILURE;LOW;PLOC supervisor crc failure in telemetry packet;linux/devices/ploc/PlocSupervisorHandler.h -12002;0x2ee2;SUPV_ACK_FAILURE;LOW;PLOC supervisor received acknowledgment failure report;linux/devices/ploc/PlocSupervisorHandler.h -12003;0x2ee3;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report P1: ID of command for which the execution failed P2: Status code sent by the supervisor handler;linux/devices/ploc/PlocSupervisorHandler.h -12004;0x2ee4;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;linux/devices/ploc/PlocSupervisorHandler.h -12005;0x2ee5;SUPV_MPSOC_SHUWDOWN_BUILD_FAILED;LOW;Failed to build the command to shutdown the MPSoC;linux/devices/ploc/PlocSupervisorHandler.h +12002;0x2ee2;SUPV_UNKNOWN_TM;LOW;Unhandled event. P1: APID, P2: Service ID;linux/devices/ploc/PlocSupervisorHandler.h +12003;0x2ee3;SUPV_UNINIMPLEMENTED_TM;LOW;;linux/devices/ploc/PlocSupervisorHandler.h +12004;0x2ee4;SUPV_ACK_FAILURE;LOW;PLOC supervisor received acknowledgment failure report;linux/devices/ploc/PlocSupervisorHandler.h +12005;0x2ee5;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report P1: ID of command for which the execution failed P2: Status code sent by the supervisor handler;linux/devices/ploc/PlocSupervisorHandler.h +12006;0x2ee6;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;linux/devices/ploc/PlocSupervisorHandler.h +12007;0x2ee7;SUPV_HELPER_EXECUTING;LOW;Supervisor helper currently executing a command;linux/devices/ploc/PlocSupervisorHandler.h +12008;0x2ee8;SUPV_MPSOC_SHUWDOWN_BUILD_FAILED;LOW;Failed to build the command to shutdown the MPSoC;linux/devices/ploc/PlocSupervisorHandler.h 12100;0x2f44;SANITIZATION_FAILED;LOW;;bsp_q7s/fs/SdCardManager.h 12101;0x2f45;MOUNTED_SD_CARD;INFO;;bsp_q7s/fs/SdCardManager.h 12300;0x300c;SEND_MRAM_DUMP_FAILED;LOW;Failed to send mram dump command to supervisor handler P1: Return value of commandAction function P2: Start address of MRAM to dump with this command;linux/devices/ploc/PlocMemoryDumper.h @@ -195,31 +198,31 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 13200;0x3390;P60_BOOT_COUNT;INFO;P60 boot count is broadcasted once at SW startup. P1: Boot count;mission/devices/P60DockHandler.h 13201;0x3391;BATT_MODE;INFO;Battery mode is broadcasted at startup. P1: Mode;mission/devices/P60DockHandler.h 13202;0x3392;BATT_MODE_CHANGED;MEDIUM;Battery mode has changed. P1: Old mode. P2: New mode;mission/devices/P60DockHandler.h -13600;0x3520;SUPV_UPDATE_FAILED;LOW;update failed;linux/devices/ploc/PlocSupvHelper.h -13601;0x3521;SUPV_UPDATE_SUCCESSFUL;LOW;update successful;linux/devices/ploc/PlocSupvHelper.h -13602;0x3522;SUPV_CONTINUE_UPDATE_FAILED;LOW;Continue update command failed;linux/devices/ploc/PlocSupvHelper.h -13603;0x3523;SUPV_CONTINUE_UPDATE_SUCCESSFUL;LOW;Continue update command successful;linux/devices/ploc/PlocSupvHelper.h -13604;0x3524;TERMINATED_UPDATE_PROCEDURE;LOW;Terminated update procedure by command;linux/devices/ploc/PlocSupvHelper.h -13605;0x3525;SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL;LOW;Requesting event buffer was successful;linux/devices/ploc/PlocSupvHelper.h -13606;0x3526;SUPV_EVENT_BUFFER_REQUEST_FAILED;LOW;Requesting event buffer failed;linux/devices/ploc/PlocSupvHelper.h -13607;0x3527;SUPV_EVENT_BUFFER_REQUEST_TERMINATED;LOW;Terminated event buffer request by command P1: Number of packets read before process was terminated;linux/devices/ploc/PlocSupvHelper.h -13608;0x3528;SUPV_MEM_CHECK_OK;INFO;;linux/devices/ploc/PlocSupvHelper.h -13609;0x3529;SUPV_MEM_CHECK_FAIL;INFO;;linux/devices/ploc/PlocSupvHelper.h -13616;0x3530;SUPV_SENDING_COMMAND_FAILED;LOW;;linux/devices/ploc/PlocSupvHelper.h -13617;0x3531;SUPV_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvHelper.h -13618;0x3532;SUPV_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvHelper.h -13619;0x3533;SUPV_MISSING_ACK;LOW;Did not receive acknowledgement report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/devices/ploc/PlocSupvHelper.h -13620;0x3534;SUPV_MISSING_EXE;LOW;Supervisor did not receive execution report P1: Number of bytes missing P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvHelper.h -13621;0x3535;SUPV_ACK_FAILURE_REPORT;LOW;Supervisor received acknowledgment failure report P1: Internal state of supervisor helper;linux/devices/ploc/PlocSupvHelper.h -13622;0x3536;SUPV_EXE_FAILURE_REPORT;LOW;Execution report failure P1:;linux/devices/ploc/PlocSupvHelper.h -13623;0x3537;SUPV_ACK_INVALID_APID;LOW;Supervisor expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvHelper.h -13624;0x3538;SUPV_EXE_INVALID_APID;LOW;Supervisor helper expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvHelper.h -13625;0x3539;ACK_RECEPTION_FAILURE;LOW;Failed to receive acknowledgment report P1: Return value P2: Apid of command for which the reception of the acknowledgment report failed;linux/devices/ploc/PlocSupvHelper.h -13626;0x353a;EXE_RECEPTION_FAILURE;LOW;Failed to receive execution report P1: Return value P2: Apid of command for which the reception of the execution report failed;linux/devices/ploc/PlocSupvHelper.h -13627;0x353b;WRITE_MEMORY_FAILED;LOW;Update procedure failed when sending packet. P1: First byte percent, third and fourth byte Sequence Count, P2: Bytes written;linux/devices/ploc/PlocSupvHelper.h -13628;0x353c;SUPV_REPLY_SIZE_MISSMATCH;LOW;;linux/devices/ploc/PlocSupvHelper.h -13629;0x353d;SUPV_REPLY_CRC_MISSMATCH;LOW;;linux/devices/ploc/PlocSupvHelper.h -13630;0x353e;SUPV_UPDATE_PROGRESS;INFO;Will be triggered every 5 percent of the update progress. P1: First byte percent, third and fourth byte Sequence Count, P2: Bytes written;linux/devices/ploc/PlocSupvHelper.h +13600;0x3520;SUPV_UPDATE_FAILED;LOW;update failed;linux/devices/ploc/PlocSupvUartMan.h +13601;0x3521;SUPV_UPDATE_SUCCESSFUL;LOW;update successful;linux/devices/ploc/PlocSupvUartMan.h +13602;0x3522;SUPV_CONTINUE_UPDATE_FAILED;LOW;Continue update command failed;linux/devices/ploc/PlocSupvUartMan.h +13603;0x3523;SUPV_CONTINUE_UPDATE_SUCCESSFUL;LOW;Continue update command successful;linux/devices/ploc/PlocSupvUartMan.h +13604;0x3524;TERMINATED_UPDATE_PROCEDURE;LOW;Terminated update procedure by command;linux/devices/ploc/PlocSupvUartMan.h +13605;0x3525;SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL;LOW;Requesting event buffer was successful;linux/devices/ploc/PlocSupvUartMan.h +13606;0x3526;SUPV_EVENT_BUFFER_REQUEST_FAILED;LOW;Requesting event buffer failed;linux/devices/ploc/PlocSupvUartMan.h +13607;0x3527;SUPV_EVENT_BUFFER_REQUEST_TERMINATED;LOW;Terminated event buffer request by command P1: Number of packets read before process was terminated;linux/devices/ploc/PlocSupvUartMan.h +13608;0x3528;SUPV_MEM_CHECK_OK;INFO;;linux/devices/ploc/PlocSupvUartMan.h +13609;0x3529;SUPV_MEM_CHECK_FAIL;INFO;;linux/devices/ploc/PlocSupvUartMan.h +13616;0x3530;SUPV_SENDING_COMMAND_FAILED;LOW;;linux/devices/ploc/PlocSupvUartMan.h +13617;0x3531;SUPV_HELPER_REQUESTING_REPLY_FAILED;LOW;Request receive message of communication interface failed P1: Return value returned by the communication interface requestReceiveMessage function P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvUartMan.h +13618;0x3532;SUPV_HELPER_READING_REPLY_FAILED;LOW;Reading receive message of communication interface failed P1: Return value returned by the communication interface readingReceivedMessage function P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvUartMan.h +13619;0x3533;SUPV_MISSING_ACK;LOW;Did not receive acknowledgement report P1: Number of bytes missing P2: Internal state of MPSoC helper;linux/devices/ploc/PlocSupvUartMan.h +13620;0x3534;SUPV_MISSING_EXE;LOW;Supervisor did not receive execution report P1: Number of bytes missing P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvUartMan.h +13621;0x3535;SUPV_ACK_FAILURE_REPORT;LOW;Supervisor received acknowledgment failure report P1: Internal state of supervisor helper;linux/devices/ploc/PlocSupvUartMan.h +13622;0x3536;SUPV_EXE_FAILURE_REPORT;LOW;Execution report failure P1:;linux/devices/ploc/PlocSupvUartMan.h +13623;0x3537;SUPV_ACK_INVALID_APID;LOW;Supervisor expected acknowledgment report but received space packet with other apid P1: Apid of received space packet P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvUartMan.h +13624;0x3538;SUPV_EXE_INVALID_APID;LOW;Supervisor helper expected execution report but received space packet with other apid P1: Apid of received space packet P2: Internal state of supervisor helper;linux/devices/ploc/PlocSupvUartMan.h +13625;0x3539;ACK_RECEPTION_FAILURE;LOW;Failed to receive acknowledgment report P1: Return value P2: Apid of command for which the reception of the acknowledgment report failed;linux/devices/ploc/PlocSupvUartMan.h +13626;0x353a;EXE_RECEPTION_FAILURE;LOW;Failed to receive execution report P1: Return value P2: Apid of command for which the reception of the execution report failed;linux/devices/ploc/PlocSupvUartMan.h +13627;0x353b;WRITE_MEMORY_FAILED;LOW;Update procedure failed when sending packet. P1: First byte percent, third and fourth byte Sequence Count, P2: Bytes written;linux/devices/ploc/PlocSupvUartMan.h +13628;0x353c;SUPV_REPLY_SIZE_MISSMATCH;LOW;;linux/devices/ploc/PlocSupvUartMan.h +13629;0x353d;SUPV_REPLY_CRC_MISSMATCH;LOW;;linux/devices/ploc/PlocSupvUartMan.h +13630;0x353e;SUPV_UPDATE_PROGRESS;INFO;Will be triggered every 5 percent of the update progress. P1: First byte percent, third and fourth byte Sequence Count, P2: Bytes written;linux/devices/ploc/PlocSupvUartMan.h 13700;0x3584;ALLOC_FAILURE;MEDIUM;;bsp_q7s/core/CoreController.h 13701;0x3585;REBOOT_SW;MEDIUM; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h 13702;0x3586;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index ee85e05c..1b534866 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 234 translations. + * @brief Auto-generated event translation file. Contains 237 translations. * @details - * Generated on: 2022-11-14 17:10:10 + * Generated on: 2022-11-15 17:44:20 */ #include "translateEvents.h" @@ -129,9 +129,12 @@ const char *RESET_OCCURED_STRING = "RESET_OCCURED"; const char *BOOTING_FIRMWARE_FAILED_STRING = "BOOTING_FIRMWARE_FAILED"; const char *BOOTING_BOOTLOADER_FAILED_STRING = "BOOTING_BOOTLOADER_FAILED"; const char *SUPV_MEMORY_READ_RPT_CRC_FAILURE_STRING = "SUPV_MEMORY_READ_RPT_CRC_FAILURE"; +const char *SUPV_UNKNOWN_TM_STRING = "SUPV_UNKNOWN_TM"; +const char *SUPV_UNINIMPLEMENTED_TM_STRING = "SUPV_UNINIMPLEMENTED_TM"; const char *SUPV_ACK_FAILURE_STRING = "SUPV_ACK_FAILURE"; const char *SUPV_EXE_FAILURE_STRING = "SUPV_EXE_FAILURE"; const char *SUPV_CRC_FAILURE_EVENT_STRING = "SUPV_CRC_FAILURE_EVENT"; +const char *SUPV_HELPER_EXECUTING_STRING = "SUPV_HELPER_EXECUTING"; const char *SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUWDOWN_BUILD_FAILED"; const char *SANITIZATION_FAILED_STRING = "SANITIZATION_FAILED"; const char *MOUNTED_SD_CARD_STRING = "MOUNTED_SD_CARD"; @@ -487,12 +490,18 @@ const char *translateEvents(Event event) { case (12001): return SUPV_MEMORY_READ_RPT_CRC_FAILURE_STRING; case (12002): - return SUPV_ACK_FAILURE_STRING; + return SUPV_UNKNOWN_TM_STRING; case (12003): - return SUPV_EXE_FAILURE_STRING; + return SUPV_UNINIMPLEMENTED_TM_STRING; case (12004): - return SUPV_CRC_FAILURE_EVENT_STRING; + return SUPV_ACK_FAILURE_STRING; case (12005): + return SUPV_EXE_FAILURE_STRING; + case (12006): + return SUPV_CRC_FAILURE_EVENT_STRING; + case (12007): + return SUPV_HELPER_EXECUTING_STRING; + case (12008): return SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING; case (12100): return SANITIZATION_FAILED_STRING; diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 96a2d656..4856c347 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2022-11-14 17:10:10 + * Generated on: 2022-11-15 17:44:20 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index ee85e05c..1b534866 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 234 translations. + * @brief Auto-generated event translation file. Contains 237 translations. * @details - * Generated on: 2022-11-14 17:10:10 + * Generated on: 2022-11-15 17:44:20 */ #include "translateEvents.h" @@ -129,9 +129,12 @@ const char *RESET_OCCURED_STRING = "RESET_OCCURED"; const char *BOOTING_FIRMWARE_FAILED_STRING = "BOOTING_FIRMWARE_FAILED"; const char *BOOTING_BOOTLOADER_FAILED_STRING = "BOOTING_BOOTLOADER_FAILED"; const char *SUPV_MEMORY_READ_RPT_CRC_FAILURE_STRING = "SUPV_MEMORY_READ_RPT_CRC_FAILURE"; +const char *SUPV_UNKNOWN_TM_STRING = "SUPV_UNKNOWN_TM"; +const char *SUPV_UNINIMPLEMENTED_TM_STRING = "SUPV_UNINIMPLEMENTED_TM"; const char *SUPV_ACK_FAILURE_STRING = "SUPV_ACK_FAILURE"; const char *SUPV_EXE_FAILURE_STRING = "SUPV_EXE_FAILURE"; const char *SUPV_CRC_FAILURE_EVENT_STRING = "SUPV_CRC_FAILURE_EVENT"; +const char *SUPV_HELPER_EXECUTING_STRING = "SUPV_HELPER_EXECUTING"; const char *SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUWDOWN_BUILD_FAILED"; const char *SANITIZATION_FAILED_STRING = "SANITIZATION_FAILED"; const char *MOUNTED_SD_CARD_STRING = "MOUNTED_SD_CARD"; @@ -487,12 +490,18 @@ const char *translateEvents(Event event) { case (12001): return SUPV_MEMORY_READ_RPT_CRC_FAILURE_STRING; case (12002): - return SUPV_ACK_FAILURE_STRING; + return SUPV_UNKNOWN_TM_STRING; case (12003): - return SUPV_EXE_FAILURE_STRING; + return SUPV_UNINIMPLEMENTED_TM_STRING; case (12004): - return SUPV_CRC_FAILURE_EVENT_STRING; + return SUPV_ACK_FAILURE_STRING; case (12005): + return SUPV_EXE_FAILURE_STRING; + case (12006): + return SUPV_CRC_FAILURE_EVENT_STRING; + case (12007): + return SUPV_HELPER_EXECUTING_STRING; + case (12008): return SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING; case (12100): return SANITIZATION_FAILED_STRING; diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 96a2d656..4856c347 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2022-11-14 17:10:10 + * Generated on: 2022-11-15 17:44:20 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index 66a1362e..a7999bdb 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 66a1362e7e977e427a66fd3176a9e7b6bc1b5998 +Subproject commit a7999bdbb1d3f490d383c3b085b9019730ec1450 From 3059e196a3d46ea225e9bb4afc67262e96cf3d38 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:47:24 +0100 Subject: [PATCH 154/244] update object list --- .../fsfwconfig/objects/translateObjects.cpp | 414 ++++++++++++++++-- .../fsfwconfig/objects/translateObjects.h | 2 +- 2 files changed, 381 insertions(+), 35 deletions(-) diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index ee9cb057..4856c347 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -1,23 +1,105 @@ /** - * @brief Auto-generated object translation file. + * @brief Auto-generated object translation file. * @details - * Contains 31 translations. - * Generated on: 2021-05-17 19:12:49 + * Contains 148 translations. + * Generated on: 2022-11-15 17:44:20 */ #include "translateObjects.h" -#include "systemObjectList.h" - -const char *TEST_TASK_STRING = "TEST_TASK"; -const char *DUMMY_HANDLER_STRING = "DUMMY_HANDLER"; +const char *P60DOCK_TEST_TASK_STRING = "P60DOCK_TEST_TASK"; +const char *ACS_CONTROLLER_STRING = "ACS_CONTROLLER"; +const char *CORE_CONTROLLER_STRING = "CORE_CONTROLLER"; +const char *GLOBAL_JSON_CFG_STRING = "GLOBAL_JSON_CFG"; +const char *THERMAL_CONTROLLER_STRING = "THERMAL_CONTROLLER"; +const char *MGM_0_LIS3_HANDLER_STRING = "MGM_0_LIS3_HANDLER"; +const char *GYRO_0_ADIS_HANDLER_STRING = "GYRO_0_ADIS_HANDLER"; +const char *SUS_0_N_LOC_XFYFZM_PT_XF_STRING = "SUS_0_N_LOC_XFYFZM_PT_XF"; +const char *SUS_1_N_LOC_XBYFZM_PT_XB_STRING = "SUS_1_N_LOC_XBYFZM_PT_XB"; +const char *SUS_2_N_LOC_XFYBZB_PT_YB_STRING = "SUS_2_N_LOC_XFYBZB_PT_YB"; +const char *SUS_3_N_LOC_XFYBZF_PT_YF_STRING = "SUS_3_N_LOC_XFYBZF_PT_YF"; +const char *SUS_4_N_LOC_XMYFZF_PT_ZF_STRING = "SUS_4_N_LOC_XMYFZF_PT_ZF"; +const char *SUS_5_N_LOC_XFYMZB_PT_ZB_STRING = "SUS_5_N_LOC_XFYMZB_PT_ZB"; +const char *SUS_6_R_LOC_XFYBZM_PT_XF_STRING = "SUS_6_R_LOC_XFYBZM_PT_XF"; +const char *SUS_7_R_LOC_XBYBZM_PT_XB_STRING = "SUS_7_R_LOC_XBYBZM_PT_XB"; +const char *SUS_8_R_LOC_XBYBZB_PT_YB_STRING = "SUS_8_R_LOC_XBYBZB_PT_YB"; +const char *SUS_9_R_LOC_XBYBZB_PT_YF_STRING = "SUS_9_R_LOC_XBYBZB_PT_YF"; +const char *SUS_10_N_LOC_XMYBZF_PT_ZF_STRING = "SUS_10_N_LOC_XMYBZF_PT_ZF"; +const char *SUS_11_R_LOC_XBYMZB_PT_ZB_STRING = "SUS_11_R_LOC_XBYMZB_PT_ZB"; +const char *RW1_STRING = "RW1"; +const char *MGM_1_RM3100_HANDLER_STRING = "MGM_1_RM3100_HANDLER"; +const char *GYRO_1_L3G_HANDLER_STRING = "GYRO_1_L3G_HANDLER"; +const char *RW2_STRING = "RW2"; +const char *MGM_2_LIS3_HANDLER_STRING = "MGM_2_LIS3_HANDLER"; +const char *GYRO_2_ADIS_HANDLER_STRING = "GYRO_2_ADIS_HANDLER"; +const char *RW3_STRING = "RW3"; +const char *MGM_3_RM3100_HANDLER_STRING = "MGM_3_RM3100_HANDLER"; +const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER"; +const char *RW4_STRING = "RW4"; +const char *STAR_TRACKER_STRING = "STAR_TRACKER"; +const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER"; +const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER"; +const char *PCDU_HANDLER_STRING = "PCDU_HANDLER"; +const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER"; +const char *PDU1_HANDLER_STRING = "PDU1_HANDLER"; +const char *PDU2_HANDLER_STRING = "PDU2_HANDLER"; +const char *ACU_HANDLER_STRING = "ACU_HANDLER"; +const char *BPX_BATT_HANDLER_STRING = "BPX_BATT_HANDLER"; +const char *PLPCDU_HANDLER_STRING = "PLPCDU_HANDLER"; +const char *RAD_SENSOR_STRING = "RAD_SENSOR"; +const char *PLOC_UPDATER_STRING = "PLOC_UPDATER"; +const char *PLOC_MEMORY_DUMPER_STRING = "PLOC_MEMORY_DUMPER"; +const char *STR_HELPER_STRING = "STR_HELPER"; +const char *PLOC_MPSOC_HELPER_STRING = "PLOC_MPSOC_HELPER"; +const char *AXI_PTME_CONFIG_STRING = "AXI_PTME_CONFIG"; +const char *PTME_CONFIG_STRING = "PTME_CONFIG"; +const char *PLOC_MPSOC_HANDLER_STRING = "PLOC_MPSOC_HANDLER"; +const char *PLOC_SUPERVISOR_HANDLER_STRING = "PLOC_SUPERVISOR_HANDLER"; +const char *PLOC_SUPERVISOR_HELPER_STRING = "PLOC_SUPERVISOR_HELPER"; +const char *SCEX_STRING = "SCEX"; +const char *SOLAR_ARRAY_DEPL_HANDLER_STRING = "SOLAR_ARRAY_DEPL_HANDLER"; +const char *HEATER_HANDLER_STRING = "HEATER_HANDLER"; +const char *TMP1075_HANDLER_TCS_0_STRING = "TMP1075_HANDLER_TCS_0"; +const char *TMP1075_HANDLER_TCS_1_STRING = "TMP1075_HANDLER_TCS_1"; +const char *TMP1075_HANDLER_PLPCDU_0_STRING = "TMP1075_HANDLER_PLPCDU_0"; +const char *TMP1075_HANDLER_PLPCDU_1_STRING = "TMP1075_HANDLER_PLPCDU_1"; +const char *TMP1075_HANDLER_IF_BOARD_STRING = "TMP1075_HANDLER_IF_BOARD"; +const char *TMP1075_HANDLER_OBC_IF_BOARD_STRING = "TMP1075_HANDLER_OBC_IF_BOARD"; +const char *RTD_0_IC3_PLOC_HEATSPREADER_STRING = "RTD_0_IC3_PLOC_HEATSPREADER"; +const char *RTD_1_IC4_PLOC_MISSIONBOARD_STRING = "RTD_1_IC4_PLOC_MISSIONBOARD"; +const char *RTD_2_IC5_4K_CAMERA_STRING = "RTD_2_IC5_4K_CAMERA"; +const char *RTD_3_IC6_DAC_HEATSPREADER_STRING = "RTD_3_IC6_DAC_HEATSPREADER"; +const char *RTD_4_IC7_STARTRACKER_STRING = "RTD_4_IC7_STARTRACKER"; +const char *RTD_5_IC8_RW1_MX_MY_STRING = "RTD_5_IC8_RW1_MX_MY"; +const char *RTD_6_IC9_DRO_STRING = "RTD_6_IC9_DRO"; +const char *RTD_7_IC10_SCEX_STRING = "RTD_7_IC10_SCEX"; +const char *RTD_8_IC11_X8_STRING = "RTD_8_IC11_X8"; +const char *RTD_9_IC12_HPA_STRING = "RTD_9_IC12_HPA"; +const char *RTD_10_IC13_PL_TX_STRING = "RTD_10_IC13_PL_TX"; +const char *RTD_11_IC14_MPA_STRING = "RTD_11_IC14_MPA"; +const char *RTD_12_IC15_ACU_STRING = "RTD_12_IC15_ACU"; +const char *RTD_13_IC16_PLPCDU_HEATSPREADER_STRING = "RTD_13_IC16_PLPCDU_HEATSPREADER"; +const char *RTD_14_IC17_TCS_BOARD_STRING = "RTD_14_IC17_TCS_BOARD"; +const char *RTD_15_IC18_IMTQ_STRING = "RTD_15_IC18_IMTQ"; +const char *SYRLINKS_HK_HANDLER_STRING = "SYRLINKS_HK_HANDLER"; const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF"; -const char *PUS_SERVICE_3_STRING = "PUS_SERVICE_3"; -const char *PUS_SERVICE_5_STRING = "PUS_SERVICE_5"; +const char *GPIO_IF_STRING = "GPIO_IF"; +const char *SCEX_UART_READER_STRING = "SCEX_UART_READER"; +const char *SPI_MAIN_COM_IF_STRING = "SPI_MAIN_COM_IF"; +const char *SPI_RW_COM_IF_STRING = "SPI_RW_COM_IF"; +const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF"; +const char *UART_COM_IF_STRING = "UART_COM_IF"; +const char *I2C_COM_IF_STRING = "I2C_COM_IF"; +const char *CSP_COM_IF_STRING = "CSP_COM_IF"; +const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR"; +const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR"; +const char *TMTC_BRIDGE_STRING = "TMTC_BRIDGE"; +const char *TMTC_POLLING_TASK_STRING = "TMTC_POLLING_TASK"; +const char *FILE_SYSTEM_HANDLER_STRING = "FILE_SYSTEM_HANDLER"; +const char *SDC_MANAGER_STRING = "SDC_MANAGER"; +const char *PTME_STRING = "PTME"; +const char *PDEC_HANDLER_STRING = "PDEC_HANDLER"; +const char *CCSDS_HANDLER_STRING = "CCSDS_HANDLER"; const char *PUS_SERVICE_6_STRING = "PUS_SERVICE_6"; -const char *PUS_SERVICE_8_STRING = "PUS_SERVICE_8"; -const char *PUS_SERVICE_23_STRING = "PUS_SERVICE_23"; -const char *PUS_SERVICE_201_STRING = "PUS_SERVICE_201"; -const char *TM_FUNNEL_STRING = "TM_FUNNEL"; const char *FSFW_OBJECTS_START_STRING = "FSFW_OBJECTS_START"; const char *PUS_SERVICE_1_VERIFICATION_STRING = "PUS_SERVICE_1_VERIFICATION"; const char *PUS_SERVICE_2_DEVICE_ACCESS_STRING = "PUS_SERVICE_2_DEVICE_ACCESS"; @@ -25,9 +107,12 @@ const char *PUS_SERVICE_3_HOUSEKEEPING_STRING = "PUS_SERVICE_3_HOUSEKEEPING"; const char *PUS_SERVICE_5_EVENT_REPORTING_STRING = "PUS_SERVICE_5_EVENT_REPORTING"; const char *PUS_SERVICE_8_FUNCTION_MGMT_STRING = "PUS_SERVICE_8_FUNCTION_MGMT"; const char *PUS_SERVICE_9_TIME_MGMT_STRING = "PUS_SERVICE_9_TIME_MGMT"; +const char *PUS_SERVICE_11_TC_SCHEDULER_STRING = "PUS_SERVICE_11_TC_SCHEDULER"; const char *PUS_SERVICE_17_TEST_STRING = "PUS_SERVICE_17_TEST"; const char *PUS_SERVICE_20_PARAMETERS_STRING = "PUS_SERVICE_20_PARAMETERS"; const char *PUS_SERVICE_200_MODE_MGMT_STRING = "PUS_SERVICE_200_MODE_MGMT"; +const char *PUS_SERVICE_201_HEALTH_STRING = "PUS_SERVICE_201_HEALTH"; +const char *CFDP_PACKET_DISTRIBUTOR_STRING = "CFDP_PACKET_DISTRIBUTOR"; const char *HEALTH_TABLE_STRING = "HEALTH_TABLE"; const char *MODE_STORE_STRING = "MODE_STORE"; const char *EVENT_MANAGER_STRING = "EVENT_MANAGER"; @@ -36,33 +121,230 @@ const char *TC_STORE_STRING = "TC_STORE"; const char *TM_STORE_STRING = "TM_STORE"; const char *IPC_STORE_STRING = "IPC_STORE"; const char *TIME_STAMPER_STRING = "TIME_STAMPER"; +const char *VERIFICATION_REPORTER_STRING = "VERIFICATION_REPORTER"; const char *FSFW_OBJECTS_END_STRING = "FSFW_OBJECTS_END"; +const char *SPI_TEST_STRING = "SPI_TEST"; +const char *UART_TEST_STRING = "UART_TEST"; +const char *I2C_TEST_STRING = "I2C_TEST"; +const char *DUMMY_COM_IF_STRING = "DUMMY_COM_IF"; +const char *DUMMY_HANDLER_STRING = "DUMMY_HANDLER"; const char *DUMMY_INTERFACE_STRING = "DUMMY_INTERFACE"; -const char *THERMAL_CONTROLLER_STRING = "THERMAL_CONTROLLER"; +const char *LIBGPIOD_TEST_STRING = "LIBGPIOD_TEST"; +const char *TEST_TASK_STRING = "TEST_TASK"; +const char *HEATER_0_PLOC_PROC_BRD_STRING = "HEATER_0_PLOC_PROC_BRD"; +const char *HEATER_1_PCDU_BRD_STRING = "HEATER_1_PCDU_BRD"; +const char *HEATER_2_ACS_BRD_STRING = "HEATER_2_ACS_BRD"; +const char *HEATER_3_OBC_BRD_STRING = "HEATER_3_OBC_BRD"; +const char *HEATER_4_CAMERA_STRING = "HEATER_4_CAMERA"; +const char *HEATER_5_STR_STRING = "HEATER_5_STR"; +const char *HEATER_6_DRO_STRING = "HEATER_6_DRO"; +const char *HEATER_7_HPA_STRING = "HEATER_7_HPA"; +const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS"; +const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS"; +const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS"; +const char *RW_ASS_STRING = "RW_ASS"; +const char *CAM_SWITCHER_STRING = "CAM_SWITCHER"; +const char *TM_FUNNEL_STRING = "TM_FUNNEL"; +const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL"; +const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL"; +const char *CFDP_HANDLER_STRING = "CFDP_HANDLER"; +const char *CFDP_DISTRIBUTOR_STRING = "CFDP_DISTRIBUTOR"; +const char *EIVE_SYSTEM_STRING = "EIVE_SYSTEM"; +const char *ACS_SUBSYSTEM_STRING = "ACS_SUBSYSTEM"; +const char *PL_SUBSYSTEM_STRING = "PL_SUBSYSTEM"; +const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE"; const char *NO_OBJECT_STRING = "NO_OBJECT"; const char *translateObject(object_id_t object) { switch ((object & 0xFFFFFFFF)) { - case 0x42694269: - return TEST_TASK_STRING; - case 0x4400AFFE: - return DUMMY_HANDLER_STRING; - case 0x49000001: + case 0x00005060: + return P60DOCK_TEST_TASK_STRING; + case 0x43000002: + return ACS_CONTROLLER_STRING; + case 0x43000003: + return CORE_CONTROLLER_STRING; + case 0x43000006: + return GLOBAL_JSON_CFG_STRING; + case 0x43400001: + return THERMAL_CONTROLLER_STRING; + case 0x44120006: + return MGM_0_LIS3_HANDLER_STRING; + case 0x44120010: + return GYRO_0_ADIS_HANDLER_STRING; + case 0x44120032: + return SUS_0_N_LOC_XFYFZM_PT_XF_STRING; + case 0x44120033: + return SUS_1_N_LOC_XBYFZM_PT_XB_STRING; + case 0x44120034: + return SUS_2_N_LOC_XFYBZB_PT_YB_STRING; + case 0x44120035: + return SUS_3_N_LOC_XFYBZF_PT_YF_STRING; + case 0x44120036: + return SUS_4_N_LOC_XMYFZF_PT_ZF_STRING; + case 0x44120037: + return SUS_5_N_LOC_XFYMZB_PT_ZB_STRING; + case 0x44120038: + return SUS_6_R_LOC_XFYBZM_PT_XF_STRING; + case 0x44120039: + return SUS_7_R_LOC_XBYBZM_PT_XB_STRING; + case 0x44120040: + return SUS_8_R_LOC_XBYBZB_PT_YB_STRING; + case 0x44120041: + return SUS_9_R_LOC_XBYBZB_PT_YF_STRING; + case 0x44120042: + return SUS_10_N_LOC_XMYBZF_PT_ZF_STRING; + case 0x44120043: + return SUS_11_R_LOC_XBYMZB_PT_ZB_STRING; + case 0x44120047: + return RW1_STRING; + case 0x44120107: + return MGM_1_RM3100_HANDLER_STRING; + case 0x44120111: + return GYRO_1_L3G_HANDLER_STRING; + case 0x44120148: + return RW2_STRING; + case 0x44120208: + return MGM_2_LIS3_HANDLER_STRING; + case 0x44120212: + return GYRO_2_ADIS_HANDLER_STRING; + case 0x44120249: + return RW3_STRING; + case 0x44120309: + return MGM_3_RM3100_HANDLER_STRING; + case 0x44120313: + return GYRO_3_L3G_HANDLER_STRING; + case 0x44120350: + return RW4_STRING; + case 0x44130001: + return STAR_TRACKER_STRING; + case 0x44130045: + return GPS_CONTROLLER_STRING; + case 0x44140014: + return IMTQ_HANDLER_STRING; + case 0x442000A1: + return PCDU_HANDLER_STRING; + case 0x44250000: + return P60DOCK_HANDLER_STRING; + case 0x44250001: + return PDU1_HANDLER_STRING; + case 0x44250002: + return PDU2_HANDLER_STRING; + case 0x44250003: + return ACU_HANDLER_STRING; + case 0x44260000: + return BPX_BATT_HANDLER_STRING; + case 0x44300000: + return PLPCDU_HANDLER_STRING; + case 0x443200A5: + return RAD_SENSOR_STRING; + case 0x44330000: + return PLOC_UPDATER_STRING; + case 0x44330001: + return PLOC_MEMORY_DUMPER_STRING; + case 0x44330002: + return STR_HELPER_STRING; + case 0x44330003: + return PLOC_MPSOC_HELPER_STRING; + case 0x44330004: + return AXI_PTME_CONFIG_STRING; + case 0x44330005: + return PTME_CONFIG_STRING; + case 0x44330015: + return PLOC_MPSOC_HANDLER_STRING; + case 0x44330016: + return PLOC_SUPERVISOR_HANDLER_STRING; + case 0x44330017: + return PLOC_SUPERVISOR_HELPER_STRING; + case 0x44330032: + return SCEX_STRING; + case 0x444100A2: + return SOLAR_ARRAY_DEPL_HANDLER_STRING; + case 0x444100A4: + return HEATER_HANDLER_STRING; + case 0x44420004: + return TMP1075_HANDLER_TCS_0_STRING; + case 0x44420005: + return TMP1075_HANDLER_TCS_1_STRING; + case 0x44420006: + return TMP1075_HANDLER_PLPCDU_0_STRING; + case 0x44420007: + return TMP1075_HANDLER_PLPCDU_1_STRING; + case 0x44420008: + return TMP1075_HANDLER_IF_BOARD_STRING; + case 0x44420009: + return TMP1075_HANDLER_OBC_IF_BOARD_STRING; + case 0x44420016: + return RTD_0_IC3_PLOC_HEATSPREADER_STRING; + case 0x44420017: + return RTD_1_IC4_PLOC_MISSIONBOARD_STRING; + case 0x44420018: + return RTD_2_IC5_4K_CAMERA_STRING; + case 0x44420019: + return RTD_3_IC6_DAC_HEATSPREADER_STRING; + case 0x44420020: + return RTD_4_IC7_STARTRACKER_STRING; + case 0x44420021: + return RTD_5_IC8_RW1_MX_MY_STRING; + case 0x44420022: + return RTD_6_IC9_DRO_STRING; + case 0x44420023: + return RTD_7_IC10_SCEX_STRING; + case 0x44420024: + return RTD_8_IC11_X8_STRING; + case 0x44420025: + return RTD_9_IC12_HPA_STRING; + case 0x44420026: + return RTD_10_IC13_PL_TX_STRING; + case 0x44420027: + return RTD_11_IC14_MPA_STRING; + case 0x44420028: + return RTD_12_IC15_ACU_STRING; + case 0x44420029: + return RTD_13_IC16_PLPCDU_HEATSPREADER_STRING; + case 0x44420030: + return RTD_14_IC17_TCS_BOARD_STRING; + case 0x44420031: + return RTD_15_IC18_IMTQ_STRING; + case 0x445300A3: + return SYRLINKS_HK_HANDLER_STRING; + case 0x49000000: return ARDUINO_COM_IF_STRING; - case 0x51000300: - return PUS_SERVICE_3_STRING; - case 0x51000400: - return PUS_SERVICE_5_STRING; + case 0x49010005: + return GPIO_IF_STRING; + case 0x49010006: + return SCEX_UART_READER_STRING; + case 0x49020004: + return SPI_MAIN_COM_IF_STRING; + case 0x49020005: + return SPI_RW_COM_IF_STRING; + case 0x49020006: + return SPI_RTD_COM_IF_STRING; + case 0x49030003: + return UART_COM_IF_STRING; + case 0x49040002: + return I2C_COM_IF_STRING; + case 0x49050001: + return CSP_COM_IF_STRING; + case 0x50000100: + return CCSDS_PACKET_DISTRIBUTOR_STRING; + case 0x50000200: + return PUS_PACKET_DISTRIBUTOR_STRING; + case 0x50000300: + return TMTC_BRIDGE_STRING; + case 0x50000400: + return TMTC_POLLING_TASK_STRING; + case 0x50000500: + return FILE_SYSTEM_HANDLER_STRING; + case 0x50000550: + return SDC_MANAGER_STRING; + case 0x50000600: + return PTME_STRING; + case 0x50000700: + return PDEC_HANDLER_STRING; + case 0x50000800: + return CCSDS_HANDLER_STRING; case 0x51000500: return PUS_SERVICE_6_STRING; - case 0x51000800: - return PUS_SERVICE_8_STRING; - case 0x51002300: - return PUS_SERVICE_23_STRING; - case 0x51020100: - return PUS_SERVICE_201_STRING; - case 0x52000002: - return TM_FUNNEL_STRING; case 0x53000000: return FSFW_OBJECTS_START_STRING; case 0x53000001: @@ -77,12 +359,18 @@ const char *translateObject(object_id_t object) { return PUS_SERVICE_8_FUNCTION_MGMT_STRING; case 0x53000009: return PUS_SERVICE_9_TIME_MGMT_STRING; + case 0x53000011: + return PUS_SERVICE_11_TC_SCHEDULER_STRING; case 0x53000017: return PUS_SERVICE_17_TEST_STRING; case 0x53000020: return PUS_SERVICE_20_PARAMETERS_STRING; case 0x53000200: return PUS_SERVICE_200_MODE_MGMT_STRING; + case 0x53000201: + return PUS_SERVICE_201_HEALTH_STRING; + case 0x53001000: + return CFDP_PACKET_DISTRIBUTOR_STRING; case 0x53010000: return HEALTH_TABLE_STRING; case 0x53010100: @@ -99,12 +387,70 @@ const char *translateObject(object_id_t object) { return IPC_STORE_STRING; case 0x53500010: return TIME_STAMPER_STRING; + case 0x53500020: + return VERIFICATION_REPORTER_STRING; case 0x53ffffff: return FSFW_OBJECTS_END_STRING; - case 0xCAFECAFE: + case 0x54000010: + return SPI_TEST_STRING; + case 0x54000020: + return UART_TEST_STRING; + case 0x54000030: + return I2C_TEST_STRING; + case 0x54000040: + return DUMMY_COM_IF_STRING; + case 0x5400AFFE: + return DUMMY_HANDLER_STRING; + case 0x5400CAFE: return DUMMY_INTERFACE_STRING; - case objects::THERMAL_CONTROLLER: - return THERMAL_CONTROLLER_STRING; + case 0x54123456: + return LIBGPIOD_TEST_STRING; + case 0x54694269: + return TEST_TASK_STRING; + case 0x60000000: + return HEATER_0_PLOC_PROC_BRD_STRING; + case 0x60000001: + return HEATER_1_PCDU_BRD_STRING; + case 0x60000002: + return HEATER_2_ACS_BRD_STRING; + case 0x60000003: + return HEATER_3_OBC_BRD_STRING; + case 0x60000004: + return HEATER_4_CAMERA_STRING; + case 0x60000005: + return HEATER_5_STR_STRING; + case 0x60000006: + return HEATER_6_DRO_STRING; + case 0x60000007: + return HEATER_7_HPA_STRING; + case 0x73000001: + return ACS_BOARD_ASS_STRING; + case 0x73000002: + return SUS_BOARD_ASS_STRING; + case 0x73000003: + return TCS_BOARD_ASS_STRING; + case 0x73000004: + return RW_ASS_STRING; + case 0x73000006: + return CAM_SWITCHER_STRING; + case 0x73000100: + return TM_FUNNEL_STRING; + case 0x73000101: + return PUS_TM_FUNNEL_STRING; + case 0x73000102: + return CFDP_TM_FUNNEL_STRING; + case 0x73000205: + return CFDP_HANDLER_STRING; + case 0x73000206: + return CFDP_DISTRIBUTOR_STRING; + case 0x73010000: + return EIVE_SYSTEM_STRING; + case 0x73010001: + return ACS_SUBSYSTEM_STRING; + case 0x73010002: + return PL_SUBSYSTEM_STRING; + case 0x73500000: + return CCSDS_IP_CORE_BRIDGE_STRING; case 0xFFFFFFFF: return NO_OBJECT_STRING; default: diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.h b/bsp_hosted/fsfwconfig/objects/translateObjects.h index dbf5b468..257912f4 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.h +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.h @@ -3,6 +3,6 @@ #include -const char* translateObject(object_id_t object); +const char *translateObject(object_id_t object); #endif /* FSFWCONFIG_OBJECTS_TRANSLATEOBJECTS_H_ */ From a55e6a17254d41deaf294caad2358d530c893572 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:48:44 +0100 Subject: [PATCH 155/244] logic fix --- linux/devices/ploc/PlocSupvUartMan.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index cdf74027..047bef59 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -294,7 +294,7 @@ void PlocSupvUartManager::stop() { void PlocSupvUartManager::start() { MutexGuard mg(lock); - if (state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) { + if (state != InternalState::SLEEPING and state != InternalState::GO_TO_SLEEP) { return; } semaphore->release(); From 1f6f11ee88998c7c397d39d2a6aaba91faeaf2ee Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 17:51:22 +0100 Subject: [PATCH 156/244] bugfix for start method --- linux/devices/ploc/PlocSupvUartMan.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 047bef59..8b341c56 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -289,6 +289,9 @@ ReturnValue_t PlocSupvUartManager::initiateUpdateContinuation() { void PlocSupvUartManager::stop() { MutexGuard mg(lock); + if(state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) { + return; + } state = InternalState::GO_TO_SLEEP; } @@ -297,6 +300,7 @@ void PlocSupvUartManager::start() { if (state != InternalState::SLEEPING and state != InternalState::GO_TO_SLEEP) { return; } + state = InternalState::DEFAULT; semaphore->release(); } From 9163db6bcf6b6eb115e8ddaa91d60bb6a1afd7f9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 18:02:17 +0100 Subject: [PATCH 157/244] add debug statements --- linux/devices/ploc/PlocSupvUartMan.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 8b341c56..47e63da7 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -3,6 +3,8 @@ #include #include #include + +#include #include #include @@ -99,6 +101,7 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { lock->unlockMutex(); semaphore->acquire(); while (true) { + sif::debug << "SUPV UART MAN: Running.." << std::endl; putTaskToSleep = handleUartReception(); if (putTaskToSleep) { performUartShutdown(); @@ -968,6 +971,8 @@ ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() { ReturnValue_t PlocSupvUartManager::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) { size_t encodedLen = 0; hdlc_add_framing(sendData, sendLen, encodedSendBuf.data(), &encodedLen); + sif::debug << "Sending TC" << std::endl; + arrayprinter::print(encodedSendBuf.data(), encodedLen); size_t bytesWritten = write(serialPort, encodedSendBuf.data(), encodedLen); if (bytesWritten != encodedLen) { sif::warning << "ScexUartReader::sendMessage: Sending ping command to solar experiment failed" From 32005a2d3ae8252689116cdf50c8d19a1e535d13 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Nov 2022 11:57:29 +0100 Subject: [PATCH 158/244] more printouts, switch on debug mode --- linux/devices/ploc/PlocSupvUartMan.cpp | 3 ++- linux/devices/ploc/PlocSupvUartMan.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 47e63da7..a80e23a8 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -159,6 +159,7 @@ bool PlocSupvUartManager::handleUartReception() { } else if (bytesRead > 0) { if (debugMode) { sif::info << "Received " << bytesRead << " bytes from the PLOC Supervisor:" << std::endl; + arrayprinter::print(recBuf.data(), bytesRead); } recRingBuf.writeData(recBuf.data(), bytesRead); tryHdlcParsing(); @@ -1013,7 +1014,7 @@ ReturnValue_t PlocSupvUartManager::tryHdlcParsing() { } recRingBuf.deleteData(bytesRead); } else if (result != NO_PACKET_FOUND) { - sif::warning << "ScexUartReader::performOperation: Possible packet loss" << std::endl; + sif::warning << "PlocSupvUartMan::performOperation: Possible packet loss" << std::endl; // Markers found at wrong place // which might be a hint for a possibly lost packet. // Maybe trigger event? diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index da8bb176..d872ff03 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -248,7 +248,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, std::array tmBuf{}; - bool debugMode = false; + bool debugMode = true; bool timestamping = true; // Remembers APID to know at which command a procedure failed From 3510cc85fcc330d1ca9c1e8047af37bdb051f0ad Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Nov 2022 13:26:49 +0100 Subject: [PATCH 159/244] a lot of bugfixes --- linux/devices/ploc/PlocSupervisorHandler.cpp | 11 +++++--- linux/devices/ploc/PlocSupvUartMan.cpp | 27 ++++++++++---------- linux/devices/ploc/PlocSupvUartMan.h | 6 ++--- thirdparty/tas/hdlc.c | 7 ++--- thirdparty/tas/tas/hdlc.h | 2 +- 5 files changed, 28 insertions(+), 25 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 6ea85c6f..8f77d619 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -1,6 +1,7 @@ #include "PlocSupervisorHandler.h" #include +#include #include #include @@ -586,6 +587,8 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r // } tmReader.setData(start, remainingSize); + sif::debug << "PlocSupervisorHandler::scanForReply: Received Packet" << std::endl; + arrayprinter::print(start, remainingSize); uint16_t apid = tmReader.getApid(); //(*(start) << 8 | *(start + 1)) & APID_MASK; switch (apid) { @@ -1915,13 +1918,13 @@ void PlocSupervisorHandler::handleBadApidServiceCombination(Event event, unsigne unsigned int serviceId) { const char* printString = ""; if (event == SUPV_UNKNOWN_TM) { - printString = "Unknown"; + printString = "PlocSupervisorHandler: Unknown"; } else if (event == SUPV_UNINIMPLEMENTED_TM) { - printString = "Unimplemented"; + printString = "PlocSupervisorHandler: Unimplemented"; } triggerEvent(event, apid, tmReader.getServiceId()); - sif::error << printString << " APID service combination 0x" << std::setw(2) << std::setfill('0') - << std::hex << apid << ", " << std::setw(2) << serviceId << std::endl; + sif::warning << printString << " APID service combination 0x" << std::setw(2) << std::setfill('0') + << std::hex << apid << ", 0x" << std::setw(2) << serviceId << std::endl; } void PlocSupervisorHandler::printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId) { diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index a80e23a8..4d4bee06 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1,10 +1,9 @@ #include #include // Contains file controls like O_RDWR #include +#include #include #include - -#include #include #include @@ -293,8 +292,8 @@ ReturnValue_t PlocSupvUartManager::initiateUpdateContinuation() { void PlocSupvUartManager::stop() { MutexGuard mg(lock); - if(state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) { - return; + if (state == InternalState::SLEEPING or state == InternalState::GO_TO_SLEEP) { + return; } state = InternalState::GO_TO_SLEEP; } @@ -1001,16 +1000,17 @@ ReturnValue_t PlocSupvUartManager::readReceivedMessage(CookieIF* cookie, uint8_t ReturnValue_t PlocSupvUartManager::tryHdlcParsing() { size_t bytesRead = 0; - ReturnValue_t result = parseRecRingBufForHdlc(bytesRead); + size_t decodedLen = 0; + ReturnValue_t result = parseRecRingBufForHdlc(bytesRead, decodedLen); if (result == returnvalue::OK) { // Packet found, advance read pointer. if (state == InternalState::DEDICATED_REQUEST) { - decodedRingBuf.writeData(decodedBuf.data(), bytesRead); - decodedQueue.insert(bytesRead); + decodedRingBuf.writeData(decodedBuf.data(), decodedLen); + decodedQueue.insert(decodedLen); } else { MutexGuard mg(ipcLock); - ipcRingBuf.writeData(decodedBuf.data(), bytesRead); - ipcQueue.insert(bytesRead); + ipcRingBuf.writeData(decodedBuf.data(), decodedLen); + ipcQueue.insert(decodedLen); } recRingBuf.deleteData(bytesRead); } else if (result != NO_PACKET_FOUND) { @@ -1023,7 +1023,7 @@ ReturnValue_t PlocSupvUartManager::tryHdlcParsing() { return result; } -ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize) { +ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize, size_t& decodedLen) { size_t availableData = recRingBuf.getAvailableReadData(); if (availableData == 0) { return NO_PACKET_FOUND; @@ -1037,7 +1037,6 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize) { } bool startMarkerFound = false; size_t startIdx = 0; - return returnvalue::OK; for (size_t idx = 0; idx < availableData; idx++) { // handle start marker if (encodedBuf[idx] == HDLC_START_MARKER) { @@ -1052,9 +1051,9 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize) { if (encodedBuf[idx] == HDLC_END_MARKER) { if (startMarkerFound) { // Probably a packet, so decode it - size_t decodedLen = 0; - hdlc_remove_framing(encodedBuf.data() + startIdx, idx + 1, decodedBuf.data(), &decodedLen); - readSize = decodedLen; + hdlc_remove_framing(encodedBuf.data() + startIdx, idx + 1 - startIdx, decodedBuf.data(), + &decodedLen); + readSize = idx + 1; return returnvalue::OK; } else { readSize = ++idx; diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index d872ff03..ada96bf5 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -177,8 +177,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF, static const uint32_t PREPARE_UPDATE_EXECUTION_REPORT = 2000; static constexpr uint8_t MAX_STORED_DECODED_PACKETS = 4; - static constexpr uint8_t HDLC_START_MARKER = 0x7C; - static constexpr uint8_t HDLC_END_MARKER = 0x7E; + static constexpr uint8_t HDLC_START_MARKER = 0x7E; + static constexpr uint8_t HDLC_END_MARKER = 0x7C; struct Update { uint8_t memoryId; @@ -261,7 +261,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, void executeFullCheckMemoryCommand(); ReturnValue_t tryHdlcParsing(); - ReturnValue_t parseRecRingBufForHdlc(size_t& readSize); + ReturnValue_t parseRecRingBufForHdlc(size_t& readSize, size_t& decodedLen); ReturnValue_t executeUpdate(); ReturnValue_t continueUpdate(); ReturnValue_t updateOperation(); diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 4734c0b9..49808c1f 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -55,15 +55,15 @@ void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dle *dlen = tlen; } -void hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) +int hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) { uint16_t tlen = 0; uint16_t ii; uint8_t bt; *dlen = 0; - if (slen == 0) return; - if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return; + if (slen == 0) return -1; + if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return -1; for (ii = 1; ii < slen-1; ii++) { bt = *src++; @@ -75,6 +75,7 @@ void hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t * dst[tlen++] = bt; } *dlen = tlen; + return 0; } diff --git a/thirdparty/tas/tas/hdlc.h b/thirdparty/tas/tas/hdlc.h index 97e69500..5599070b 100644 --- a/thirdparty/tas/tas/hdlc.h +++ b/thirdparty/tas/tas/hdlc.h @@ -28,7 +28,7 @@ extern "C" { void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); -void hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); +int hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); #ifdef __cplusplus } From 5b770a6407976b29b1da869d6e2b1cda20c23775 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Nov 2022 14:42:18 +0100 Subject: [PATCH 160/244] remove HDLC framing including CRC --- linux/devices/ploc/PlocSupvUartMan.cpp | 9 +++++++-- linux/devices/ploc/PlocSupvUartMan.h | 3 +++ thirdparty/tas/hdlc.c | 12 ++++++++---- thirdparty/tas/tas/hdlc.h | 15 ++++++++++++++- 4 files changed, 32 insertions(+), 7 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 4d4bee06..19dd25e0 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1051,9 +1051,14 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize, size if (encodedBuf[idx] == HDLC_END_MARKER) { if (startMarkerFound) { // Probably a packet, so decode it - hdlc_remove_framing(encodedBuf.data() + startIdx, idx + 1 - startIdx, decodedBuf.data(), - &decodedLen); + int retval = hdlc_remove_framing_with_crc_check( + encodedBuf.data() + startIdx, idx + 1 - startIdx, decodedBuf.data(), &decodedLen); readSize = idx + 1; + if (retval == -1 or retval == -2) { + triggerEvent(HDLC_FRAME_REMOVAL_ERROR, retval); + } else if (retval == 1) { + triggerEvent(HDLC_CRC_ERROR); + } return returnvalue::OK; } else { readSize = ++idx; diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index ada96bf5..58abf68b 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -114,6 +114,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF, //! [EXPORT] : [COMMENT] Will be triggered every 5 percent of the update progress. //! P1: First byte percent, third and fourth byte Sequence Count, P2: Bytes written static constexpr Event SUPV_UPDATE_PROGRESS = MAKE_EVENT(30, severity::INFO); + static constexpr Event HDLC_FRAME_REMOVAL_ERROR = MAKE_EVENT(31, severity::INFO); + static constexpr Event HDLC_CRC_ERROR = MAKE_EVENT(32, severity::INFO); PlocSupvUartManager(object_id_t objectId); virtual ~PlocSupvUartManager(); @@ -167,6 +169,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS_CONSECUTIVE_START = returnvalue::makeCode(1, 3); static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS_CONSECUTIVE_END = returnvalue::makeCode(1, 4); + static constexpr ReturnValue_t HDLC_ERROR = returnvalue::makeCode(1, 5); static const uint16_t CRC16_INIT = 0xFFFF; // Event buffer reply will carry 24 space packets with 1016 bytes and one space packet with diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 49808c1f..428a28da 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -55,15 +55,15 @@ void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dle *dlen = tlen; } -int hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) +int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) { uint16_t tlen = 0; uint16_t ii; uint8_t bt; *dlen = 0; - if (slen == 0) return -1; - if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return -1; + if (slen < 4) return -1; + if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return -2; for (ii = 1; ii < slen-1; ii++) { bt = *src++; @@ -74,7 +74,11 @@ int hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *d } dst[tlen++] = bt; } - *dlen = tlen; + // calc crc16 + if(calc_crc16_buff_reflected( dst, tlen ) != 0) { + return 1; + } + *dlen = tlen - 2; return 0; } diff --git a/thirdparty/tas/tas/hdlc.h b/thirdparty/tas/tas/hdlc.h index 5599070b..c4179d11 100644 --- a/thirdparty/tas/tas/hdlc.h +++ b/thirdparty/tas/tas/hdlc.h @@ -28,7 +28,20 @@ extern "C" { void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); -int hdlc_remove_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); +/** + * Decode a HDLC frame, including CRC check and CRC removal in addition + * to the removal of the frame markers. + * @param src + * @param slen + * @param dst + * @param dlen + * @return + * -1 Invalid source length + * -2 No start marker at first byte or end marker at slen - 1 + * 1 Invalid CRC + * 0 CRC OK, framing and CRC removed + */ +int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); #ifdef __cplusplus } From 03e007fd36d9a43b5bd4e20d596d50201d252f74 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Nov 2022 14:43:22 +0100 Subject: [PATCH 161/244] afmt --- linux/devices/ploc/PlocSupvUartMan.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 19dd25e0..0833cd67 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1059,6 +1059,9 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize, size } else if (retval == 1) { triggerEvent(HDLC_CRC_ERROR); } + if (retval != 0) { + return HDLC_ERROR; + } return returnvalue::OK; } else { readSize = ++idx; From 3ef0b3ea6de20519d2177ec569373d5c92b9bceb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Nov 2022 15:03:10 +0100 Subject: [PATCH 162/244] TAS PLOC SUPV --- bsp_q7s/core/InitMission.cpp | 88 ++++++++++---------- bsp_q7s/core/ObjectFactory.cpp | 8 +- linux/devices/ploc/PlocSupervisorHandler.cpp | 7 +- linux/devices/ploc/PlocSupervisorHandler.h | 5 +- 4 files changed, 53 insertions(+), 55 deletions(-) diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index bbeaded8..3b1eb357 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -1,7 +1,7 @@ #include "bsp_q7s/core/InitMission.h" #include -#include +#include #include #include @@ -74,7 +74,7 @@ void initmission::initTasks() { "SOLAR_ARRAY_DEPL", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc); result = solarArrayDeplTask->addComponent(objects::SOLAR_ARRAY_DEPL_HANDLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("SOLAR_ARRAY_DEPL", objects::SOLAR_ARRAY_DEPL_HANDLER); + scheduling::printAddObjectError("SOLAR_ARRAY_DEPL", objects::SOLAR_ARRAY_DEPL_HANDLER); } #endif @@ -82,11 +82,11 @@ void initmission::initTasks() { "CORE_CTRL", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc); result = sysTask->addComponent(objects::CORE_CONTROLLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER); + scheduling::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER); } result = sysTask->addComponent(objects::PL_SUBSYSTEM); if (result != returnvalue::OK) { - initmission::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM); + scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM); } /* TMTC Distribution */ @@ -95,24 +95,24 @@ void initmission::initTasks() { #if OBSW_ADD_TCPIP_BRIDGE == 1 result = tmTcDistributor->addComponent(objects::TMTC_BRIDGE); if (result != returnvalue::OK) { - initmission::printAddObjectError("TMTC_BRIDGE", objects::TMTC_BRIDGE); + scheduling::printAddObjectError("TMTC_BRIDGE", objects::TMTC_BRIDGE); } #endif result = tmTcDistributor->addComponent(objects::CCSDS_PACKET_DISTRIBUTOR); if (result != returnvalue::OK) { - initmission::printAddObjectError("CCSDS_DISTRIB", objects::CCSDS_PACKET_DISTRIBUTOR); + scheduling::printAddObjectError("CCSDS_DISTRIB", objects::CCSDS_PACKET_DISTRIBUTOR); } result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_PACKET_DISTRIB", objects::PUS_PACKET_DISTRIBUTOR); + scheduling::printAddObjectError("PUS_PACKET_DISTRIB", objects::PUS_PACKET_DISTRIBUTOR); } result = tmTcDistributor->addComponent(objects::CFDP_DISTRIBUTOR); if (result != returnvalue::OK) { - initmission::printAddObjectError("CFDP_DISTRIBUTOR", objects::CFDP_DISTRIBUTOR); + scheduling::printAddObjectError("CFDP_DISTRIBUTOR", objects::CFDP_DISTRIBUTOR); } result = tmTcDistributor->addComponent(objects::TM_FUNNEL); if (result != returnvalue::OK) { - initmission::printAddObjectError("TM_FUNNEL", objects::TM_FUNNEL); + scheduling::printAddObjectError("TM_FUNNEL", objects::TM_FUNNEL); } #if OBSW_ADD_TCPIP_BRIDGE == 1 @@ -120,7 +120,7 @@ void initmission::initTasks() { "TMTC_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = tmtcPollingTask->addComponent(objects::TMTC_POLLING_TASK); if (result != returnvalue::OK) { - initmission::printAddObjectError("UDP_POLLING", objects::TMTC_POLLING_TASK); + scheduling::printAddObjectError("UDP_POLLING", objects::TMTC_POLLING_TASK); } #endif @@ -129,7 +129,7 @@ void initmission::initTasks() { "CCSDS_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = ccsdsHandlerTask->addComponent(objects::CCSDS_HANDLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("CCSDS Handler", objects::CCSDS_HANDLER); + scheduling::printAddObjectError("CCSDS Handler", objects::CCSDS_HANDLER); } // Runs in IRQ mode, frequency does not really matter @@ -137,7 +137,7 @@ void initmission::initTasks() { "PDEC_HANDLER", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = pdecHandlerTask->addComponent(objects::PDEC_HANDLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("PDEC Handler", objects::PDEC_HANDLER); + scheduling::printAddObjectError("PDEC Handler", objects::PDEC_HANDLER); } #endif /* OBSW_ADD_CCSDS_IP_CORE == 1 */ @@ -146,7 +146,7 @@ void initmission::initTasks() { "CFDP Handler", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc); result = cfdpTask->addComponent(objects::CFDP_HANDLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("CFDP Handler", objects::CFDP_HANDLER); + scheduling::printAddObjectError("CFDP Handler", objects::CFDP_HANDLER); } #endif @@ -155,14 +155,14 @@ void initmission::initTasks() { #if OBSW_ADD_GPS_CTRL == 1 result = acsCtrlTask->addComponent(objects::GPS_CONTROLLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER); + scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER); } #endif /* OBSW_ADD_GPS_CTRL */ #if OBSW_ADD_ACS_CTRL == 1 acsCtrlTask->addComponent(objects::ACS_CONTROLLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); + scheduling::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER); } #endif @@ -172,24 +172,24 @@ void initmission::initTasks() { #if OBSW_ADD_ACS_BOARD == 1 result = acsSysTask->addComponent(objects::ACS_BOARD_ASS); if (result != returnvalue::OK) { - initmission::printAddObjectError("ACS_BOARD_ASS", objects::ACS_BOARD_ASS); + scheduling::printAddObjectError("ACS_BOARD_ASS", objects::ACS_BOARD_ASS); } #endif /* OBSW_ADD_ACS_HANDLERS */ #if OBSW_ADD_RW == 1 result = acsSysTask->addComponent(objects::RW_ASS); if (result != returnvalue::OK) { - initmission::printAddObjectError("RW_ASS", objects::RW_ASS); + scheduling::printAddObjectError("RW_ASS", objects::RW_ASS); } #endif #if OBSW_ADD_SUS_BOARD_ASS == 1 result = acsSysTask->addComponent(objects::SUS_BOARD_ASS); if (result != returnvalue::OK) { - initmission::printAddObjectError("SUS_BOARD_ASS", objects::SUS_BOARD_ASS); + scheduling::printAddObjectError("SUS_BOARD_ASS", objects::SUS_BOARD_ASS); } #endif result = acsSysTask->addComponent(objects::ACS_SUBSYSTEM); if (result != returnvalue::OK) { - initmission::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM); + scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM); } #if OBSW_ADD_RTD_DEVICES == 1 @@ -197,7 +197,7 @@ void initmission::initTasks() { "TCS_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc); result = tcsPollingTask->addComponent(objects::SPI_RTD_COM_IF); if (result != returnvalue::OK) { - initmission::printAddObjectError("SPI_RTD_POLLING", objects::SPI_RTD_COM_IF); + scheduling::printAddObjectError("SPI_RTD_POLLING", objects::SPI_RTD_COM_IF); } PeriodicTaskIF* tcsTask = factory->createPeriodicTask( @@ -235,19 +235,19 @@ void initmission::initTasks() { #if OBSW_ADD_RTD_DEVICES == 1 result = tcsSystemTask->addComponent(objects::TCS_BOARD_ASS); if (result != returnvalue::OK) { - initmission::printAddObjectError("TCS_BOARD_ASS", objects::TCS_BOARD_ASS); + scheduling::printAddObjectError("TCS_BOARD_ASS", objects::TCS_BOARD_ASS); } #endif /* OBSW_ADD_RTD_DEVICES */ #if OBSW_ADD_TCS_CTRL == 1 result = tcsSystemTask->addComponent(objects::THERMAL_CONTROLLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("THERMAL_CONTROLLER", objects::THERMAL_CONTROLLER); + scheduling::printAddObjectError("THERMAL_CONTROLLER", objects::THERMAL_CONTROLLER); } #endif #if OBSW_ADD_HEATERS == 1 result = tcsSystemTask->addComponent(objects::HEATER_HANDLER); if (result != returnvalue::OK) { - initmission::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER); + scheduling::printAddObjectError("HEATER_HANDLER", objects::HEATER_HANDLER); } #endif @@ -256,7 +256,7 @@ void initmission::initTasks() { "STR_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = strHelperTask->addComponent(objects::STR_HELPER); if (result != returnvalue::OK) { - initmission::printAddObjectError("STR_HELPER", objects::STR_HELPER); + scheduling::printAddObjectError("STR_HELPER", objects::STR_HELPER); } #endif /* OBSW_ADD_STAR_TRACKER == 1 */ @@ -265,7 +265,7 @@ void initmission::initTasks() { "PLOC_MPSOC_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = mpsocHelperTask->addComponent(objects::PLOC_MPSOC_HELPER); if (result != returnvalue::OK) { - initmission::printAddObjectError("PLOC_MPSOC_HELPER", objects::PLOC_MPSOC_HELPER); + scheduling::printAddObjectError("PLOC_MPSOC_HELPER", objects::PLOC_MPSOC_HELPER); } #endif /* OBSW_ADD_PLOC_MPSOC */ @@ -274,7 +274,7 @@ void initmission::initTasks() { "PLOC_SUPV_HELPER", 10, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc); result = supvHelperTask->addComponent(objects::PLOC_SUPERVISOR_HELPER); if (result != returnvalue::OK) { - initmission::printAddObjectError("PLOC_SUPV_HELPER", objects::PLOC_SUPERVISOR_HELPER); + scheduling::printAddObjectError("PLOC_SUPV_HELPER", objects::PLOC_SUPERVISOR_HELPER); } #endif /* OBSW_ADD_PLOC_SUPERVISOR */ @@ -300,7 +300,7 @@ void initmission::initTasks() { "PTME_TEST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc); result = ptmeTestTask->addComponent(objects::CCSDS_IP_CORE_BRIDGE); if (result != returnvalue::OK) { - initmission::printAddObjectError("PTME_TEST", objects::CCSDS_IP_CORE_BRIDGE); + scheduling::printAddObjectError("PTME_TEST", objects::CCSDS_IP_CORE_BRIDGE); } #endif std::vector testTasks; @@ -458,7 +458,7 @@ void initmission::createPusTasks(TaskFactory& factory, "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_VERIF", objects::PUS_SERVICE_1_VERIFICATION); + scheduling::printAddObjectError("PUS_VERIF", objects::PUS_SERVICE_1_VERIFICATION); } taskVec.push_back(pusVerification); @@ -466,11 +466,11 @@ void initmission::createPusTasks(TaskFactory& factory, "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc); result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING); + scheduling::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING); } result = pusEvents->addComponent(objects::EVENT_MANAGER); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER); + scheduling::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER); } taskVec.push_back(pusEvents); @@ -478,11 +478,11 @@ void initmission::createPusTasks(TaskFactory& factory, "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc); result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS); + scheduling::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS); } result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_9", objects::PUS_SERVICE_9_TIME_MGMT); + scheduling::printAddObjectError("PUS_9", objects::PUS_SERVICE_9_TIME_MGMT); } taskVec.push_back(pusHighPrio); @@ -492,32 +492,32 @@ void initmission::createPusTasks(TaskFactory& factory, result = pusMedPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_3", objects::PUS_SERVICE_3_HOUSEKEEPING); + scheduling::printAddObjectError("PUS_3", objects::PUS_SERVICE_3_HOUSEKEEPING); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_8", objects::PUS_SERVICE_8_FUNCTION_MGMT); + scheduling::printAddObjectError("PUS_8", objects::PUS_SERVICE_8_FUNCTION_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_11_TC_SCHEDULER); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_11", objects::PUS_SERVICE_11_TC_SCHEDULER); + scheduling::printAddObjectError("PUS_11", objects::PUS_SERVICE_11_TC_SCHEDULER); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_20", objects::PUS_SERVICE_20_PARAMETERS); + scheduling::printAddObjectError("PUS_20", objects::PUS_SERVICE_20_PARAMETERS); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_200", objects::PUS_SERVICE_200_MODE_MGMT); + scheduling::printAddObjectError("PUS_200", objects::PUS_SERVICE_200_MODE_MGMT); } result = pusMedPrio->addComponent(objects::PUS_SERVICE_201_HEALTH); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_201", objects::PUS_SERVICE_201_HEALTH); + scheduling::printAddObjectError("PUS_201", objects::PUS_SERVICE_201_HEALTH); } // Used for connection tests, therefore use higher priority result = pusMedPrio->addComponent(objects::PUS_SERVICE_17_TEST); if (result != returnvalue::OK) { - initmission::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); + scheduling::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST); } taskVec.push_back(pusMedPrio); @@ -525,7 +525,7 @@ void initmission::createPusTasks(TaskFactory& factory, "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc); result = pusLowPrio->addComponent(objects::INTERNAL_ERROR_REPORTER); if (result != returnvalue::OK) { - initmission::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); + scheduling::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER); } taskVec.push_back(pusLowPrio); } @@ -542,25 +542,25 @@ void initmission::createTestTasks(TaskFactory& factory, result = testTask->addComponent(objects::TEST_TASK); if (result != returnvalue::OK) { - initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK); + scheduling::printAddObjectError("TEST_TASK", objects::TEST_TASK); } #if OBSW_ADD_SPI_TEST_CODE == 1 result = testTask->addComponent(objects::SPI_TEST); if (result != returnvalue::OK) { - initmission::printAddObjectError("SPI_TEST", objects::SPI_TEST); + scheduling::printAddObjectError("SPI_TEST", objects::SPI_TEST); } #endif #if OBSW_ADD_I2C_TEST_CODE == 1 result = testTask->addComponent(objects::I2C_TEST); if (result != returnvalue::OK) { - initmission::printAddObjectError("I2C_TEST", objects::I2C_TEST); + scheduling::printAddObjectError("I2C_TEST", objects::I2C_TEST); } #endif #if OBSW_ADD_UART_TEST_CODE == 1 result = testTask->addComponent(objects::UART_TEST); if (result != returnvalue::OK) { - initmission::printAddObjectError("UART_TEST", objects::UART_TEST); + scheduling::printAddObjectError("UART_TEST", objects::UART_TEST); } #endif diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index a416fd58..85157e66 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -628,10 +628,10 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwit new SerialCookie(objects::PLOC_SUPERVISOR_HANDLER, q7s::UART_PLOC_SUPERVSIOR_DEV, uart::PLOC_SUPV_BAUD, supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); supervisorCookie->setNoFixedSizeReply(); - auto supvHelper = new PlocSupvHelper(objects::PLOC_SUPERVISOR_HELPER); - auto* supvHandler = new PlocSupervisorHandler( - objects::PLOC_SUPERVISOR_HANDLER, objects::UART_COM_IF, supervisorCookie, - Gpio(gpioIds::ENABLE_SUPV_UART, gpioComIF), pcdu::PDU1_CH6_PLOC_12V, supvHelper); + auto supvHelper = new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER); + auto* supvHandler = new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, supervisorCookie, + Gpio(gpioIds::ENABLE_SUPV_UART, gpioComIF), + pcdu::PDU1_CH6_PLOC_12V, *supvHelper); supvHandler->connectModeTreeParent(satsystem::pl::SUBSYSTEM); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ static_cast(consumer); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 8f77d619..feb67265 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -18,11 +18,10 @@ using namespace supv; using namespace returnvalue; -PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, object_id_t uartComIFid, - CookieIF* comCookie, Gpio uartIsolatorSwitch, - power::Switch_t powerSwitch, +PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, CookieIF* comCookie, + Gpio uartIsolatorSwitch, power::Switch_t powerSwitch, PlocSupvUartManager& supvHelper) - : DeviceHandlerBase(objectId, uartComIFid, comCookie), + : DeviceHandlerBase(objectId, supvHelper.getObjectId(), comCookie), uartIsolatorSwitch(uartIsolatorSwitch), hkset(this), bootStatusReport(this), diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index dd63c9e6..60970f33 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -32,9 +32,8 @@ using supv::ExecutionReport; */ class PlocSupervisorHandler : public DeviceHandlerBase { public: - PlocSupervisorHandler(object_id_t objectId, object_id_t uartComIFid, CookieIF* comCookie, - Gpio uartIsolatorSwitch, power::Switch_t powerSwitch, - PlocSupvUartManager& supvHelper); + PlocSupervisorHandler(object_id_t objectId, CookieIF* comCookie, Gpio uartIsolatorSwitch, + power::Switch_t powerSwitch, PlocSupvUartManager& supvHelper); virtual ~PlocSupervisorHandler(); virtual ReturnValue_t initialize() override; From eb371b30244994bbf71324be506db4ca00efb05d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Nov 2022 15:05:12 +0100 Subject: [PATCH 163/244] ctor fix --- bsp_hosted/ObjectFactory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 10c9e46d..2fd8230f 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -99,7 +99,7 @@ void ObjectFactory::produce(void* args) { supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); supervisorCookie->setNoFixedSizeReply(); auto supvHelper = new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER); - new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, objects::PLOC_SUPERVISOR_HELPER, + new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, dummyGpioIF), pcdu::PDU1_CH6_PLOC_12V, *supvHelper); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ From 4e3abb92f255760121f170b3596bd084b4cefac8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Nov 2022 15:25:29 +0100 Subject: [PATCH 164/244] update events --- bsp_hosted/ObjectFactory.cpp | 6 +++--- generators/bsp_q7s_events.csv | 2 ++ generators/events/translateEvents.cpp | 10 ++++++++-- generators/objects/translateObjects.cpp | 2 +- linux/devices/ploc/PlocSupvUartMan.cpp | 6 +++--- linux/fsfwconfig/events/translateEvents.cpp | 10 ++++++++-- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 8 files changed, 27 insertions(+), 13 deletions(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 2fd8230f..89b83041 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -99,9 +99,9 @@ void ObjectFactory::produce(void* args) { supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL); supervisorCookie->setNoFixedSizeReply(); auto supvHelper = new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER); - new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, - supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, dummyGpioIF), - pcdu::PDU1_CH6_PLOC_12V, *supvHelper); + new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, supervisorCookie, + Gpio(gpioIds::ENABLE_SUPV_UART, dummyGpioIF), pcdu::PDU1_CH6_PLOC_12V, + *supvHelper); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ #endif diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index bb762004..9a89eca9 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -223,6 +223,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 13628;0x353c;SUPV_REPLY_SIZE_MISSMATCH;LOW;;linux/devices/ploc/PlocSupvUartMan.h 13629;0x353d;SUPV_REPLY_CRC_MISSMATCH;LOW;;linux/devices/ploc/PlocSupvUartMan.h 13630;0x353e;SUPV_UPDATE_PROGRESS;INFO;Will be triggered every 5 percent of the update progress. P1: First byte percent, third and fourth byte Sequence Count, P2: Bytes written;linux/devices/ploc/PlocSupvUartMan.h +13631;0x353f;HDLC_FRAME_REMOVAL_ERROR;INFO;;linux/devices/ploc/PlocSupvUartMan.h +13632;0x3540;HDLC_CRC_ERROR;INFO;;linux/devices/ploc/PlocSupvUartMan.h 13700;0x3584;ALLOC_FAILURE;MEDIUM;;bsp_q7s/core/CoreController.h 13701;0x3585;REBOOT_SW;MEDIUM; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h 13702;0x3586;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 1b534866..ac00745c 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 237 translations. + * @brief Auto-generated event translation file. Contains 239 translations. * @details - * Generated on: 2022-11-15 17:44:20 + * Generated on: 2022-11-16 15:25:08 */ #include "translateEvents.h" @@ -225,6 +225,8 @@ const char *WRITE_MEMORY_FAILED_STRING = "WRITE_MEMORY_FAILED"; const char *SUPV_REPLY_SIZE_MISSMATCH_STRING = "SUPV_REPLY_SIZE_MISSMATCH"; const char *SUPV_REPLY_CRC_MISSMATCH_STRING = "SUPV_REPLY_CRC_MISSMATCH"; const char *SUPV_UPDATE_PROGRESS_STRING = "SUPV_UPDATE_PROGRESS"; +const char *HDLC_FRAME_REMOVAL_ERROR_STRING = "HDLC_FRAME_REMOVAL_ERROR"; +const char *HDLC_CRC_ERROR_STRING = "HDLC_CRC_ERROR"; const char *ALLOC_FAILURE_STRING = "ALLOC_FAILURE"; const char *REBOOT_SW_STRING = "REBOOT_SW"; const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED"; @@ -681,6 +683,10 @@ const char *translateEvents(Event event) { return SUPV_REPLY_CRC_MISSMATCH_STRING; case (13630): return SUPV_UPDATE_PROGRESS_STRING; + case (13631): + return HDLC_FRAME_REMOVAL_ERROR_STRING; + case (13632): + return HDLC_CRC_ERROR_STRING; case (13700): return ALLOC_FAILURE_STRING; case (13701): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 4856c347..0890725e 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2022-11-15 17:44:20 + * Generated on: 2022-11-16 15:25:08 */ #include "translateObjects.h" diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 0833cd67..4fe9a033 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -51,8 +51,8 @@ ReturnValue_t PlocSupvUartManager::initializeInterface(CookieIF* cookie) { /* Get file descriptor */ serialPort = open(devname.c_str(), O_RDWR); if (serialPort < 0) { - sif::warning << "ScexUartReader::initializeInterface: open call failed with error [" << errno - << ", " << strerror(errno) << std::endl; + sif::warning << "PlocSupvUartManager::initializeInterface: open call failed with error [" + << errno << ", " << strerror(errno) << std::endl; return FAILED; } // Setting up UART parameters @@ -73,7 +73,7 @@ ReturnValue_t PlocSupvUartManager::initializeInterface(CookieIF* cookie) { uart::setBaudrate(tty, uartCookie->getBaudrate()); if (tcsetattr(serialPort, TCSANOW, &tty) != 0) { - sif::warning << "ScexUartReader::initializeInterface: tcsetattr call failed with error [" + sif::warning << "PlocSupvUartManager::initializeInterface: tcsetattr call failed with error [" << errno << ", " << strerror(errno) << std::endl; } // Flush received and unread data diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 1b534866..ac00745c 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 237 translations. + * @brief Auto-generated event translation file. Contains 239 translations. * @details - * Generated on: 2022-11-15 17:44:20 + * Generated on: 2022-11-16 15:25:08 */ #include "translateEvents.h" @@ -225,6 +225,8 @@ const char *WRITE_MEMORY_FAILED_STRING = "WRITE_MEMORY_FAILED"; const char *SUPV_REPLY_SIZE_MISSMATCH_STRING = "SUPV_REPLY_SIZE_MISSMATCH"; const char *SUPV_REPLY_CRC_MISSMATCH_STRING = "SUPV_REPLY_CRC_MISSMATCH"; const char *SUPV_UPDATE_PROGRESS_STRING = "SUPV_UPDATE_PROGRESS"; +const char *HDLC_FRAME_REMOVAL_ERROR_STRING = "HDLC_FRAME_REMOVAL_ERROR"; +const char *HDLC_CRC_ERROR_STRING = "HDLC_CRC_ERROR"; const char *ALLOC_FAILURE_STRING = "ALLOC_FAILURE"; const char *REBOOT_SW_STRING = "REBOOT_SW"; const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED"; @@ -681,6 +683,10 @@ const char *translateEvents(Event event) { return SUPV_REPLY_CRC_MISSMATCH_STRING; case (13630): return SUPV_UPDATE_PROGRESS_STRING; + case (13631): + return HDLC_FRAME_REMOVAL_ERROR_STRING; + case (13632): + return HDLC_CRC_ERROR_STRING; case (13700): return ALLOC_FAILURE_STRING; case (13701): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 4856c347..0890725e 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2022-11-15 17:44:20 + * Generated on: 2022-11-16 15:25:08 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index a7999bdb..d5813e1a 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a7999bdbb1d3f490d383c3b085b9019730ec1450 +Subproject commit d5813e1a422beae7d0cb092a885e0956e9f1ddc1 From 612a475eecf58f7cdb11be65c019988ec5115f4b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Nov 2022 15:26:47 +0100 Subject: [PATCH 165/244] update event translations --- .../fsfwconfig/events/translateEvents.cpp | 480 +++++++++++++++++- .../fsfwconfig/events/translateEvents.h | 4 +- 2 files changed, 470 insertions(+), 14 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index fb9e1bf4..ac00745c 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 83 translations. + * @brief Auto-generated event translation file. Contains 239 translations. * @details - * Generated on: 2021-05-17 19:49:55 + * Generated on: 2022-11-16 15:25:08 */ #include "translateEvents.h" @@ -34,6 +34,7 @@ const char *DEVICE_UNREQUESTED_REPLY_STRING = "DEVICE_UNREQUESTED_REPLY"; const char *INVALID_DEVICE_COMMAND_STRING = "INVALID_DEVICE_COMMAND"; const char *MONITORING_LIMIT_EXCEEDED_STRING = "MONITORING_LIMIT_EXCEEDED"; const char *MONITORING_AMBIGUOUS_STRING = "MONITORING_AMBIGUOUS"; +const char *DEVICE_WANTS_HARD_REBOOT_STRING = "DEVICE_WANTS_HARD_REBOOT"; const char *FUSE_CURRENT_HIGH_STRING = "FUSE_CURRENT_HIGH"; const char *FUSE_WENT_OFF_STRING = "FUSE_WENT_OFF"; const char *POWER_ABOVE_HIGH_LIMIT_STRING = "POWER_ABOVE_HIGH_LIMIT"; @@ -59,7 +60,6 @@ const char *MONITOR_CHANGED_STATE_STRING = "MONITOR_CHANGED_STATE"; const char *VALUE_BELOW_LOW_LIMIT_STRING = "VALUE_BELOW_LOW_LIMIT"; const char *VALUE_ABOVE_HIGH_LIMIT_STRING = "VALUE_ABOVE_HIGH_LIMIT"; const char *VALUE_OUT_OF_RANGE_STRING = "VALUE_OUT_OF_RANGE"; -const char *SWITCHING_TM_FAILED_STRING = "SWITCHING_TM_FAILED"; const char *CHANGING_MODE_STRING = "CHANGING_MODE"; const char *MODE_INFO_STRING = "MODE_INFO"; const char *FALLBACK_FAILED_STRING = "FALLBACK_FAILED"; @@ -75,6 +75,7 @@ const char *OVERWRITING_HEALTH_STRING = "OVERWRITING_HEALTH"; const char *TRYING_RECOVERY_STRING = "TRYING_RECOVERY"; const char *RECOVERY_STEP_STRING = "RECOVERY_STEP"; const char *RECOVERY_DONE_STRING = "RECOVERY_DONE"; +const char *HANDLE_PACKET_FAILED_STRING = "HANDLE_PACKET_FAILED"; const char *RF_AVAILABLE_STRING = "RF_AVAILABLE"; const char *RF_LOST_STRING = "RF_LOST"; const char *BIT_LOCK_STRING = "BIT_LOCK"; @@ -82,15 +83,166 @@ const char *BIT_LOCK_LOST_STRING = "BIT_LOCK_LOST"; const char *FRAME_PROCESSING_FAILED_STRING = "FRAME_PROCESSING_FAILED"; const char *CLOCK_SET_STRING = "CLOCK_SET"; const char *CLOCK_SET_FAILURE_STRING = "CLOCK_SET_FAILURE"; +const char *TC_DELETION_FAILED_STRING = "TC_DELETION_FAILED"; const char *TEST_STRING = "TEST"; const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER"; +const char *STORE_ERROR_STRING = "STORE_ERROR"; +const char *MSG_QUEUE_ERROR_STRING = "MSG_QUEUE_ERROR"; +const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR"; +const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; +const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; +const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; +const char *FDIR_REACTION_IGNORED_STRING = "FDIR_REACTION_IGNORED"; +const char *GPIO_PULL_HIGH_FAILED_STRING = "GPIO_PULL_HIGH_FAILED"; +const char *GPIO_PULL_LOW_FAILED_STRING = "GPIO_PULL_LOW_FAILED"; +const char *HEATER_WENT_ON_STRING = "HEATER_WENT_ON"; +const char *HEATER_WENT_OFF_STRING = "HEATER_WENT_OFF"; +const char *SWITCH_ALREADY_ON_STRING = "SWITCH_ALREADY_ON"; +const char *SWITCH_ALREADY_OFF_STRING = "SWITCH_ALREADY_OFF"; +const char *MAIN_SWITCH_TIMEOUT_STRING = "MAIN_SWITCH_TIMEOUT"; +const char *FAULTY_HEATER_WAS_ON_STRING = "FAULTY_HEATER_WAS_ON"; +const char *BURN_PHASE_START_STRING = "BURN_PHASE_START"; +const char *BURN_PHASE_DONE_STRING = "BURN_PHASE_DONE"; +const char *MAIN_SWITCH_ON_TIMEOUT_STRING = "MAIN_SWITCH_ON_TIMEOUT"; +const char *MAIN_SWITCH_OFF_TIMEOUT_STRING = "MAIN_SWITCH_OFF_TIMEOUT"; +const char *DEPL_SA1_GPIO_SWTICH_ON_FAILED_STRING = "DEPL_SA1_GPIO_SWTICH_ON_FAILED"; +const char *DEPL_SA2_GPIO_SWTICH_ON_FAILED_STRING = "DEPL_SA2_GPIO_SWTICH_ON_FAILED"; +const char *DEPL_SA1_GPIO_SWTICH_OFF_FAILED_STRING = "DEPL_SA1_GPIO_SWTICH_OFF_FAILED"; +const char *DEPL_SA2_GPIO_SWTICH_OFF_FAILED_STRING = "DEPL_SA2_GPIO_SWTICH_OFF_FAILED"; +const char *AUTONOMOUS_DEPLOYMENT_COMPLETED_STRING = "AUTONOMOUS_DEPLOYMENT_COMPLETED"; const char *MEMORY_READ_RPT_CRC_FAILURE_STRING = "MEMORY_READ_RPT_CRC_FAILURE"; const char *ACK_FAILURE_STRING = "ACK_FAILURE"; const char *EXE_FAILURE_STRING = "EXE_FAILURE"; -const char *CRC_FAILURE_EVENT_STRING = "CRC_FAILURE_EVENT"; +const char *MPSOC_HANDLER_CRC_FAILURE_STRING = "MPSOC_HANDLER_CRC_FAILURE"; +const char *MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH_STRING = "MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH"; +const char *MPSOC_SHUTDOWN_FAILED_STRING = "MPSOC_SHUTDOWN_FAILED"; +const char *SELF_TEST_I2C_FAILURE_STRING = "SELF_TEST_I2C_FAILURE"; +const char *SELF_TEST_SPI_FAILURE_STRING = "SELF_TEST_SPI_FAILURE"; +const char *SELF_TEST_ADC_FAILURE_STRING = "SELF_TEST_ADC_FAILURE"; +const char *SELF_TEST_PWM_FAILURE_STRING = "SELF_TEST_PWM_FAILURE"; +const char *SELF_TEST_TC_FAILURE_STRING = "SELF_TEST_TC_FAILURE"; +const char *SELF_TEST_MTM_RANGE_FAILURE_STRING = "SELF_TEST_MTM_RANGE_FAILURE"; +const char *SELF_TEST_COIL_CURRENT_FAILURE_STRING = "SELF_TEST_COIL_CURRENT_FAILURE"; +const char *INVALID_ERROR_BYTE_STRING = "INVALID_ERROR_BYTE"; +const char *ERROR_STATE_STRING = "ERROR_STATE"; +const char *RESET_OCCURED_STRING = "RESET_OCCURED"; +const char *BOOTING_FIRMWARE_FAILED_STRING = "BOOTING_FIRMWARE_FAILED"; +const char *BOOTING_BOOTLOADER_FAILED_STRING = "BOOTING_BOOTLOADER_FAILED"; +const char *SUPV_MEMORY_READ_RPT_CRC_FAILURE_STRING = "SUPV_MEMORY_READ_RPT_CRC_FAILURE"; +const char *SUPV_UNKNOWN_TM_STRING = "SUPV_UNKNOWN_TM"; +const char *SUPV_UNINIMPLEMENTED_TM_STRING = "SUPV_UNINIMPLEMENTED_TM"; +const char *SUPV_ACK_FAILURE_STRING = "SUPV_ACK_FAILURE"; +const char *SUPV_EXE_FAILURE_STRING = "SUPV_EXE_FAILURE"; +const char *SUPV_CRC_FAILURE_EVENT_STRING = "SUPV_CRC_FAILURE_EVENT"; +const char *SUPV_HELPER_EXECUTING_STRING = "SUPV_HELPER_EXECUTING"; +const char *SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUWDOWN_BUILD_FAILED"; +const char *SANITIZATION_FAILED_STRING = "SANITIZATION_FAILED"; +const char *MOUNTED_SD_CARD_STRING = "MOUNTED_SD_CARD"; +const char *SEND_MRAM_DUMP_FAILED_STRING = "SEND_MRAM_DUMP_FAILED"; +const char *MRAM_DUMP_FAILED_STRING = "MRAM_DUMP_FAILED"; +const char *MRAM_DUMP_FINISHED_STRING = "MRAM_DUMP_FINISHED"; +const char *INVALID_TC_FRAME_STRING = "INVALID_TC_FRAME"; +const char *INVALID_FAR_STRING = "INVALID_FAR"; +const char *CARRIER_LOCK_STRING = "CARRIER_LOCK"; +const char *BIT_LOCK_PDEC_STRING = "BIT_LOCK_PDEC"; +const char *LOST_CARRIER_LOCK_PDEC_STRING = "LOST_CARRIER_LOCK_PDEC"; +const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC"; +const char *POLL_ERROR_PDEC_STRING = "POLL_ERROR_PDEC"; +const char *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED"; +const char *IMAGE_DOWNLOAD_FAILED_STRING = "IMAGE_DOWNLOAD_FAILED"; +const char *IMAGE_UPLOAD_SUCCESSFUL_STRING = "IMAGE_UPLOAD_SUCCESSFUL"; +const char *IMAGE_DOWNLOAD_SUCCESSFUL_STRING = "IMAGE_DOWNLOAD_SUCCESSFUL"; +const char *FLASH_WRITE_SUCCESSFUL_STRING = "FLASH_WRITE_SUCCESSFUL"; +const char *FLASH_READ_SUCCESSFUL_STRING = "FLASH_READ_SUCCESSFUL"; +const char *FLASH_READ_FAILED_STRING = "FLASH_READ_FAILED"; +const char *FIRMWARE_UPDATE_SUCCESSFUL_STRING = "FIRMWARE_UPDATE_SUCCESSFUL"; +const char *FIRMWARE_UPDATE_FAILED_STRING = "FIRMWARE_UPDATE_FAILED"; +const char *STR_HELPER_READING_REPLY_FAILED_STRING = "STR_HELPER_READING_REPLY_FAILED"; +const char *STR_HELPER_COM_ERROR_STRING = "STR_HELPER_COM_ERROR"; +const char *STR_HELPER_NO_REPLY_STRING = "STR_HELPER_NO_REPLY"; +const char *STR_HELPER_DEC_ERROR_STRING = "STR_HELPER_DEC_ERROR"; +const char *POSITION_MISMATCH_STRING = "POSITION_MISMATCH"; +const char *STR_HELPER_FILE_NOT_EXISTS_STRING = "STR_HELPER_FILE_NOT_EXISTS"; +const char *STR_HELPER_SENDING_PACKET_FAILED_STRING = "STR_HELPER_SENDING_PACKET_FAILED"; +const char *STR_HELPER_REQUESTING_MSG_FAILED_STRING = "STR_HELPER_REQUESTING_MSG_FAILED"; +const char *MPSOC_FLASH_WRITE_FAILED_STRING = "MPSOC_FLASH_WRITE_FAILED"; +const char *MPSOC_FLASH_WRITE_SUCCESSFUL_STRING = "MPSOC_FLASH_WRITE_SUCCESSFUL"; +const char *MPSOC_SENDING_COMMAND_FAILED_STRING = "MPSOC_SENDING_COMMAND_FAILED"; +const char *MPSOC_HELPER_REQUESTING_REPLY_FAILED_STRING = "MPSOC_HELPER_REQUESTING_REPLY_FAILED"; +const char *MPSOC_HELPER_READING_REPLY_FAILED_STRING = "MPSOC_HELPER_READING_REPLY_FAILED"; +const char *MPSOC_MISSING_ACK_STRING = "MPSOC_MISSING_ACK"; +const char *MPSOC_MISSING_EXE_STRING = "MPSOC_MISSING_EXE"; +const char *MPSOC_ACK_FAILURE_REPORT_STRING = "MPSOC_ACK_FAILURE_REPORT"; +const char *MPSOC_EXE_FAILURE_REPORT_STRING = "MPSOC_EXE_FAILURE_REPORT"; +const char *MPSOC_ACK_INVALID_APID_STRING = "MPSOC_ACK_INVALID_APID"; +const char *MPSOC_EXE_INVALID_APID_STRING = "MPSOC_EXE_INVALID_APID"; +const char *MPSOC_HELPER_SEQ_CNT_MISMATCH_STRING = "MPSOC_HELPER_SEQ_CNT_MISMATCH"; +const char *MPSOC_TM_SIZE_ERROR_STRING = "MPSOC_TM_SIZE_ERROR"; +const char *MPSOC_TM_CRC_MISSMATCH_STRING = "MPSOC_TM_CRC_MISSMATCH"; +const char *TRANSITION_BACK_TO_OFF_STRING = "TRANSITION_BACK_TO_OFF"; +const char *NEG_V_OUT_OF_BOUNDS_STRING = "NEG_V_OUT_OF_BOUNDS"; +const char *U_DRO_OUT_OF_BOUNDS_STRING = "U_DRO_OUT_OF_BOUNDS"; +const char *I_DRO_OUT_OF_BOUNDS_STRING = "I_DRO_OUT_OF_BOUNDS"; +const char *U_X8_OUT_OF_BOUNDS_STRING = "U_X8_OUT_OF_BOUNDS"; +const char *I_X8_OUT_OF_BOUNDS_STRING = "I_X8_OUT_OF_BOUNDS"; +const char *U_TX_OUT_OF_BOUNDS_STRING = "U_TX_OUT_OF_BOUNDS"; +const char *I_TX_OUT_OF_BOUNDS_STRING = "I_TX_OUT_OF_BOUNDS"; +const char *U_MPA_OUT_OF_BOUNDS_STRING = "U_MPA_OUT_OF_BOUNDS"; +const char *I_MPA_OUT_OF_BOUNDS_STRING = "I_MPA_OUT_OF_BOUNDS"; +const char *U_HPA_OUT_OF_BOUNDS_STRING = "U_HPA_OUT_OF_BOUNDS"; +const char *I_HPA_OUT_OF_BOUNDS_STRING = "I_HPA_OUT_OF_BOUNDS"; +const char *TRANSITION_OTHER_SIDE_FAILED_STRING = "TRANSITION_OTHER_SIDE_FAILED"; +const char *NOT_ENOUGH_DEVICES_DUAL_MODE_STRING = "NOT_ENOUGH_DEVICES_DUAL_MODE"; +const char *POWER_STATE_MACHINE_TIMEOUT_STRING = "POWER_STATE_MACHINE_TIMEOUT"; +const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED"; +const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE"; +const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE"; +const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT"; +const char *BATT_MODE_STRING = "BATT_MODE"; +const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED"; +const char *SUPV_UPDATE_FAILED_STRING = "SUPV_UPDATE_FAILED"; +const char *SUPV_UPDATE_SUCCESSFUL_STRING = "SUPV_UPDATE_SUCCESSFUL"; +const char *SUPV_CONTINUE_UPDATE_FAILED_STRING = "SUPV_CONTINUE_UPDATE_FAILED"; +const char *SUPV_CONTINUE_UPDATE_SUCCESSFUL_STRING = "SUPV_CONTINUE_UPDATE_SUCCESSFUL"; +const char *TERMINATED_UPDATE_PROCEDURE_STRING = "TERMINATED_UPDATE_PROCEDURE"; +const char *SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL_STRING = "SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL"; +const char *SUPV_EVENT_BUFFER_REQUEST_FAILED_STRING = "SUPV_EVENT_BUFFER_REQUEST_FAILED"; +const char *SUPV_EVENT_BUFFER_REQUEST_TERMINATED_STRING = "SUPV_EVENT_BUFFER_REQUEST_TERMINATED"; +const char *SUPV_MEM_CHECK_OK_STRING = "SUPV_MEM_CHECK_OK"; +const char *SUPV_MEM_CHECK_FAIL_STRING = "SUPV_MEM_CHECK_FAIL"; +const char *SUPV_SENDING_COMMAND_FAILED_STRING = "SUPV_SENDING_COMMAND_FAILED"; +const char *SUPV_HELPER_REQUESTING_REPLY_FAILED_STRING = "SUPV_HELPER_REQUESTING_REPLY_FAILED"; +const char *SUPV_HELPER_READING_REPLY_FAILED_STRING = "SUPV_HELPER_READING_REPLY_FAILED"; +const char *SUPV_MISSING_ACK_STRING = "SUPV_MISSING_ACK"; +const char *SUPV_MISSING_EXE_STRING = "SUPV_MISSING_EXE"; +const char *SUPV_ACK_FAILURE_REPORT_STRING = "SUPV_ACK_FAILURE_REPORT"; +const char *SUPV_EXE_FAILURE_REPORT_STRING = "SUPV_EXE_FAILURE_REPORT"; +const char *SUPV_ACK_INVALID_APID_STRING = "SUPV_ACK_INVALID_APID"; +const char *SUPV_EXE_INVALID_APID_STRING = "SUPV_EXE_INVALID_APID"; +const char *ACK_RECEPTION_FAILURE_STRING = "ACK_RECEPTION_FAILURE"; +const char *EXE_RECEPTION_FAILURE_STRING = "EXE_RECEPTION_FAILURE"; +const char *WRITE_MEMORY_FAILED_STRING = "WRITE_MEMORY_FAILED"; +const char *SUPV_REPLY_SIZE_MISSMATCH_STRING = "SUPV_REPLY_SIZE_MISSMATCH"; +const char *SUPV_REPLY_CRC_MISSMATCH_STRING = "SUPV_REPLY_CRC_MISSMATCH"; +const char *SUPV_UPDATE_PROGRESS_STRING = "SUPV_UPDATE_PROGRESS"; +const char *HDLC_FRAME_REMOVAL_ERROR_STRING = "HDLC_FRAME_REMOVAL_ERROR"; +const char *HDLC_CRC_ERROR_STRING = "HDLC_CRC_ERROR"; +const char *ALLOC_FAILURE_STRING = "ALLOC_FAILURE"; +const char *REBOOT_SW_STRING = "REBOOT_SW"; +const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED"; +const char *REBOOT_HW_STRING = "REBOOT_HW"; +const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE"; +const char *MISSING_PACKET_STRING = "MISSING_PACKET"; +const char *EXPERIMENT_TIMEDOUT_STRING = "EXPERIMENT_TIMEDOUT"; +const char *MULTI_PACKET_COMMAND_DONE_STRING = "MULTI_PACKET_COMMAND_DONE"; +const char *SET_CONFIGFILEVALUE_FAILED_STRING = "SET_CONFIGFILEVALUE_FAILED"; +const char *GET_CONFIGFILEVALUE_FAILED_STRING = "GET_CONFIGFILEVALUE_FAILED"; +const char *INSERT_CONFIGFILEVALUE_FAILED_STRING = "INSERT_CONFIGFILEVALUE_FAILED"; +const char *WRITE_CONFIGFILE_FAILED_STRING = "WRITE_CONFIGFILE_FAILED"; +const char *READ_CONFIGFILE_FAILED_STRING = "READ_CONFIGFILE_FAILED"; const char *translateEvents(Event event) { - switch ((event & 0xffff)) { + switch ((event & 0xFFFF)) { case (2200): return STORE_SEND_WRITE_FAILED_STRING; case (2201): @@ -149,6 +301,8 @@ const char *translateEvents(Event event) { return MONITORING_LIMIT_EXCEEDED_STRING; case (2810): return MONITORING_AMBIGUOUS_STRING; + case (2811): + return DEVICE_WANTS_HARD_REBOOT_STRING; case (4201): return FUSE_CURRENT_HIGH_STRING; case (4202): @@ -199,8 +353,6 @@ const char *translateEvents(Event event) { return VALUE_ABOVE_HIGH_LIMIT_STRING; case (7204): return VALUE_OUT_OF_RANGE_STRING; - case (7301): - return SWITCHING_TM_FAILED_STRING; case (7400): return CHANGING_MODE_STRING; case (7401): @@ -231,6 +383,8 @@ const char *translateEvents(Event event) { return RECOVERY_STEP_STRING; case (7512): return RECOVERY_DONE_STRING; + case (7600): + return HANDLE_PACKET_FAILED_STRING; case (7900): return RF_AVAILABLE_STRING; case (7901): @@ -245,18 +399,320 @@ const char *translateEvents(Event event) { return CLOCK_SET_STRING; case (8901): return CLOCK_SET_FAILURE_STRING; + case (9100): + return TC_DELETION_FAILED_STRING; case (9700): return TEST_STRING; case (10600): return CHANGE_OF_SETUP_PARAMETER_STRING; - case (11101): + case (10800): + return STORE_ERROR_STRING; + case (10801): + return MSG_QUEUE_ERROR_STRING; + case (10802): + return SERIALIZATION_ERROR_STRING; + case (11300): + return SWITCH_CMD_SENT_STRING; + case (11301): + return SWITCH_HAS_CHANGED_STRING; + case (11302): + return SWITCHING_Q7S_DENIED_STRING; + case (11303): + return FDIR_REACTION_IGNORED_STRING; + case (11400): + return GPIO_PULL_HIGH_FAILED_STRING; + case (11401): + return GPIO_PULL_LOW_FAILED_STRING; + case (11402): + return HEATER_WENT_ON_STRING; + case (11403): + return HEATER_WENT_OFF_STRING; + case (11404): + return SWITCH_ALREADY_ON_STRING; + case (11405): + return SWITCH_ALREADY_OFF_STRING; + case (11406): + return MAIN_SWITCH_TIMEOUT_STRING; + case (11407): + return FAULTY_HEATER_WAS_ON_STRING; + case (11500): + return BURN_PHASE_START_STRING; + case (11501): + return BURN_PHASE_DONE_STRING; + case (11502): + return MAIN_SWITCH_ON_TIMEOUT_STRING; + case (11503): + return MAIN_SWITCH_OFF_TIMEOUT_STRING; + case (11504): + return DEPL_SA1_GPIO_SWTICH_ON_FAILED_STRING; + case (11505): + return DEPL_SA2_GPIO_SWTICH_ON_FAILED_STRING; + case (11506): + return DEPL_SA1_GPIO_SWTICH_OFF_FAILED_STRING; + case (11507): + return DEPL_SA2_GPIO_SWTICH_OFF_FAILED_STRING; + case (11508): + return AUTONOMOUS_DEPLOYMENT_COMPLETED_STRING; + case (11601): return MEMORY_READ_RPT_CRC_FAILURE_STRING; - case (11102): + case (11602): return ACK_FAILURE_STRING; - case (11103): + case (11603): return EXE_FAILURE_STRING; - case (11104): - return CRC_FAILURE_EVENT_STRING; + case (11604): + return MPSOC_HANDLER_CRC_FAILURE_STRING; + case (11605): + return MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH_STRING; + case (11606): + return MPSOC_SHUTDOWN_FAILED_STRING; + case (11701): + return SELF_TEST_I2C_FAILURE_STRING; + case (11702): + return SELF_TEST_SPI_FAILURE_STRING; + case (11703): + return SELF_TEST_ADC_FAILURE_STRING; + case (11704): + return SELF_TEST_PWM_FAILURE_STRING; + case (11705): + return SELF_TEST_TC_FAILURE_STRING; + case (11706): + return SELF_TEST_MTM_RANGE_FAILURE_STRING; + case (11707): + return SELF_TEST_COIL_CURRENT_FAILURE_STRING; + case (11708): + return INVALID_ERROR_BYTE_STRING; + case (11801): + return ERROR_STATE_STRING; + case (11802): + return RESET_OCCURED_STRING; + case (11901): + return BOOTING_FIRMWARE_FAILED_STRING; + case (11902): + return BOOTING_BOOTLOADER_FAILED_STRING; + case (12001): + return SUPV_MEMORY_READ_RPT_CRC_FAILURE_STRING; + case (12002): + return SUPV_UNKNOWN_TM_STRING; + case (12003): + return SUPV_UNINIMPLEMENTED_TM_STRING; + case (12004): + return SUPV_ACK_FAILURE_STRING; + case (12005): + return SUPV_EXE_FAILURE_STRING; + case (12006): + return SUPV_CRC_FAILURE_EVENT_STRING; + case (12007): + return SUPV_HELPER_EXECUTING_STRING; + case (12008): + return SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING; + case (12100): + return SANITIZATION_FAILED_STRING; + case (12101): + return MOUNTED_SD_CARD_STRING; + case (12300): + return SEND_MRAM_DUMP_FAILED_STRING; + case (12301): + return MRAM_DUMP_FAILED_STRING; + case (12302): + return MRAM_DUMP_FINISHED_STRING; + case (12401): + return INVALID_TC_FRAME_STRING; + case (12402): + return INVALID_FAR_STRING; + case (12403): + return CARRIER_LOCK_STRING; + case (12404): + return BIT_LOCK_PDEC_STRING; + case (12405): + return LOST_CARRIER_LOCK_PDEC_STRING; + case (12406): + return LOST_BIT_LOCK_PDEC_STRING; + case (12407): + return POLL_ERROR_PDEC_STRING; + case (12500): + return IMAGE_UPLOAD_FAILED_STRING; + case (12501): + return IMAGE_DOWNLOAD_FAILED_STRING; + case (12502): + return IMAGE_UPLOAD_SUCCESSFUL_STRING; + case (12503): + return IMAGE_DOWNLOAD_SUCCESSFUL_STRING; + case (12504): + return FLASH_WRITE_SUCCESSFUL_STRING; + case (12505): + return FLASH_READ_SUCCESSFUL_STRING; + case (12506): + return FLASH_READ_FAILED_STRING; + case (12507): + return FIRMWARE_UPDATE_SUCCESSFUL_STRING; + case (12508): + return FIRMWARE_UPDATE_FAILED_STRING; + case (12509): + return STR_HELPER_READING_REPLY_FAILED_STRING; + case (12510): + return STR_HELPER_COM_ERROR_STRING; + case (12511): + return STR_HELPER_NO_REPLY_STRING; + case (12512): + return STR_HELPER_DEC_ERROR_STRING; + case (12513): + return POSITION_MISMATCH_STRING; + case (12514): + return STR_HELPER_FILE_NOT_EXISTS_STRING; + case (12515): + return STR_HELPER_SENDING_PACKET_FAILED_STRING; + case (12516): + return STR_HELPER_REQUESTING_MSG_FAILED_STRING; + case (12600): + return MPSOC_FLASH_WRITE_FAILED_STRING; + case (12601): + return MPSOC_FLASH_WRITE_SUCCESSFUL_STRING; + case (12602): + return MPSOC_SENDING_COMMAND_FAILED_STRING; + case (12603): + return MPSOC_HELPER_REQUESTING_REPLY_FAILED_STRING; + case (12604): + return MPSOC_HELPER_READING_REPLY_FAILED_STRING; + case (12605): + return MPSOC_MISSING_ACK_STRING; + case (12606): + return MPSOC_MISSING_EXE_STRING; + case (12607): + return MPSOC_ACK_FAILURE_REPORT_STRING; + case (12608): + return MPSOC_EXE_FAILURE_REPORT_STRING; + case (12609): + return MPSOC_ACK_INVALID_APID_STRING; + case (12610): + return MPSOC_EXE_INVALID_APID_STRING; + case (12611): + return MPSOC_HELPER_SEQ_CNT_MISMATCH_STRING; + case (12612): + return MPSOC_TM_SIZE_ERROR_STRING; + case (12613): + return MPSOC_TM_CRC_MISSMATCH_STRING; + case (12700): + return TRANSITION_BACK_TO_OFF_STRING; + case (12701): + return NEG_V_OUT_OF_BOUNDS_STRING; + case (12702): + return U_DRO_OUT_OF_BOUNDS_STRING; + case (12703): + return I_DRO_OUT_OF_BOUNDS_STRING; + case (12704): + return U_X8_OUT_OF_BOUNDS_STRING; + case (12705): + return I_X8_OUT_OF_BOUNDS_STRING; + case (12706): + return U_TX_OUT_OF_BOUNDS_STRING; + case (12707): + return I_TX_OUT_OF_BOUNDS_STRING; + case (12708): + return U_MPA_OUT_OF_BOUNDS_STRING; + case (12709): + return I_MPA_OUT_OF_BOUNDS_STRING; + case (12710): + return U_HPA_OUT_OF_BOUNDS_STRING; + case (12711): + return I_HPA_OUT_OF_BOUNDS_STRING; + case (12800): + return TRANSITION_OTHER_SIDE_FAILED_STRING; + case (12801): + return NOT_ENOUGH_DEVICES_DUAL_MODE_STRING; + case (12802): + return POWER_STATE_MACHINE_TIMEOUT_STRING; + case (12803): + return SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING; + case (13000): + return CHILDREN_LOST_MODE_STRING; + case (13100): + return GPS_FIX_CHANGE_STRING; + case (13200): + return P60_BOOT_COUNT_STRING; + case (13201): + return BATT_MODE_STRING; + case (13202): + return BATT_MODE_CHANGED_STRING; + case (13600): + return SUPV_UPDATE_FAILED_STRING; + case (13601): + return SUPV_UPDATE_SUCCESSFUL_STRING; + case (13602): + return SUPV_CONTINUE_UPDATE_FAILED_STRING; + case (13603): + return SUPV_CONTINUE_UPDATE_SUCCESSFUL_STRING; + case (13604): + return TERMINATED_UPDATE_PROCEDURE_STRING; + case (13605): + return SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL_STRING; + case (13606): + return SUPV_EVENT_BUFFER_REQUEST_FAILED_STRING; + case (13607): + return SUPV_EVENT_BUFFER_REQUEST_TERMINATED_STRING; + case (13608): + return SUPV_MEM_CHECK_OK_STRING; + case (13609): + return SUPV_MEM_CHECK_FAIL_STRING; + case (13616): + return SUPV_SENDING_COMMAND_FAILED_STRING; + case (13617): + return SUPV_HELPER_REQUESTING_REPLY_FAILED_STRING; + case (13618): + return SUPV_HELPER_READING_REPLY_FAILED_STRING; + case (13619): + return SUPV_MISSING_ACK_STRING; + case (13620): + return SUPV_MISSING_EXE_STRING; + case (13621): + return SUPV_ACK_FAILURE_REPORT_STRING; + case (13622): + return SUPV_EXE_FAILURE_REPORT_STRING; + case (13623): + return SUPV_ACK_INVALID_APID_STRING; + case (13624): + return SUPV_EXE_INVALID_APID_STRING; + case (13625): + return ACK_RECEPTION_FAILURE_STRING; + case (13626): + return EXE_RECEPTION_FAILURE_STRING; + case (13627): + return WRITE_MEMORY_FAILED_STRING; + case (13628): + return SUPV_REPLY_SIZE_MISSMATCH_STRING; + case (13629): + return SUPV_REPLY_CRC_MISSMATCH_STRING; + case (13630): + return SUPV_UPDATE_PROGRESS_STRING; + case (13631): + return HDLC_FRAME_REMOVAL_ERROR_STRING; + case (13632): + return HDLC_CRC_ERROR_STRING; + case (13700): + return ALLOC_FAILURE_STRING; + case (13701): + return REBOOT_SW_STRING; + case (13702): + return REBOOT_MECHANISM_TRIGGERED_STRING; + case (13703): + return REBOOT_HW_STRING; + case (13704): + return NO_SD_CARD_ACTIVE_STRING; + case (13800): + return MISSING_PACKET_STRING; + case (13801): + return EXPERIMENT_TIMEDOUT_STRING; + case (13802): + return MULTI_PACKET_COMMAND_DONE_STRING; + case (13901): + return SET_CONFIGFILEVALUE_FAILED_STRING; + case (13902): + return GET_CONFIGFILEVALUE_FAILED_STRING; + case (13903): + return INSERT_CONFIGFILEVALUE_FAILED_STRING; + case (13904): + return WRITE_CONFIGFILE_FAILED_STRING; + case (13905): + return READ_CONFIGFILE_FAILED_STRING; default: return "UNKNOWN_EVENT"; } diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.h b/bsp_hosted/fsfwconfig/events/translateEvents.h index a42d9b5a..99554317 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.h +++ b/bsp_hosted/fsfwconfig/events/translateEvents.h @@ -1,8 +1,8 @@ #ifndef FSFWCONFIG_EVENTS_TRANSLATEEVENTS_H_ #define FSFWCONFIG_EVENTS_TRANSLATEEVENTS_H_ -#include +#include "fsfw/events/Event.h" -const char* translateEvents(Event event); +const char *translateEvents(Event event); #endif /* FSFWCONFIG_EVENTS_TRANSLATEEVENTS_H_ */ From bce8c71e3887d870047363a3ce45f573280dc915 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 11:33:47 +0100 Subject: [PATCH 166/244] crc check now works --- linux/devices/ploc/PlocSupvUartMan.cpp | 61 ++++++++++++++++++++++++-- linux/devices/ploc/PlocSupvUartMan.h | 3 ++ thirdparty/tas/hdlc.c | 10 +++-- thirdparty/tas/tas/crc.h | 9 ++++ thirdparty/tas/tas/hdlc.h | 2 + 5 files changed, 79 insertions(+), 6 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 4fe9a033..ff57222d 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -970,7 +970,7 @@ ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() { ReturnValue_t PlocSupvUartManager::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) { size_t encodedLen = 0; - hdlc_add_framing(sendData, sendLen, encodedSendBuf.data(), &encodedLen); + addHdlcFraming(sendData, sendLen, encodedSendBuf.data(), &encodedLen, encodedSendBuf.size()); sif::debug << "Sending TC" << std::endl; arrayprinter::print(encodedSendBuf.data(), encodedLen); size_t bytesWritten = write(serialPort, encodedSendBuf.data(), encodedLen); @@ -1051,8 +1051,8 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize, size if (encodedBuf[idx] == HDLC_END_MARKER) { if (startMarkerFound) { // Probably a packet, so decode it - int retval = hdlc_remove_framing_with_crc_check( - encodedBuf.data() + startIdx, idx + 1 - startIdx, decodedBuf.data(), &decodedLen); + int retval = removeHdlcFramingWithCrcCheck(encodedBuf.data() + startIdx, idx + 1 - startIdx, + decodedBuf.data(), &decodedLen); readSize = idx + 1; if (retval == -1 or retval == -2) { triggerEvent(HDLC_FRAME_REMOVAL_ERROR, retval); @@ -1097,3 +1097,58 @@ void PlocSupvUartManager::performUartShutdown() { } state = InternalState::GO_TO_SLEEP; } + +void PlocSupvUartManager::addHdlcFraming(const uint8_t* src, size_t slen, uint8_t* dst, + size_t* dlen, size_t maxDest) { + size_t tlen = 0; + uint16_t ii; + uint8_t bt; + + // calc crc16 + uint16_t crc16 = calc_crc16_buff_reflected(src, slen); + + dst[tlen++] = 0x7E; + for (ii = 0; ii < slen; ii++) { + bt = *src++; + hdlc_add_byte(bt, dst, &tlen); + } + + size_t dummy = 0; + // hdlc crc16 is in little endian format + SerializeAdapter::serialize(&crc16, dst + tlen, &dummy, maxDest, SerializeIF::Endianness::LITTLE); + tlen += dummy; + + dst[tlen++] = 0x7C; + *dlen = tlen; +} + +int PlocSupvUartManager::removeHdlcFramingWithCrcCheck(const uint8_t* src, size_t slen, + uint8_t* dst, size_t* dlen) { + uint16_t tlen = 0; + uint16_t ii; + uint8_t bt; + + *dlen = 0; + if (slen < 4) return -1; + if ((src[tlen] != 0x7E) && (src[slen - 1] != 0x7C)) return -2; + src++; + for (ii = 1; ii < slen - 1; ii++) { + bt = *src++; + + if (bt == 0x7D) { + bt = *src++ ^ 0x20; + ii++; + } + dst[tlen++] = bt; + } + // calc crc16 + uint16_t calcCrc = calc_crc16_buff_reflected(dst, tlen - 2); + uint16_t crc; + size_t dummy; + SerializeAdapter::deSerialize(&crc, dst + tlen - 2, &dummy, SerializeIF::Endianness::LITTLE); + if (calcCrc != crc) { + return 1; + } + *dlen = tlen - 2; + return 0; +} diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 58abf68b..bc39a477 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -15,6 +15,7 @@ #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" +#include "tas/crc.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/SdCardManager.h" @@ -259,6 +260,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF, ReturnValue_t handleRunningLongerRequest(); bool handleUartReception(); + void addHdlcFraming(const uint8_t* src, size_t slen, uint8_t* dst, size_t* dlen, size_t maxDest); + int removeHdlcFramingWithCrcCheck(const uint8_t* src, size_t slen, uint8_t* dst, size_t* dlen); ReturnValue_t encodeAndSendPacket(const uint8_t* sendData, size_t sendLen); void executeFullCheckMemoryCommand(); diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 428a28da..296c8988 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -14,9 +14,9 @@ #include -static void hdlc_add_byte(uint8_t ch, uint8_t *buff, uint16_t *pos) +void hdlc_add_byte(uint8_t ch, uint8_t *buff, size_t *pos) { - uint16_t templen = *pos; + size_t templen = *pos; if ((ch == 0x7E) || (ch == 0x7D) || @@ -32,7 +32,7 @@ static void hdlc_add_byte(uint8_t ch, uint8_t *buff, uint16_t *pos) void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) { - uint16_t tlen = 0; + size_t tlen = 0; uint16_t ii; uint16_t crc16; uint8_t bt; @@ -48,6 +48,9 @@ void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dle } // hdlc crc16 is in little endian format + // WARNING: This is not portable code! Bytes need to be swapped on a big + // endian system + // TODO: Fix hdlc_add_byte((uint8_t) (crc16 & 0xFF), dst, &tlen); hdlc_add_byte((uint8_t) ((crc16 >> 8) & 0xFF), dst, &tlen); @@ -75,6 +78,7 @@ int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t dst[tlen++] = bt; } // calc crc16 + // TODO: Warning: This does not work because the CRC16 is little endian if(calc_crc16_buff_reflected( dst, tlen ) != 0) { return 1; } diff --git a/thirdparty/tas/tas/crc.h b/thirdparty/tas/tas/crc.h index a9735402..0a45653c 100644 --- a/thirdparty/tas/tas/crc.h +++ b/thirdparty/tas/tas/crc.h @@ -10,6 +10,10 @@ #ifndef TAS_D_C_CRC_H #define TAS_D_C_CRC_H +#ifdef __cplusplus +extern "C" { +#endif + #include #include @@ -104,4 +108,9 @@ void calc_crc16_byte_reflected(uint16_t *crc16, uint8_t bt); * \return CRC result */ uint16_t calc_crc16_buff_reflected(const uint8_t *data, uint16_t len); + +#ifdef __cplusplus +} +#endif + #endif diff --git a/thirdparty/tas/tas/hdlc.h b/thirdparty/tas/tas/hdlc.h index c4179d11..b137fc4c 100644 --- a/thirdparty/tas/tas/hdlc.h +++ b/thirdparty/tas/tas/hdlc.h @@ -26,6 +26,8 @@ extern "C" { #define HDLC_END_BYTE (0x7Cu) #define HDLC_ESCAPE_CHAR (0x20u) +void hdlc_add_byte(uint8_t ch, uint8_t *buff, size_t *pos); + void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); /** From bb6622883789f2a52035ba84b9909cd4027fcd49 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 11:38:45 +0100 Subject: [PATCH 167/244] re-test the simpler method --- linux/devices/ploc/PlocSupvUartMan.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index ff57222d..149a553a 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1149,6 +1149,9 @@ int PlocSupvUartManager::removeHdlcFramingWithCrcCheck(const uint8_t* src, size_ if (calcCrc != crc) { return 1; } + // if(calc_crc16_buff_reflected(dst, tlen) != 0) { + // return 1; + // } *dlen = tlen - 2; return 0; } From 53535b6023c5ff7b201959dee94fe48d8c7631da Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 11:45:09 +0100 Subject: [PATCH 168/244] fix bugs in SW --- thirdparty/tas/hdlc.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 296c8988..895bad7a 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -67,6 +67,7 @@ int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t *dlen = 0; if (slen < 4) return -1; if ((src[tlen] != 0x7E) && (src[slen-1] != 0x7C)) return -2; + src++; for (ii = 1; ii < slen-1; ii++) { bt = *src++; @@ -74,6 +75,7 @@ int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t if (bt == 0x7D) { bt = *src++ ^ 0x20; + ii++; } dst[tlen++] = bt; } From 72f8e359bbf8c79b71cefde8f19d70f89703a24d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 13:24:39 +0100 Subject: [PATCH 169/244] CRC failure --- .../PlocSupervisorDefinitions.h | 17 ++++++++++++----- linux/devices/ploc/PlocSupervisorHandler.cpp | 2 +- linux/devices/ploc/PlocSupvUartMan.cpp | 7 ++++--- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index c74e81ff..d4ec1caa 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -310,7 +310,9 @@ enum class TmtcManStatusCode : uint32_t { BAD_SP_HEADER = 0x608 }; -static const uint16_t APID_MASK = 0x3FF; +static constexpr uint16_t APID_MASK_TC = 0x200; +static constexpr uint16_t APID_MASK_TM = 0x400; +static constexpr uint16_t APID_MODULE_MASK = 0x7F; static const uint16_t SEQUENCE_COUNT_MASK = 0xFFF; static const uint8_t HK_SET_ENTRIES = 13; @@ -485,7 +487,8 @@ class TcBase : public ploc::SpTcBase { : TcBase(params, apid, service, payloadLen, DEFAULT_SEQ_COUNT) {} TcBase(TcParams params, uint16_t apid, uint8_t serviceId, size_t payloadLen, uint16_t seqCount) - : ploc::SpTcBase(params, apid, fullSpDataLenFromPayloadLen(payloadLen), seqCount) { + : ploc::SpTcBase(params, apid | APID_MASK_TC, fullSpDataLenFromPayloadLen(payloadLen), + seqCount) { setup(serviceId); } @@ -496,6 +499,8 @@ class TcBase : public ploc::SpTcBase { payloadStart[supv::PAYLOAD_OFFSET] = id; } + uint16_t getModuleApid() const { return getApid() & APID_MODULE_MASK; } + uint8_t getServiceId() const { return payloadStart[supv::PAYLOAD_OFFSET]; } static size_t fullSpDataLenFromPayloadLen(size_t payloadLen) { @@ -541,6 +546,8 @@ class TmBase : public ploc::SpTmReader { uint8_t getServiceId() const { return getPacketData()[TIMESTAMP_LEN]; } + uint16_t getModuleApid() const { return getApid() & APID_MODULE_MASK; } + const uint8_t* getPayloadStart() const { return getPacketData() + SECONDARY_HEADER_LEN; } size_t getPayloadLen() const { if (getFullPacketLen() > SECONDARY_HEADER_LEN + ccsds::HEADER_LEN) { @@ -1430,7 +1437,7 @@ class VerificationReport { break; } } - } else if (statusCode < 0x200 and statusCode > 0x100) { + } else if (statusCode < 0x200 and statusCode >= 0x100) { BootManStatusCode code = static_cast(statusCode); switch (code) { case BootManStatusCode::NOTHING_TODO: { @@ -1474,7 +1481,7 @@ class VerificationReport { break; } } - } else if (statusCode < 0x300 and statusCode > 0x200) { + } else if (statusCode < 0x300 and statusCode >= 0x200) { MemManStatusCode code = static_cast(statusCode); switch (code) { case MemManStatusCode::SP_NOT_AVAILABLE: { @@ -1522,7 +1529,7 @@ class VerificationReport { break; } } - } else if (statusCode < 0x400 and statusCode > 0x300) { + } else if (statusCode < 0x400 and statusCode >= 0x300) { PowerManStatusCode code = static_cast(statusCode); switch (code) { case PowerManStatusCode::PG_LOW: { diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index feb67265..8566b5b9 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -588,7 +588,7 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r tmReader.setData(start, remainingSize); sif::debug << "PlocSupervisorHandler::scanForReply: Received Packet" << std::endl; arrayprinter::print(start, remainingSize); - uint16_t apid = tmReader.getApid(); //(*(start) << 8 | *(start + 1)) & APID_MASK; + uint16_t apid = tmReader.getModuleApid(); switch (apid) { case (Apid::TMTC_MAN): { diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 149a553a..2ec117c5 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -583,7 +583,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( if (result != returnvalue::OK) { continue; } - if (tmReader.getApid() == Apid::TMTC_MAN) { + if (tmReader.getModuleApid() == Apid::TMTC_MAN) { uint8_t serviceId = tmReader.getServiceId(); int retval = 0; if (not ackReceived) { @@ -769,7 +769,7 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { continue; } packetWasHandled = false; - if (tmReader.getApid() == Apid::TMTC_MAN) { + if (tmReader.getModuleApid() == Apid::TMTC_MAN) { uint8_t serviceId = tmReader.getServiceId(); int retval = 0; if (not ackReceived) { @@ -790,7 +790,7 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { return returnvalue::FAILED; } } - } else if (tmReader.getApid() == Apid::MEM_MAN) { + } else if (tmReader.getModuleApid() == Apid::MEM_MAN) { if (ackReceived) { supv::UpdateStatusReport report(tmReader); result = report.parse(); @@ -1149,6 +1149,7 @@ int PlocSupvUartManager::removeHdlcFramingWithCrcCheck(const uint8_t* src, size_ if (calcCrc != crc) { return 1; } + // This does not work because the CRC is little endian // if(calc_crc16_buff_reflected(dst, tlen) != 0) { // return 1; // } From ffc4a3456b263a2a4ceec3545c8d06633b619e8b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 13:26:03 +0100 Subject: [PATCH 170/244] some better names --- .../devices/devicedefinitions/PlocSupervisorDefinitions.h | 2 +- linux/devices/ploc/PlocSupvUartMan.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index d4ec1caa..c3223833 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -1339,7 +1339,7 @@ class VerificationReport { /** * @brief Gets the APID of command which caused the transmission of this verification report. */ - uint8_t getRefApid() const { return refApid; } + uint8_t getRefModuleApid() const { return refApid; } uint8_t getRefServiceId() const { return refServiceId; } diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 2ec117c5..b3ac004c 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -626,14 +626,14 @@ int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, uint8_t serviceId, triggerEvent(ACK_RECEPTION_FAILURE); return -1; } - if (ackReport.getRefApid() == tc.getApid() and + if (ackReport.getRefModuleApid() == tc.getModuleApid() and ackReport.getRefServiceId() == tc.getServiceId()) { if (serviceId == static_cast(supv::tm::TmtcId::ACK)) { return 1; } else if (serviceId == static_cast(supv::tm::TmtcId::NAK)) { ackReport.printStatusInformation(); triggerEvent(SUPV_ACK_FAILURE_REPORT, - buildApidServiceParam1(ackReport.getRefApid(), ackReport.getRefServiceId()), + buildApidServiceParam1(ackReport.getRefModuleApid(), ackReport.getRefServiceId()), ackReport.getStatusCode()); return -1; } @@ -657,14 +657,14 @@ int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, uint8_t service triggerEvent(EXE_RECEPTION_FAILURE); return -1; } - if (exeReport.getRefApid() == tc.getApid() and + if (exeReport.getRefModuleApid() == tc.getApid() and exeReport.getRefServiceId() == tc.getServiceId()) { if (serviceId == static_cast(supv::tm::TmtcId::EXEC_ACK)) { return 1; } else if (serviceId == static_cast(supv::tm::TmtcId::EXEC_NAK)) { exeReport.printStatusInformation(); triggerEvent(SUPV_EXE_FAILURE_REPORT, - buildApidServiceParam1(exeReport.getRefApid(), exeReport.getRefServiceId()), + buildApidServiceParam1(exeReport.getRefModuleApid(), exeReport.getRefServiceId()), exeReport.getStatusCode()); return -1; } From 00dd2e99a82dcd9f378fc52943c23348f124ab7b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 13:39:59 +0100 Subject: [PATCH 171/244] getting there --- .../PlocSupervisorDefinitions.h | 52 +++++++++++++++++-- linux/devices/ploc/PlocSupervisorHandler.cpp | 4 +- linux/devices/ploc/PlocSupvUartMan.cpp | 14 ++--- 3 files changed, 58 insertions(+), 12 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index c3223833..312f5c14 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -1296,7 +1296,7 @@ class VerificationReport { if (not readerBase.crcIsOk()) { return result::CRC_FAILURE; } - if (readerBase.getApid() != Apid::TMTC_MAN) { + if (readerBase.getModuleApid() != Apid::TMTC_MAN) { return result::INVALID_APID; } if (readerBase.getBufSize() < MIN_TMTC_LEN + PAYLOAD_LEN or @@ -1477,7 +1477,7 @@ class VerificationReport { break; } default: { - codeHandled = true; + codeHandled = false; break; } } @@ -1525,7 +1525,7 @@ class VerificationReport { break; } default: { - codeHandled = true; + codeHandled = false; break; } } @@ -1565,7 +1565,51 @@ class VerificationReport { break; } default: { - codeHandled = true; + codeHandled = false; + break; + } + } + } else if (statusCode >= 0x600) { + TmtcManStatusCode code = static_cast(statusCode); + switch (code) { + case TmtcManStatusCode::BUF_FULL: { + sif::warning << prefix << "TMTC MAN: Buffer full" << std::endl; + break; + } + case TmtcManStatusCode::WRONG_APID: { + sif::warning << prefix << "TMTC MAN: Wrong APID" << std::endl; + break; + } + case TmtcManStatusCode::WRONG_SERVICE_ID: { + sif::warning << prefix << "TMTC MAN: Wrong Service ID" << std::endl; + break; + } + case TmtcManStatusCode::TC_DELIVERY_ACCEPTED: { + sif::warning << prefix << "TMTC MAN: TC accepted" << std::endl; + break; + } + case TmtcManStatusCode::TC_DELIVERY_REJECTED: { + sif::warning << prefix << "TMTC MAN: TC rejected" << std::endl; + break; + } + case TmtcManStatusCode::TC_PACKET_LEN_INCORRECT: { + sif::warning << prefix << "TMTC MAN: TC packet lenght incorrect" << std::endl; + break; + } + case TmtcManStatusCode::BAD_CRC: { + sif::warning << prefix << "TMTC MAN: Bad CRC" << std::endl; + break; + } + case TmtcManStatusCode::BAD_DEST: { + sif::warning << prefix << "TMTC MAN: Bad destination" << std::endl; + break; + } + case TmtcManStatusCode::BAD_SP_HEADER: { + sif::warning << prefix << "TMTC MAN: Bad SP header" << std::endl; + break; + } + default: { + codeHandled = false; break; } } diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 8566b5b9..f7dd4fcd 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -839,7 +839,7 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { using namespace supv; ReturnValue_t result = returnvalue::OK; - if (tmReader.checkCrc() != returnvalue::OK) { + if (not tmReader.verifyCrc()) { sif::error << "PlocSupervisorHandler::handleAckReport: CRC failure" << std::endl; nextReplyId = supv::NONE; replyRawReplyIfnotWiretapped(data, supv::SIZE_ACK_REPORT); @@ -878,7 +878,7 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data) using namespace supv; ReturnValue_t result = returnvalue::OK; - if (tmReader.checkCrc() != OK) { + if (not tmReader.verifyCrc()) { nextReplyId = supv::NONE; return result::CRC_FAILURE; } diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index b3ac004c..fc03cbd7 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -632,9 +632,10 @@ int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, uint8_t serviceId, return 1; } else if (serviceId == static_cast(supv::tm::TmtcId::NAK)) { ackReport.printStatusInformation(); - triggerEvent(SUPV_ACK_FAILURE_REPORT, - buildApidServiceParam1(ackReport.getRefModuleApid(), ackReport.getRefServiceId()), - ackReport.getStatusCode()); + triggerEvent( + SUPV_ACK_FAILURE_REPORT, + buildApidServiceParam1(ackReport.getRefModuleApid(), ackReport.getRefServiceId()), + ackReport.getStatusCode()); return -1; } // Should never happen @@ -663,9 +664,10 @@ int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, uint8_t service return 1; } else if (serviceId == static_cast(supv::tm::TmtcId::EXEC_NAK)) { exeReport.printStatusInformation(); - triggerEvent(SUPV_EXE_FAILURE_REPORT, - buildApidServiceParam1(exeReport.getRefModuleApid(), exeReport.getRefServiceId()), - exeReport.getStatusCode()); + triggerEvent( + SUPV_EXE_FAILURE_REPORT, + buildApidServiceParam1(exeReport.getRefModuleApid(), exeReport.getRefServiceId()), + exeReport.getStatusCode()); return -1; } // Should never happen From 9bf6ac7a9a9f6c5b3853e1caaac506441905e1f9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 13:48:40 +0100 Subject: [PATCH 172/244] clean up printouts a bit --- linux/devices/ploc/PlocSupervisorHandler.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index f7dd4fcd..5f4092f8 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -863,6 +863,7 @@ ReturnValue_t PlocSupervisorHandler::handleAckReport(const uint8_t* data) { if (commandId != DeviceHandlerIF::NO_COMMAND_ID) { triggerEvent(SUPV_ACK_FAILURE, commandId, static_cast(ack.getStatusCode())); } + ack.printStatusInformation(); printAckFailureInfo(ack.getStatusCode(), commandId); sendFailureReport(supv::ACK_REPORT, result::RECEIVED_ACK_FAILURE); disableAllReplies(); @@ -1927,12 +1928,11 @@ void PlocSupervisorHandler::handleBadApidServiceCombination(Event event, unsigne } void PlocSupervisorHandler::printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId) { - sif::warning << "PlocSupervisorHandler: Received Ack failure report with status code: 0x" - << std::hex << statusCode << std::endl; switch (commandId) { case (supv::SET_TIME_REF): { - sif::info << "PlocSupervisoHandler: Setting time failed. Make sure the OBC has a valid time" - << std::endl; + sif::warning + << "PlocSupervisoHandler: Setting time failed. Make sure the OBC has a valid time" + << std::endl; break; } default: From cdadb024202ce4e6e20716b775b5cbd8046826f7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 13:58:40 +0100 Subject: [PATCH 173/244] mask corrections ,set sec header flag --- .../devices/devicedefinitions/PlocSupervisorDefinitions.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 312f5c14..ac89c8d4 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -310,8 +310,8 @@ enum class TmtcManStatusCode : uint32_t { BAD_SP_HEADER = 0x608 }; -static constexpr uint16_t APID_MASK_TC = 0x200; -static constexpr uint16_t APID_MASK_TM = 0x400; +static constexpr uint16_t APID_MASK_TC = 0x80; +static constexpr uint16_t APID_MASK_TM = 0x200; static constexpr uint16_t APID_MODULE_MASK = 0x7F; static const uint16_t SEQUENCE_COUNT_MASK = 0xFFF; @@ -520,7 +520,8 @@ class TcBase : public ploc::SpTcBase { } std::memset(spParams.buf + ccsds::HEADER_LEN, 0, TIMESTAMP_LEN); payloadStart = spParams.buf + ccsds::HEADER_LEN + SECONDARY_HEADER_LEN; - payloadStart[supv::PAYLOAD_OFFSET] = serviceId; + spParams.buf[ccsds::HEADER_LEN + SECONDARY_HEADER_LEN - 1] = serviceId; + spParams.creator.setSecHeaderFlag(); return returnvalue::OK; } }; From 114d16224e5b910f8e3801830dd07e3b5e4374e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 15:22:15 +0100 Subject: [PATCH 174/244] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index c5f91926..160ff799 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit c5f91926c95a8db0c7921176aa3f62cc2bebef47 +Subproject commit 160ff799ace61e24708dcf1fdeaf5fafdf23a4ca From f08709ef29e6c927f93672215b84dee6a35b19bc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 15:24:26 +0100 Subject: [PATCH 175/244] switch off debugging spam --- linux/devices/ploc/PlocSupvUartMan.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index fc03cbd7..c423944f 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -100,7 +100,7 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { lock->unlockMutex(); semaphore->acquire(); while (true) { - sif::debug << "SUPV UART MAN: Running.." << std::endl; + // sif::debug << "SUPV UART MAN: Running.." << std::endl; putTaskToSleep = handleUartReception(); if (putTaskToSleep) { performUartShutdown(); From 9a3fd51337a2e83da268a8921aa9fad9aa370f57 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 15:48:44 +0100 Subject: [PATCH 176/244] tweak do off activity --- linux/devices/ploc/PlocSupervisorHandler.cpp | 4 ++-- linux/devices/ploc/PlocSupervisorHandler.h | 2 +- tmtc | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 5f4092f8..599ef54d 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -1230,9 +1230,9 @@ ReturnValue_t PlocSupervisorHandler::doSendReadHook() { return returnvalue::OK; } -void PlocSupervisorHandler::doOffActivity() { startupState = StartupState::OFF; } +void PlocSupervisorHandler::doOffActivity() {} -void PlocSupervisorHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, +void PlocSupervisorHandler::handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) { ReturnValue_t result = returnvalue::OK; diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 60970f33..557639e1 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -237,7 +237,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { * @param dataSize Size of telemetry in bytes. * @param replyId Id of the reply. This will be added to the ActionMessage. */ - void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); + void handleDeviceTm(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId); /** * @brief This function prepares a space packet which does not transport any data in the diff --git a/tmtc b/tmtc index d5813e1a..f730ff41 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit d5813e1a422beae7d0cb092a885e0956e9f1ddc1 +Subproject commit f730ff4180b02f68122f853800a6247a92ac722e From 7c821e33f04354a57e2fce64a37f6a7eaefb59cd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 15:59:31 +0100 Subject: [PATCH 177/244] somethings wrong --- linux/devices/ploc/PlocSupvUartMan.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index c423944f..f7d15317 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -907,12 +907,12 @@ ReturnValue_t PlocSupvUartManager::sendMessage(CookieIF* cookie, const uint8_t* if (sendData == nullptr or sendLen == 0) { return FAILED; } - lock->lockMutex(); - if (state == InternalState::SLEEPING or state == InternalState::DEDICATED_REQUEST) { - lock->unlockMutex(); - return FAILED; + { + MutexGuard mg(lock); + if (state == InternalState::SLEEPING or state == InternalState::DEDICATED_REQUEST) { + return FAILED; + } } - lock->unlockMutex(); return encodeAndSendPacket(sendData, sendLen); } @@ -977,8 +977,9 @@ ReturnValue_t PlocSupvUartManager::encodeAndSendPacket(const uint8_t* sendData, arrayprinter::print(encodedSendBuf.data(), encodedLen); size_t bytesWritten = write(serialPort, encodedSendBuf.data(), encodedLen); if (bytesWritten != encodedLen) { - sif::warning << "ScexUartReader::sendMessage: Sending ping command to solar experiment failed" - << std::endl; + sif::warning + << "PlocSupvUartManager::sendMessage: Sending ping command to solar experiment failed" + << std::endl; return FAILED; } return returnvalue::OK; From 3ab27f63f2dbc24cfd218a286d4efd89f24d28ef Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 16:20:13 +0100 Subject: [PATCH 178/244] fix scheduling order --- linux/scheduling.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/linux/scheduling.cpp b/linux/scheduling.cpp index 9a4c1a93..1300266f 100644 --- a/linux/scheduling.cpp +++ b/linux/scheduling.cpp @@ -50,17 +50,17 @@ void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHa void scheduling::addMpsocSupvHandlers(PeriodicTaskIF* plTask) { #if OBSW_ADD_PLOC_SUPERVISOR == 1 plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::PERFORM_OPERATION); - plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_READ); - plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_READ); plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_WRITE); plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_WRITE); + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_READ); + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_READ); #endif #if OBSW_ADD_PLOC_MPSOC == 1 plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::PERFORM_OPERATION); - plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_READ); - plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ); plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_WRITE); plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_WRITE); + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_READ); + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ); #endif } From ae11f73e247a8efb2b1e1aada365ae172f2e3831 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 16:43:54 +0100 Subject: [PATCH 179/244] disable debug mode --- linux/devices/ploc/PlocSupvUartMan.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index bc39a477..9f6d4fe1 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -252,7 +252,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, std::array tmBuf{}; - bool debugMode = true; + bool debugMode = false; bool timestamping = true; // Remembers APID to know at which command a procedure failed From 2c00119b47e15dc7fa2d8cc9651d04f16dc0a767 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 17:28:41 +0100 Subject: [PATCH 180/244] set SP max size --- linux/devices/ploc/PlocSupvUartMan.cpp | 1 + tmtc | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index f7d15317..da9d4b3d 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -34,6 +34,7 @@ PlocSupvUartManager::PlocSupvUartManager(object_id_t objectId) decodedRingBuf(1200 * MAX_STORED_DECODED_PACKETS, true), ipcRingBuf(1200 * MAX_STORED_DECODED_PACKETS, true) { resetSpParams(); + spParams.maxSize = cmdBuf.size(); semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); semaphore->acquire(); lock = MutexFactory::instance()->createMutex(); diff --git a/tmtc b/tmtc index f730ff41..066adee1 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f730ff4180b02f68122f853800a6247a92ac722e +Subproject commit 066adee13a980f0a297ab401a10abc157a7d921d From 0857a480e517367897d7a7df446b1b1fc9efc431 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 17:39:16 +0100 Subject: [PATCH 181/244] zero init some values --- linux/devices/ploc/PlocSupvUartMan.h | 6 +++--- tmtc | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 9f6d4fe1..ce24e721 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -190,10 +190,10 @@ class PlocSupvUartManager : public DeviceCommunicationIF, // Absolute name of file containing update data std::string file; // Length of full file - size_t fullFileSize; + size_t fullFileSize = 0; // Size of update - uint32_t length; - uint32_t crc; + uint32_t length = 0; + uint32_t crc = 0; bool crcShouldBeChecked = true; size_t bytesWritten; uint32_t packetNum; diff --git a/tmtc b/tmtc index 066adee1..9cd4846d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 066adee13a980f0a297ab401a10abc157a7d921d +Subproject commit 9cd4846d396373325fc5e03a367fed236dfc16d7 From c248cdb876c7916619864de366accec534be4776 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 17:46:06 +0100 Subject: [PATCH 182/244] fix for error handling longer request --- linux/devices/ploc/PlocSupvUartMan.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index da9d4b3d..ab4aea5e 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -117,10 +117,9 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { break; } case InternalState::DEDICATED_REQUEST: { - if (handleRunningLongerRequest() == REQUEST_DONE) { - MutexGuard mg(lock); - state = InternalState::DEFAULT; - } + handleRunningLongerRequest(); + MutexGuard mg(lock); + state = InternalState::DEFAULT; break; } case InternalState::DEFAULT: { From 02eff4a26f0c1cdf5a530b1ebb1f02269589577d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Nov 2022 14:18:45 +0100 Subject: [PATCH 183/244] some more bugfixes --- linux/devices/ploc/PlocSupervisorHandler.cpp | 66 +++++++++++--------- linux/devices/ploc/PlocSupvUartMan.cpp | 1 - thirdparty/tas/hdlc.c | 3 +- 3 files changed, 36 insertions(+), 34 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 599ef54d..e596d768 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -977,7 +977,7 @@ ReturnValue_t PlocSupervisorHandler::handleHkReport(const uint8_t* data) { ReturnValue_t PlocSupervisorHandler::handleBootStatusReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; - result = verifyPacket(data, supv::SIZE_BOOT_STATUS_REPORT); + result = verifyPacket(data, tmReader.getFullPacketLen()); if (result == result::CRC_FAILURE) { sif::error << "PlocSupervisorHandler::handleBootStatusReport: Boot status report has invalid" @@ -986,28 +986,31 @@ ReturnValue_t PlocSupervisorHandler::handleBootStatusReport(const uint8_t* data) return result; } - uint16_t offset = supv::PAYLOAD_OFFSET; - bootStatusReport.socState = *(data + offset); + const uint8_t* payloadStart = tmReader.getPayloadStart(); + uint16_t offset = 0; + bootStatusReport.socState = payloadStart[0]; offset += 1; - bootStatusReport.powerCycles = *(data + offset); + bootStatusReport.powerCycles = payloadStart[1]; offset += 1; - bootStatusReport.bootAfterMs = *(data + offset) << 24 | *(data + offset + 1) << 16 | - *(data + offset + 2) << 8 | *(data + offset + 3); + bootStatusReport.bootAfterMs = *(payloadStart + offset) << 24 | + *(payloadStart + offset + 1) << 16 | + *(payloadStart + offset + 2) << 8 | *(payloadStart + offset + 3); offset += 4; - bootStatusReport.bootTimeoutMs = *(data + offset) << 24 | *(data + offset + 1) << 16 | - *(data + offset + 2) << 8 | *(data + offset + 3); + bootStatusReport.bootTimeoutMs = *(payloadStart + offset) << 24 | + *(payloadStart + offset + 1) << 16 | + *(payloadStart + offset + 2) << 8 | *(payloadStart + offset + 3); offset += 4; - bootStatusReport.activeNvm = *(data + offset); + bootStatusReport.activeNvm = *(payloadStart + offset); offset += 1; - bootStatusReport.bp0State = *(data + offset); + bootStatusReport.bp0State = *(payloadStart + offset); offset += 1; - bootStatusReport.bp1State = *(data + offset); + bootStatusReport.bp1State = *(payloadStart + offset); offset += 1; - bootStatusReport.bp2State = *(data + offset); + bootStatusReport.bp2State = *(payloadStart + offset); offset += 1; - bootStatusReport.bootState = *(data + offset); + bootStatusReport.bootState = *(payloadStart + offset); offset += 1; - bootStatusReport.bootCycles = *(data + offset); + bootStatusReport.bootCycles = *(payloadStart + offset); nextReplyId = supv::EXE_REPORT; bootStatusReport.setValidity(true, true); @@ -1043,7 +1046,7 @@ ReturnValue_t PlocSupervisorHandler::handleBootStatusReport(const uint8_t* data) ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; - result = verifyPacket(data, supv::SIZE_LATCHUP_STATUS_REPORT); + result = verifyPacket(data, tmReader.getFullPacketLen()); if (result == result::CRC_FAILURE) { sif::error << "PlocSupervisorHandler::handleLatchupStatusReport: Latchup status report has " @@ -1051,38 +1054,39 @@ ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* da return result; } - uint16_t offset = supv::PAYLOAD_OFFSET; - latchupStatusReport.id = *(data + offset); + const uint8_t* payloadData = tmReader.getPayloadStart(); + uint16_t offset = 0; + latchupStatusReport.id = *(payloadData + offset); offset += 1; - latchupStatusReport.cnt0 = *(data + offset) << 8 | *(data + offset + 1); + latchupStatusReport.cnt0 = *(payloadData + offset) << 8 | *(payloadData + offset + 1); offset += 2; - latchupStatusReport.cnt1 = *(data + offset) << 8 | *(data + offset + 1); + latchupStatusReport.cnt1 = *(payloadData + offset) << 8 | *(payloadData + offset + 1); offset += 2; - latchupStatusReport.cnt2 = *(data + offset) << 8 | *(data + offset + 1); + latchupStatusReport.cnt2 = *(payloadData + offset) << 8 | *(payloadData + offset + 1); offset += 2; - latchupStatusReport.cnt3 = *(data + offset) << 8 | *(data + offset + 1); + latchupStatusReport.cnt3 = *(payloadData + offset) << 8 | *(payloadData + offset + 1); offset += 2; - latchupStatusReport.cnt4 = *(data + offset) << 8 | *(data + offset + 1); + latchupStatusReport.cnt4 = *(payloadData + offset) << 8 | *(payloadData + offset + 1); offset += 2; - latchupStatusReport.cnt5 = *(data + offset) << 8 | *(data + offset + 1); + latchupStatusReport.cnt5 = *(payloadData + offset) << 8 | *(payloadData + offset + 1); offset += 2; - latchupStatusReport.cnt6 = *(data + offset) << 8 | *(data + offset + 1); + latchupStatusReport.cnt6 = *(payloadData + offset) << 8 | *(data + offset + 1); offset += 2; - uint16_t msec = *(data + offset) << 8 | *(data + offset + 1); + uint16_t msec = *(payloadData + offset) << 8 | *(payloadData + offset + 1); latchupStatusReport.isSet = msec >> supv::LatchupStatusReport::IS_SET_BIT_POS; latchupStatusReport.timeMsec = msec & (~(1 << latchupStatusReport.IS_SET_BIT_POS)); offset += 2; - latchupStatusReport.timeSec = *(data + offset); + latchupStatusReport.timeSec = *(payloadData + offset); offset += 1; - latchupStatusReport.timeMin = *(data + offset); + latchupStatusReport.timeMin = *(payloadData + offset); offset += 1; - latchupStatusReport.timeHour = *(data + offset); + latchupStatusReport.timeHour = *(payloadData + offset); offset += 1; - latchupStatusReport.timeDay = *(data + offset); + latchupStatusReport.timeDay = *(payloadData + offset); offset += 1; - latchupStatusReport.timeMon = *(data + offset); + latchupStatusReport.timeMon = *(payloadData + offset); offset += 1; - latchupStatusReport.timeYear = *(data + offset); + latchupStatusReport.timeYear = *(payloadData + offset); nextReplyId = supv::EXE_REPORT; diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index ab4aea5e..8938b5d3 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1152,7 +1152,6 @@ int PlocSupvUartManager::removeHdlcFramingWithCrcCheck(const uint8_t* src, size_ if (calcCrc != crc) { return 1; } - // This does not work because the CRC is little endian // if(calc_crc16_buff_reflected(dst, tlen) != 0) { // return 1; // } diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 895bad7a..92f9817f 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -80,8 +80,7 @@ int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t dst[tlen++] = bt; } // calc crc16 - // TODO: Warning: This does not work because the CRC16 is little endian - if(calc_crc16_buff_reflected( dst, tlen ) != 0) { + if(calc_crc16_buff_reflected( dst, tlen ) != 0x0f47) { return 1; } *dlen = tlen - 2; From 2a00f5af47d4c3f71ba3b0986d9fc82d413fd1a1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Nov 2022 14:20:57 +0100 Subject: [PATCH 184/244] bump deps --- fsfw | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fsfw b/fsfw index c5f91926..160ff799 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit c5f91926c95a8db0c7921176aa3f62cc2bebef47 +Subproject commit 160ff799ace61e24708dcf1fdeaf5fafdf23a4ca diff --git a/tmtc b/tmtc index 66a1362e..9cd4846d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 66a1362e7e977e427a66fd3176a9e7b6bc1b5998 +Subproject commit 9cd4846d396373325fc5e03a367fed236dfc16d7 From 2bbe98a403957f627ea340aa1424d47547a2aeaa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Nov 2022 14:22:00 +0100 Subject: [PATCH 185/244] bump version and changelog --- CHANGELOG.md | 2 ++ CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index acef9e92..7016c40f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,8 @@ list yields a list of all related PRs for each release. # [unreleased] +# [v1.16.0] 18.11.2022 + - It is now possible to compile Linux components for the hosted build conditionally PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/322 - ACS Subsystem. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/231 diff --git a/CMakeLists.txt b/CMakeLists.txt index 97b7601c..64716301 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR_IF_GIT_FAILS 1) -set(OBSW_VERSION_MINOR_IF_GIT_FAILS 15) +set(OBSW_VERSION_MINOR_IF_GIT_FAILS 16) set(OBSW_VERSION_REVISION_IF_GIT_FAILS 0) # set(CMAKE_VERBOSE TRUE) From 28c662e6a7075c798223a53b305a78deb09ab675 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Nov 2022 14:26:40 +0100 Subject: [PATCH 186/244] update changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7016c40f..6dd1f9da 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,11 @@ list yields a list of all related PRs for each release. # [unreleased] +## Added + +- PLOC Supervisor Update: Update SW to use newest PLOC SUPV version by TAS + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/316 + # [v1.16.0] 18.11.2022 - It is now possible to compile Linux components for the hosted build conditionally From f545928adcbf2a93634059260e5bd47a2a1cf863 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Nov 2022 16:13:55 +0100 Subject: [PATCH 187/244] some bugfixes --- linux/devices/ploc/PlocSupervisorHandler.cpp | 33 +++++++------------- linux/devices/ploc/PlocSupvUartMan.cpp | 11 ++++--- 2 files changed, 19 insertions(+), 25 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index e596d768..3293c652 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -136,28 +136,19 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, void PlocSupervisorHandler::doStartUp() { if (setTimeDuringStartup) { - switch (startupState) { - case StartupState::OFF: { - bootTimeout.resetTimer(); - uartManager.start(); - startupState = StartupState::BOOTING; - break; + if (startupState == StartupState::OFF) { + bootTimeout.resetTimer(); + uartManager.start(); + startupState = StartupState::BOOTING; + } + if (startupState == StartupState::BOOTING) { + if (bootTimeout.hasTimedOut()) { + uartIsolatorSwitch.pullHigh(); + startupState = StartupState::SET_TIME; } - case StartupState::BOOTING: { - if (bootTimeout.hasTimedOut()) { - uartIsolatorSwitch.pullHigh(); - startupState = StartupState::SET_TIME; - } - break; - } - case StartupState::SET_TIME_EXECUTING: - break; - case StartupState::ON: { - setMode(_MODE_TO_ON); - break; - } - default: - break; + } + if (startupState == StartupState::ON) { + setMode(_MODE_TO_ON); } } else { uartIsolatorSwitch.pullHigh(); diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 8938b5d3..8bc2019d 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -571,9 +571,9 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( bool ackReceived = false; bool packetWasHandled = false; while (true) { - while (result != NO_PACKET_FOUND) { + do { result = tryHdlcParsing(); - } + } while (result != NO_PACKET_FOUND); if (not decodedQueue.empty()) { size_t packetLen = 0; decodedQueue.retrieve(&packetLen); @@ -589,6 +589,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( if (not ackReceived) { retval = handleAckReception(packet, serviceId, packetLen); if (retval == 1) { + sif::debug << "ACK received" << std::endl; ackReceived = true; packetWasHandled = true; } else if (retval == -1) { @@ -597,6 +598,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( } else { retval = handleExeAckReception(packet, serviceId, packetLen); if (retval == 1) { + sif::debug << "EXE ACK received" << std::endl; break; } else if (retval == -1) { return returnvalue::FAILED; @@ -758,9 +760,10 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { bool packetWasHandled = false; bool exeReceived = false; while (true) { - while (result != NO_PACKET_FOUND) { + do { result = tryHdlcParsing(); - } + + } while (result != NO_PACKET_FOUND); if (not decodedQueue.empty()) { size_t packetLen = 0; decodedQueue.retrieve(&packetLen); From 98fcf06e31c66f5c6ce482394283d659d20912db Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Nov 2022 17:25:52 +0100 Subject: [PATCH 188/244] various improvements and bugfixes --- .../PlocSupervisorDefinitions.h | 2 +- linux/devices/ploc/PlocSupervisorHandler.cpp | 2 +- linux/devices/ploc/PlocSupvUartMan.cpp | 67 ++++++++----------- linux/devices/ploc/PlocSupvUartMan.h | 8 +-- linux/scheduling.cpp | 4 ++ mission/devices/devicedefinitions/SpBase.h | 2 + 6 files changed, 39 insertions(+), 46 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index ac89c8d4..18e92ccb 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -501,7 +501,7 @@ class TcBase : public ploc::SpTcBase { uint16_t getModuleApid() const { return getApid() & APID_MODULE_MASK; } - uint8_t getServiceId() const { return payloadStart[supv::PAYLOAD_OFFSET]; } + uint8_t getServiceId() const { return getPacketData()[TIMESTAMP_LEN]; } static size_t fullSpDataLenFromPayloadLen(size_t payloadLen) { return SECONDARY_HEADER_LEN + payloadLen + CRC_LEN; diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 3293c652..bd027e37 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -138,12 +138,12 @@ void PlocSupervisorHandler::doStartUp() { if (setTimeDuringStartup) { if (startupState == StartupState::OFF) { bootTimeout.resetTimer(); - uartManager.start(); startupState = StartupState::BOOTING; } if (startupState == StartupState::BOOTING) { if (bootTimeout.hasTimedOut()) { uartIsolatorSwitch.pullHigh(); + uartManager.start(); startupState = StartupState::SET_TIME; } } diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 8bc2019d..0db0feef 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -101,12 +101,11 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { lock->unlockMutex(); semaphore->acquire(); while (true) { - // sif::debug << "SUPV UART MAN: Running.." << std::endl; - putTaskToSleep = handleUartReception(); if (putTaskToSleep) { performUartShutdown(); break; } + handleUartReception(); lock->lockMutex(); InternalState currentState = state; lock->unlockMutex(); @@ -126,44 +125,39 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { break; } } - if (putTaskToSleep) { - performUartShutdown(); - break; - } } } } -bool PlocSupvUartManager::handleUartReception() { +ReturnValue_t PlocSupvUartManager::handleUartReception() { ReturnValue_t result = OK; + ReturnValue_t status = OK; ssize_t bytesRead = read(serialPort, reinterpret_cast(recBuf.data()), static_cast(recBuf.size())); if (bytesRead == 0) { - { - MutexGuard mg(lock); - if (state == InternalState::GO_TO_SLEEP) { - return true; - } - } while (result != NO_PACKET_FOUND) { result = tryHdlcParsing(); + if (result != NO_PACKET_FOUND and result != OK) { + status = result; + } } } else if (bytesRead < 0) { sif::warning << "PlocSupvHelper::performOperation: read call failed with error [" << errno << ", " << strerror(errno) << "]" << std::endl; - return true; + return FAILED; } else if (bytesRead >= static_cast(recBuf.size())) { sif::error << "PlocSupvHelper::performOperation: Receive buffer too small for " << bytesRead << " bytes" << std::endl; + return FAILED; } else if (bytesRead > 0) { if (debugMode) { sif::info << "Received " << bytesRead << " bytes from the PLOC Supervisor:" << std::endl; arrayprinter::print(recBuf.data(), bytesRead); } recRingBuf.writeData(recBuf.data(), bytesRead); - tryHdlcParsing(); + status = tryHdlcParsing(); } - return false; + return status; } ReturnValue_t PlocSupvUartManager::startUpdate(std::string file, uint8_t memoryId, @@ -571,23 +565,20 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( bool ackReceived = false; bool packetWasHandled = false; while (true) { - do { - result = tryHdlcParsing(); - } while (result != NO_PACKET_FOUND); + handleUartReception(); if (not decodedQueue.empty()) { size_t packetLen = 0; decodedQueue.retrieve(&packetLen); - decodedRingBuf.readData(decodedBuf.data(), packetLen); + decodedRingBuf.readData(decodedBuf.data(), packetLen, true); tmReader.setData(decodedBuf.data(), packetLen); result = checkReceivedTm(); if (result != returnvalue::OK) { continue; } if (tmReader.getModuleApid() == Apid::TMTC_MAN) { - uint8_t serviceId = tmReader.getServiceId(); int retval = 0; if (not ackReceived) { - retval = handleAckReception(packet, serviceId, packetLen); + retval = handleAckReception(packet, packetLen); if (retval == 1) { sif::debug << "ACK received" << std::endl; ackReceived = true; @@ -596,7 +587,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( return returnvalue::FAILED; } } else { - retval = handleExeAckReception(packet, serviceId, packetLen); + retval = handleExeAckReception(packet, packetLen); if (retval == 1) { sif::debug << "EXE ACK received" << std::endl; break; @@ -619,7 +610,8 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( return returnvalue::OK; } -int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen) { +int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, size_t packetLen) { + uint8_t serviceId = tmReader.getServiceId(); if (serviceId == static_cast(supv::tm::TmtcId::ACK) or serviceId == static_cast(supv::tm::TmtcId::NAK)) { AcknowledgmentReport ackReport(tmReader); @@ -650,8 +642,8 @@ int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, uint8_t serviceId, return 0; } -int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, - size_t packetLen) { +int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, size_t packetLen) { + uint8_t serviceId = tmReader.getServiceId(); if (serviceId == static_cast(supv::tm::TmtcId::EXEC_ACK) or serviceId == static_cast(supv::tm::TmtcId::EXEC_NAK)) { ExecutionReport exeReport(tmReader); @@ -660,7 +652,7 @@ int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, uint8_t service triggerEvent(EXE_RECEPTION_FAILURE); return -1; } - if (exeReport.getRefModuleApid() == tc.getApid() and + if (exeReport.getRefModuleApid() == tc.getModuleApid() and exeReport.getRefServiceId() == tc.getServiceId()) { if (serviceId == static_cast(supv::tm::TmtcId::EXEC_ACK)) { return 1; @@ -688,8 +680,7 @@ ReturnValue_t PlocSupvUartManager::checkReceivedTm() { triggerEvent(SUPV_REPLY_SIZE_MISSMATCH, rememberApid); return result; } - result = tmReader.checkCrc(); - if (result != returnvalue::OK) { + if (not tmReader.verifyCrc()) { triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid); return result; } @@ -745,12 +736,12 @@ ReturnValue_t PlocSupvUartManager::calcImageCrc() { ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { ReturnValue_t result = returnvalue::OK; resetSpParams(); - supv::CheckMemory packet(spParams); - result = packet.buildPacket(update.memoryId, update.startAddress, update.fullFileSize); + supv::CheckMemory tcPacket(spParams); + result = tcPacket.buildPacket(update.memoryId, update.startAddress, update.fullFileSize); if (result != returnvalue::OK) { return result; } - result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); + result = encodeAndSendPacket(tcPacket.getFullPacket(), tcPacket.getFullPacketLen()); if (result != returnvalue::OK) { return result; } @@ -760,14 +751,11 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { bool packetWasHandled = false; bool exeReceived = false; while (true) { - do { - result = tryHdlcParsing(); - - } while (result != NO_PACKET_FOUND); + handleUartReception(); if (not decodedQueue.empty()) { size_t packetLen = 0; decodedQueue.retrieve(&packetLen); - decodedRingBuf.readData(decodedBuf.data(), packetLen); + decodedRingBuf.readData(decodedBuf.data(), packetLen, true); tmReader.setData(decodedBuf.data(), packetLen); result = checkReceivedTm(); if (result != returnvalue::OK) { @@ -775,10 +763,9 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { } packetWasHandled = false; if (tmReader.getModuleApid() == Apid::TMTC_MAN) { - uint8_t serviceId = tmReader.getServiceId(); int retval = 0; if (not ackReceived) { - retval = handleAckReception(packet, serviceId, packetLen); + retval = handleAckReception(tcPacket, packetLen); if (retval == 1) { packetWasHandled = true; ackReceived = true; @@ -786,7 +773,7 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { return returnvalue::FAILED; } } else { - retval = handleExeAckReception(packet, serviceId, packetLen); + retval = handleExeAckReception(tcPacket, packetLen); if (retval == 1) { packetWasHandled = true; exeReceived = true; diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index ce24e721..9d9e3738 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -252,14 +252,14 @@ class PlocSupvUartManager : public DeviceCommunicationIF, std::array tmBuf{}; - bool debugMode = false; + bool debugMode = true; bool timestamping = true; // Remembers APID to know at which command a procedure failed uint16_t rememberApid = 0; ReturnValue_t handleRunningLongerRequest(); - bool handleUartReception(); + ReturnValue_t handleUartReception(); void addHdlcFraming(const uint8_t* src, size_t slen, uint8_t* dst, size_t* dlen, size_t maxDest); int removeHdlcFramingWithCrcCheck(const uint8_t* src, size_t slen, uint8_t* dst, size_t* dlen); @@ -275,8 +275,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF, // ReturnValue_t performEventBufferRequest(); ReturnValue_t handlePacketTransmissionNoReply(supv::TcBase& packet, uint32_t timeoutExecutionReport = 60000); - int handleAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen); - int handleExeAckReception(supv::TcBase& tc, uint8_t serviceId, size_t packetLen); + int handleAckReception(supv::TcBase& tc, size_t packetLen); + int handleExeAckReception(supv::TcBase& tc, size_t packetLen); /** * @brief Handles reading of TM packets from the communication interface diff --git a/linux/scheduling.cpp b/linux/scheduling.cpp index 1300266f..75706185 100644 --- a/linux/scheduling.cpp +++ b/linux/scheduling.cpp @@ -54,6 +54,8 @@ void scheduling::addMpsocSupvHandlers(PeriodicTaskIF* plTask) { plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_WRITE); plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_READ); plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_READ); + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::SEND_READ); + plTask->addComponent(objects::PLOC_SUPERVISOR_HANDLER, DeviceHandlerIF::GET_READ); #endif #if OBSW_ADD_PLOC_MPSOC == 1 @@ -62,5 +64,7 @@ void scheduling::addMpsocSupvHandlers(PeriodicTaskIF* plTask) { plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_WRITE); plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_READ); plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ); + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_READ); + plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ); #endif } diff --git a/mission/devices/devicedefinitions/SpBase.h b/mission/devices/devicedefinitions/SpBase.h index 22c138b5..11d67a59 100644 --- a/mission/devices/devicedefinitions/SpBase.h +++ b/mission/devices/devicedefinitions/SpBase.h @@ -46,6 +46,8 @@ class SpTcBase { void updateLenFromParams() { spParams.creator.setDataLenField(spParams.fullPayloadLen - 1); } const uint8_t* getFullPacket() const { return spParams.buf; } + const uint8_t* getPacketData() const { return spParams.buf + ccsds::HEADER_LEN; } + size_t getFullPacketLen() const { return spParams.creator.getFullPacketLen(); } uint16_t getApid() const { return spParams.creator.getApid(); } From c430caadd7446e46cf59ee9cd2d32c202072570c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 17 Nov 2022 12:00:53 +0100 Subject: [PATCH 189/244] changed SusDummies from ExtendedController to DeviceHandler --- dummies/SusDummy.cpp | 102 ++++++++++++------------------------------- dummies/SusDummy.h | 47 ++++++++++++-------- dummies/helpers.cpp | 17 ++++++-- 3 files changed, 71 insertions(+), 95 deletions(-) diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 46b2ec68..28f85969 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -1,87 +1,43 @@ - #include "SusDummy.h" -#include -#include +SusDummy::SusDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) + : DeviceHandlerBase(objectId, comif, comCookie), susSet(this) {} -#include -#include +SusDummy::~SusDummy() {} -SusDummy::SusDummy() : ExtendedControllerBase(objects::SUS_0_N_LOC_XFYFZM_PT_XF), susSet(this) { - ObjectManager::instance()->insert(objects::SUS_6_R_LOC_XFYBZM_PT_XF, this); - ObjectManager::instance()->insert(objects::SUS_1_N_LOC_XBYFZM_PT_XB, this); - ObjectManager::instance()->insert(objects::SUS_7_R_LOC_XBYBZM_PT_XB, this); - ObjectManager::instance()->insert(objects::SUS_2_N_LOC_XFYBZB_PT_YB, this); - ObjectManager::instance()->insert(objects::SUS_8_R_LOC_XBYBZB_PT_YB, this); - ObjectManager::instance()->insert(objects::SUS_3_N_LOC_XFYBZF_PT_YF, this); - ObjectManager::instance()->insert(objects::SUS_9_R_LOC_XBYBZB_PT_YF, this); - ObjectManager::instance()->insert(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, this); - ObjectManager::instance()->insert(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, this); - ObjectManager::instance()->insert(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, this); - ObjectManager::instance()->insert(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, this); +void SusDummy::doStartUp() {} + +void SusDummy::doShutDown() {} + +ReturnValue_t SusDummy::buildNormalDeviceCommand(DeviceCommandId_t *id) { return NOTHING_TO_SEND; } + +ReturnValue_t SusDummy::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return NOTHING_TO_SEND; } -ReturnValue_t SusDummy::initialize() { - static bool done = false; - if (not done) { - done = true; - ReturnValue_t result = ExtendedControllerBase::initialize(); - if (result != returnvalue::OK) { - return result; - } - } +ReturnValue_t SusDummy::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) { return returnvalue::OK; } -ReturnValue_t SusDummy::handleCommandMessage(CommandMessage* message) { - return returnvalue::FAILED; -} - -void SusDummy::performControlOperation() { - // value = sin(iteration / 80. * M_PI + 10) * 10 - 10; - - susSet.read(); - // susSet.temperatureCelcius = value; - // if ((iteration % 100) < 20) { - // susSet.setValidity(false, true); - // } else { - // susSet.setValidity(true, true); - // } - susSet.channels[0] = 3913; - susSet.channels[1] = 3912; - susSet.channels[2] = 3799; - susSet.channels[3] = 3797; - susSet.channels[4] = 4056; - susSet.setValidity(true, true); - susSet.commit(); - iteration++; -} - -ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, - LocalDataPoolManager& poolManager) { - localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); - localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, - new PoolEntry({0, 0, 0, 0, 0, 0}, true)); - +ReturnValue_t SusDummy::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) { return returnvalue::OK; } -LocalPoolDataSetBase* SusDummy::getDataSetHandle(sid_t sid) { - switch (sid.ownerSetId) { - case SUS::SUS_DATA_SET_ID: - return &susSet; - default: - return nullptr; - } -} - -ReturnValue_t SusDummy::checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t* msToReachTheMode) { - if (submode != SUBMODE_NONE) { - return INVALID_SUBMODE; - } - if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) { - return INVALID_MODE; - } +ReturnValue_t SusDummy::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { return returnvalue::OK; } + +void SusDummy::fillCommandAndReplyMap() {} + +uint32_t SusDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; } + +ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); + localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, + new PoolEntry({0, 0, 0, 0, 0, 0},true)); + + return returnvalue::OK; +} diff --git a/dummies/SusDummy.h b/dummies/SusDummy.h index 3e24f368..a229bd87 100644 --- a/dummies/SusDummy.h +++ b/dummies/SusDummy.h @@ -1,27 +1,36 @@ -#pragma once +#ifndef DUMMIES_SUSDUMMY_H_ +#define DUMMIES_SUSDUMMY_H_ -#include -#include +#include -class SusDummy : public ExtendedControllerBase { +#include "mission/devices/devicedefinitions/SusDefinitions.h" + +class SusDummy : public DeviceHandlerBase { public: - SusDummy(); + static const DeviceCommandId_t SIMPLE_COMMAND = 1; + static const DeviceCommandId_t PERIODIC_REPLY = 2; - ReturnValue_t initialize() override; + static const uint8_t SIMPLE_COMMAND_DATA = 1; + static const uint8_t PERIODIC_REPLY_DATA = 2; + + SusDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie); + virtual ~SusDummy(); protected: - virtual ReturnValue_t handleCommandMessage(CommandMessage* message) override; - virtual void performControlOperation() override; - virtual ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, - LocalDataPoolManager& poolManager) override; - virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; - - // Mode abstract functions - virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t* msToReachTheMode) override; - - private: - int iteration = 0; - float value = 0; SUS::SusDataset susSet; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; }; + +#endif /* DUMMIES_SUSDUMMY_H_ */ diff --git a/dummies/helpers.cpp b/dummies/helpers.cpp index ef144dc6..8d8ac70b 100644 --- a/dummies/helpers.cpp +++ b/dummies/helpers.cpp @@ -1,3 +1,5 @@ +#include "helpers.h" + #include #include #include @@ -19,8 +21,6 @@ #include #include -#include "helpers.h" - using namespace dummy; void dummy::createDummies(DummyCfg cfg) { @@ -63,7 +63,18 @@ void dummy::createDummies(DummyCfg cfg) { } if (cfg.addSusDummies) { - new SusDummy(); + new SusDummy(objects::SUS_0_N_LOC_XFYFZM_PT_XF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_1_N_LOC_XBYFZM_PT_XB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_2_N_LOC_XFYBZB_PT_YB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_3_N_LOC_XFYBZF_PT_YF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_6_R_LOC_XFYBZM_PT_XF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_7_R_LOC_XBYBZM_PT_XB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_8_R_LOC_XBYBZB_PT_YB, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_9_R_LOC_XBYBZB_PT_YF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, objects::DUMMY_COM_IF, comCookieDummy); + new SusDummy(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, objects::DUMMY_COM_IF, comCookieDummy); } if (cfg.addTempSensorDummies) { From f3ac6d4a7e03658e45b588b496115838f36850ff Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 17 Nov 2022 13:01:29 +0100 Subject: [PATCH 190/244] added GPS and RW sensor readings. added RW1..4 dummies to acsTask --- bsp_q7s/core/InitMission.cpp | 4 ++++ mission/controller/acs/SensorValues.cpp | 5 ++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp index b922ae88..1406c375 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/InitMission.cpp @@ -186,6 +186,10 @@ void initmission::initTasks() { acsCtrlTask->addComponent(objects::GYRO_3_L3G_HANDLER); acsCtrlTask->addComponent(objects::GPS_CONTROLLER); acsCtrlTask->addComponent(objects::STAR_TRACKER); + acsCtrlTask->addComponent(objects::RW1); + acsCtrlTask->addComponent(objects::RW2); + acsCtrlTask->addComponent(objects::RW3); + acsCtrlTask->addComponent(objects::RW4); #endif PeriodicTaskIF* acsSysTask = factory->createPeriodicTask( diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 18f0d16f..568a9e1c 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -82,8 +82,11 @@ ReturnValue_t SensorValues::update() { ReturnValue_t susUpdate = updateSus(); ReturnValue_t gyrUpdate = updateGyr(); ReturnValue_t strUpdate = updateStr(); + ReturnValue_t gpsUpdate = updateGps(); + ReturnValue_t rwUpdate = updateRw(); - if ((mgmUpdate && susUpdate && gyrUpdate && strUpdate) == returnvalue::FAILED) { + if ((mgmUpdate && susUpdate && gyrUpdate && strUpdate && gpsUpdate && rwUpdate) == + returnvalue::FAILED) { return returnvalue::FAILED; }; return returnvalue::OK; From 281eb3237cb489bc8d29d4484d0b4f2f3128e936 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 17 Nov 2022 15:55:30 +0100 Subject: [PATCH 191/244] fixed susTotVec and mgmTotVec types to dataSet types --- mission/controller/acs/SensorProcessing.cpp | 29 ++++++++++++++------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 483b94c2..407ba900 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -115,13 +115,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; } } - float mgmVecTot[3] = {0.0, 0.0, 0.0}; + double mgmVecTot[3] = {0.0, 0.0, 0.0}; for (uint8_t i = 0; i < 3; i++) { mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } //-----------------------Mgm Rate Computation --------------------------------------------------- - float mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; + double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool mgmVecTotDerivativeValid = false; double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); if (timeOfSavedMagFieldEst.tv_sec != 0) { @@ -367,7 +367,7 @@ void SensorProcessing::processSus( sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; - float susMeanValue[3] = {0, 0, 0}; + double susMeanValue[3] = {0, 0, 0}; for (uint8_t i = 0; i < 12; i++) { if (validIds[i]) { susMeanValue[0] += susVecBody[0][i]; @@ -375,12 +375,12 @@ void SensorProcessing::processSus( susMeanValue[2] += susVecBody[2][i]; } } - float susVecTot[3] = {0.0, 0.0, 0.0}; - VectorOperations::normalize(susMeanValue, susVecTot, 3); + double susVecTot[3] = {0.0, 0.0, 0.0}; + VectorOperations::normalize(susMeanValue, susVecTot, 3); /* -------- Sun Derivatiative --------------------- */ - float susVecTotDerivative[3] = {0.0, 0.0, 0.0}; + double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool susVecTotDerivativeValid = false; double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); if (timeOfSavedSusDirEst.tv_sec != 0) { @@ -438,10 +438,10 @@ void SensorProcessing::processSus( susDataProcessed->sus10vec.setValid(sus10valid); std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float)); susDataProcessed->sus11vec.setValid(sus11valid); - std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(float)); + std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double)); susDataProcessed->susVecTot.setValid(true); std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative, - 3 * sizeof(float)); + 3 * sizeof(double)); susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); susDataProcessed->sunIjkModel.setValid(true); @@ -538,9 +538,13 @@ void SensorProcessing::processGyr( std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double)); gyrDataProcessed->gyr0vec.setValid(gyr0valid); std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double)); + gyrDataProcessed->gyr1vec.setValid(gyr1valid); std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double)); + gyrDataProcessed->gyr2vec.setValid(gyr2valid); std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double)); + gyrDataProcessed->gyr3vec.setValid(gyr3valid); std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double)); + gyrDataProcessed->gyrVecTot.setValid(true); gyrDataProcessed->setValidity(true, false); } } @@ -582,7 +586,9 @@ 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(), gpsDataProcessed); + (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && + sensorValues->gpsSet.altitude.isValid()), + gpsDataProcessed); processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), @@ -594,7 +600,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, - sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.isValid(), mgmDataProcessed); + sensorValues->gpsSet.altitude.value, + (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() && + sensorValues->gpsSet.altitude.isValid()), + mgmDataProcessed); processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), From 902caa39850f47c7b70f16046d1b664c43359075 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 17 Nov 2022 16:08:44 +0100 Subject: [PATCH 192/244] double length fixes --- mission/controller/acs/SensorProcessing.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 407ba900..4635e673 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -158,10 +158,10 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const mgmDataProcessed->mgm3vec.setValid(mgm3valid); std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float)); mgmDataProcessed->mgm4vec.setValid(mgm4valid); - std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(float)); + std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double)); mgmDataProcessed->mgmVecTot.setValid(true); std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative, - 3 * sizeof(float)); + 3 * sizeof(double)); mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid); std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double)); mgmDataProcessed->magIgrfModel.setValid(gpsValid); From f07b905df54dd138a3cb654b48f5803d40bab82f Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 18 Nov 2022 10:49:38 +0100 Subject: [PATCH 193/244] corrected output direction of sunVectors --- mission/controller/acs/SusConverter.cpp | 10 +++++----- mission/controller/acs/SusConverter.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 26804f12..1dc3a2e2 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -107,17 +107,17 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB float* SusConverter::calculateSunVector() { // Calculate the normalized Sun Vector - sunVectorBodyFrame[0] = (tan(alphaBetaCalibrated[0] * (M_PI / 180)) / + sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) / (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - sunVectorBodyFrame[1] = (tan(alphaBetaCalibrated[1] * (M_PI / 180)) / + sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - sunVectorBodyFrame[2] = - (-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + + sunVectorSensorFrame[2] = + -(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); - return sunVectorBodyFrame; + return sunVectorSensorFrame; } float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6], diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 01143a39..10079e90 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -31,7 +31,7 @@ class SusConverter { // float coeffAlpha[9][10]; // float coeffBeta[9][10]; float alphaBetaCalibrated[2]; //[°] - float sunVectorBodyFrame[3]; //[-] + float sunVectorSensorFrame[3]; //[-] bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, From 13fe25ff031e9d96c5e5d0989c87662b75c3457e Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 21 Nov 2022 10:30:48 +0100 Subject: [PATCH 194/244] added dataPool Output, in case MEKF inverion fails --- mission/controller/acs/MultiplicativeKalmanFilter.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index cc136f61..5d66bd8d 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -893,6 +893,16 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov, *invResidualCov, invResidualCov1, MDF); if (inversionFailed) { + { + PoolReadGuard pg(mekfData); + if (pg.getReadResult() == returnvalue::OK) { + double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; + double zeroVec[3] = {0.0, 0.0, 0.0}; + std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + mekfData->setValidity(false, true); + } + } validMekf = false; return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } From 3eed1508fda415db816f89140af6629c697ff1fd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Nov 2022 13:37:05 +0100 Subject: [PATCH 195/244] bump tmtc and scex baud --- common/config/devConf.h | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/common/config/devConf.h b/common/config/devConf.h index 5f5a5bef..931d1ad1 100644 --- a/common/config/devConf.h +++ b/common/config/devConf.h @@ -57,7 +57,7 @@ namespace uart { static constexpr size_t HYPERION_GPS_REPLY_MAX_BUFFER = 1024; static constexpr UartBaudRate SYRLINKS_BAUD = UartBaudRate::RATE_38400; -static constexpr UartBaudRate SCEX_BAUD = UartBaudRate::RATE_38400; +static constexpr UartBaudRate SCEX_BAUD = UartBaudRate::RATE_115200; static constexpr UartBaudRate GNSS_BAUD = UartBaudRate::RATE_9600; static constexpr UartBaudRate PLOC_MPSOC_BAUD = UartBaudRate::RATE_115200; static constexpr UartBaudRate PLOC_SUPV_BAUD = UartBaudRate::RATE_115200; diff --git a/tmtc b/tmtc index 9cd4846d..f5766ea4 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 9cd4846d396373325fc5e03a367fed236dfc16d7 +Subproject commit f5766ea419ff2213468dab7e19a81075aefd1ad9 From c1e92605d40ceace834e921d1d1582e753fe7749 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Nov 2022 13:38:33 +0100 Subject: [PATCH 196/244] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 9cd4846d..f5766ea4 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 9cd4846d396373325fc5e03a367fed236dfc16d7 +Subproject commit f5766ea419ff2213468dab7e19a81075aefd1ad9 From 0a5b79b8cd68da29502900f71a8a741b6ee02db8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Nov 2022 14:01:31 +0100 Subject: [PATCH 197/244] extend power switch handling --- bsp_q7s/em/emObjectFactory.cpp | 16 ++++++++++++---- dummies/helpers.cpp | 4 +++- dummies/helpers.h | 4 +++- 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index aba884f1..825f7c03 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -38,13 +38,21 @@ void ObjectFactory::produce(void* args) { #if OBSW_ADD_SYRLINKS == 1 dummyCfg.addSyrlinksDummies = false; #endif - dummy::createDummies(dummyCfg); +#if OBSW_ADD_GOMSPACE_PCDU == 1 + dummyCfg.addPowerDummies = false; +#endif + + PowerSwitchIF* pwrSwitcher = nullptr; +#if OBSW_ADD_GOMSPACE_PCDU == 0 + pwrSwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); +#else + createPcduComponents(gpioComIF, &pwrSwitcher); +#endif + + dummy::createDummies(dummyCfg, *pwrSwitcher); new CoreController(objects::CORE_CONTROLLER); - PowerSwitchIF* pwrSwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); - static_cast(pwrSwitcher); - // Regular FM code, does not work for EM if the hardware is not connected // createPcduComponents(gpioComIF, &pwrSwitcher); // createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher); diff --git a/dummies/helpers.cpp b/dummies/helpers.cpp index 495d6f6c..c489d3a0 100644 --- a/dummies/helpers.cpp +++ b/dummies/helpers.cpp @@ -19,10 +19,11 @@ #include #include #include +#include using namespace dummy; -void dummy::createDummies(DummyCfg cfg) { +void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch) { new ComIFDummy(objects::DUMMY_COM_IF); ComCookieDummy* comCookieDummy = new ComCookieDummy(); new BpxDummy(objects::BPX_BATT_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); @@ -67,5 +68,6 @@ void dummy::createDummies(DummyCfg cfg) { if (cfg.addTempSensorDummies) { new TemperatureSensorsDummy(); } + new CamSwitcher(objects::CAM_SWITCHER, pwrSwitch, power::NO_SWITCH); new PlPcduDummy(objects::PLPCDU_HANDLER, objects::DUMMY_COM_IF, comCookieDummy); } diff --git a/dummies/helpers.h b/dummies/helpers.h index f509f316..bbaab34d 100644 --- a/dummies/helpers.h +++ b/dummies/helpers.h @@ -1,5 +1,7 @@ #pragma once +#include + namespace dummy { struct DummyCfg { @@ -12,6 +14,6 @@ struct DummyCfg { bool addRtdComIFDummy = true; }; -void createDummies(DummyCfg cfg); +void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch); } // namespace dummy From 697e27f876827b8d8c3ad9f8b4af0bc3eaae13f3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Nov 2022 14:16:21 +0100 Subject: [PATCH 198/244] scex working --- bsp_q7s/em/emObjectFactory.cpp | 5 +++-- linux/scheduling.cpp | 11 +++++++++++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 825f7c03..8d9edd6c 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include "OBSWConfig.h" #include "bsp_q7s/core/CoreController.h" @@ -103,8 +104,8 @@ void ObjectFactory::produce(void* args) { createTestComponents(gpioComIF); #endif /* OBSW_ADD_TEST_CODE == 1 */ #if OBSW_ADD_SCEX_DEVICE == 1 - createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), true, - std::nullopt); + createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false, + pcdu::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V); #endif createAcsController(true); } diff --git a/linux/scheduling.cpp b/linux/scheduling.cpp index 1300266f..c159ccc9 100644 --- a/linux/scheduling.cpp +++ b/linux/scheduling.cpp @@ -37,6 +37,17 @@ void scheduling::schedulingScex(TaskFactory& factory, PeriodicTaskIF*& scexDevHa printAddObjectError("SCEX_DEV", objects::SCEX); } result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } + result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ); + if (result != returnvalue::OK) { + printAddObjectError("SCEX_DEV", objects::SCEX); + } result = returnvalue::OK; scexReaderTask = factory.createPeriodicTask( From 4dfd97a3001f370197dfd917a6e7e8a6dc103c40 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Nov 2022 14:17:23 +0100 Subject: [PATCH 199/244] add PCDU for EM permanently --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 21ba92cf..765f1fc9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -141,7 +141,7 @@ set(OBSW_ADD_TMP_DEVICES ${INIT_VAL} CACHE STRING "Add TMP devices") set(OBSW_ADD_GOMSPACE_PCDU - ${INIT_VAL} + 1 CACHE STRING "Add GomSpace PCDU modules") set(OBSW_ADD_RW ${INIT_VAL} From 9953a49b09ab34d1e75ba1a4ff03ca0b570a0ac6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Nov 2022 17:55:56 +0100 Subject: [PATCH 200/244] minor improvements for PLOC SUPV update --- linux/devices/ploc/PlocSupvUartMan.cpp | 12 ++++++------ linux/devices/ploc/PlocSupvUartMan.h | 2 +- tmtc | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 0db0feef..ee91554e 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -314,17 +314,17 @@ void PlocSupvUartManager::executeFullCheckMemoryCommand() { sif::info << "PLOC SUPV Mem Check: Selecting Memory" << std::endl; result = selectMemory(); if (result != returnvalue::OK) { - triggerEvent(SUPV_MEM_CHECK_FAIL, result); + triggerEvent(SUPV_MEM_CHECK_FAIL, result, 1); return; } sif::info << "PLOC SUPV Mem Check: Preparing Update" << std::endl; result = prepareUpdate(); if (result != returnvalue::OK) { - triggerEvent(SUPV_MEM_CHECK_FAIL, result); + triggerEvent(SUPV_MEM_CHECK_FAIL, result, 2); return; } sif::info << "PLOC SUPV Mem Check: Memory Check" << std::endl; - handleCheckMemoryCommand(); + handleCheckMemoryCommand(3); } ReturnValue_t PlocSupvUartManager::executeUpdate() { @@ -369,7 +369,7 @@ ReturnValue_t PlocSupvUartManager::updateOperation() { return result; } sif::info << "PlocSupvHelper::performUpdate: Memory Check" << std::endl; - return handleCheckMemoryCommand(); + return handleCheckMemoryCommand(0); } ReturnValue_t PlocSupvUartManager::writeUpdatePackets() { @@ -733,7 +733,7 @@ ReturnValue_t PlocSupvUartManager::calcImageCrc() { return result; } -ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { +ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand(uint8_t failStep) { ReturnValue_t result = returnvalue::OK; resetSpParams(); supv::CheckMemory tcPacket(spParams); @@ -803,7 +803,7 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand() { << std::setfill('0') << std::hex << std::setw(4) << static_cast(update.crc) << " but received CRC 0x" << std::setw(4) << report.getCrc() << std::dec << std::endl; - triggerEvent(SUPV_MEM_CHECK_FAIL, result); + triggerEvent(SUPV_MEM_CHECK_FAIL, result, failStep); } } } diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 9d9e3738..0d191b7e 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -298,7 +298,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, // Calculates CRC over image. Will be used for verification after update writing has // finished. ReturnValue_t calcImageCrc(); - ReturnValue_t handleCheckMemoryCommand(); + ReturnValue_t handleCheckMemoryCommand(uint8_t failStep); ReturnValue_t exeReportHandling(); /** * @brief Return size of file with name filename diff --git a/tmtc b/tmtc index f5766ea4..ac2b9cb7 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f5766ea419ff2213468dab7e19a81075aefd1ad9 +Subproject commit ac2b9cb712405ee07b9de1950fd6c923a6f184b5 From cc79ffc57bec8fbdad4b22aed3dbeec86de869f3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Nov 2022 18:32:23 +0100 Subject: [PATCH 201/244] some remaining bugfixes --- .../PlocSupervisorDefinitions.h | 7 +--- linux/devices/ploc/PlocSupervisorHandler.cpp | 37 +++++++++---------- linux/devices/ploc/PlocSupervisorHandler.h | 8 ++-- linux/devices/ploc/PlocSupvUartMan.cpp | 5 +++ linux/devices/ploc/PlocSupvUartMan.h | 3 +- tmtc | 2 +- 6 files changed, 30 insertions(+), 32 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 18e92ccb..8c638c83 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -1681,7 +1681,7 @@ class UpdateStatusReport { if (not tmReader.crcIsOk()) { return result::CRC_FAILURE; } - if (tmReader.getApid() != Apid::MEM_MAN) { + if (tmReader.getModuleApid() != Apid::MEM_MAN) { return result::INVALID_APID; } if (tmReader.getBufSize() < MIN_TMTC_LEN + PAYLOAD_LEN or @@ -1690,10 +1690,7 @@ class UpdateStatusReport { << std::endl; return result::BUF_TOO_SMALL; } - size_t remLen = PAYLOAD_LEN; - if (remLen < PAYLOAD_LEN) { - return result::INVALID_REPLY_LENGTH; - } + size_t remLen = tmReader.getPayloadLen(); const uint8_t* dataFieldPtr = tmReader.getPayloadStart(); SerializeAdapter::deSerialize(&memoryId, &dataFieldPtr, &remLen, SerializeIF::Endianness::BIG); SerializeAdapter::deSerialize(&n, &dataFieldPtr, &remLen, SerializeIF::Endianness::BIG); diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index bd027e37..3a8565eb 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -84,7 +85,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, break; } - if (plocSupvHelperExecuting) { + if (uartManager.longerRequestActive()) { return result::SUPV_HELPER_EXECUTING; } @@ -98,6 +99,7 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) { return result::FILENAME_TOO_LONG; } + shutdownCmdSent = false; UpdateParams params; result = extractUpdateCommand(data, size, params); if (result != returnvalue::OK) { @@ -107,15 +109,15 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, if (result != returnvalue::OK) { return result; } - plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } case CONTINUE_UPDATE: { + shutdownCmdSent = false; uartManager.initiateUpdateContinuation(); - plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } case MEMORY_CHECK_WITH_FILE: { + shutdownCmdSent = false; UpdateParams params; ReturnValue_t result = extractBaseParams(&data, size, params); if (result != returnvalue::OK) { @@ -125,7 +127,6 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId, return HasFileSystemIF::FILE_DOES_NOT_EXIST; } uartManager.performMemCheck(params.file, params.memId, params.startAddr); - plocSupvHelperExecuting = true; return EXECUTION_FINISHED; } default: @@ -777,7 +778,6 @@ void PlocSupervisorHandler::handleEvent(EventMessage* eventMessage) { Event event = eventMessage->getEvent(); switch (objectId) { case objects::PLOC_SUPERVISOR_HELPER: { - plocSupvHelperExecuting = false; // After execution of update procedure, PLOC is in a state where it draws approx. 700 mA of // current. To leave this state the shutdown MPSoC command must be sent here. if (event == PlocSupvUartManager::SUPV_UPDATE_FAILED || @@ -786,13 +786,18 @@ void PlocSupervisorHandler::handleEvent(EventMessage* eventMessage) { event == PlocSupvUartManager::SUPV_CONTINUE_UPDATE_SUCCESSFUL || event == PlocSupvUartManager::SUPV_MEM_CHECK_FAIL || event == PlocSupvUartManager::SUPV_MEM_CHECK_OK) { - result = this->executeAction(supv::SHUTDOWN_MPSOC, NO_COMMANDER, nullptr, 0); - if (result != returnvalue::OK) { - triggerEvent(SUPV_MPSOC_SHUWDOWN_BUILD_FAILED); - sif::warning << "PlocSupervisorHandler::handleEvent: Failed to build MPSoC shutdown " - "command" - << std::endl; - return; + // Wait for a short period for the uart state machine to adjust + // TaskFactory::delayTask(5); + if (not shutdownCmdSent) { + shutdownCmdSent = true; + result = this->executeAction(supv::SHUTDOWN_MPSOC, NO_COMMANDER, nullptr, 0); + if (result != returnvalue::OK) { + triggerEvent(SUPV_MPSOC_SHUTDOWN_BUILD_FAILED); + sif::warning << "PlocSupervisorHandler::handleEvent: Failed to build MPSoC shutdown " + "command" + << std::endl; + return; + } } } break; @@ -1217,14 +1222,6 @@ size_t PlocSupervisorHandler::getNextReplyLength(DeviceCommandId_t commandId) { return replyLen; } -ReturnValue_t PlocSupervisorHandler::doSendReadHook() { - // Prevent DHB from polling UART during commands executed by the supervisor helper task - if (plocSupvHelperExecuting) { - return returnvalue::FAILED; - } - return returnvalue::OK; -} - void PlocSupervisorHandler::doOffActivity() {} void PlocSupervisorHandler::handleDeviceTm(const uint8_t* data, size_t dataSize, diff --git a/linux/devices/ploc/PlocSupervisorHandler.h b/linux/devices/ploc/PlocSupervisorHandler.h index 557639e1..4ffe4166 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.h +++ b/linux/devices/ploc/PlocSupervisorHandler.h @@ -59,7 +59,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { uint8_t expectedReplies = 1, bool useAlternateId = false, DeviceCommandId_t alternateReplyID = 0) override; size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override; - ReturnValue_t doSendReadHook() override; + // ReturnValue_t doSendReadHook() override; void doOffActivity() override; private: @@ -81,7 +81,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { //! [EXPORT] : [COMMENT] Supervisor helper currently executing a command static const Event SUPV_HELPER_EXECUTING = MAKE_EVENT(7, severity::LOW); //! [EXPORT] : [COMMENT] Failed to build the command to shutdown the MPSoC - static const Event SUPV_MPSOC_SHUWDOWN_BUILD_FAILED = MAKE_EVENT(8, severity::LOW); + static const Event SUPV_MPSOC_SHUTDOWN_BUILD_FAILED = MAKE_EVENT(8, severity::LOW); static const uint16_t APID_MASK = 0x7FF; static const uint16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF; @@ -119,6 +119,7 @@ class PlocSupervisorHandler : public DeviceHandlerBase { SerialComIF* uartComIf = nullptr; LinuxLibgpioIF* gpioComIF = nullptr; Gpio uartIsolatorSwitch; + bool shutdownCmdSent = false; supv::HkSet hkset; supv::BootStatusReport bootStatusReport; @@ -151,9 +152,6 @@ class PlocSupervisorHandler : public DeviceHandlerBase { std::string supervisorFilePath = "ploc/supervisor"; std::string activeMramFile; - // Supervisor helper class currently executing a command - bool plocSupvHelperExecuting = false; - Countdown executionReportTimeout = Countdown(EXECUTION_DEFAULT_TIMEOUT, false); Countdown acknowledgementReportTimeout = Countdown(ACKNOWLEDGE_DEFAULT_TIMEOUT, false); // Vorago nees some time to boot properly diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index ee91554e..6d938efe 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1148,3 +1148,8 @@ int PlocSupvUartManager::removeHdlcFramingWithCrcCheck(const uint8_t* src, size_ *dlen = tlen - 2; return 0; } + +bool PlocSupvUartManager::longerRequestActive() const { + MutexGuard mg(lock); + return state == InternalState::DEDICATED_REQUEST; +} diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 0d191b7e..d36a9cd4 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -159,6 +159,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, * @brief Can be used to start the UART reception */ void start(); + bool longerRequestActive() const; static uint32_t buildProgParams1(uint8_t percent, uint16_t seqCount); static uint32_t buildApidServiceParam1(uint8_t apid, uint8_t serviceId); @@ -252,7 +253,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, std::array tmBuf{}; - bool debugMode = true; + bool debugMode = false; bool timestamping = true; // Remembers APID to know at which command a procedure failed diff --git a/tmtc b/tmtc index ac2b9cb7..70a1d492 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit ac2b9cb712405ee07b9de1950fd6c923a6f184b5 +Subproject commit 70a1d49246b5bd5297c22d336e9dd8f58f019f90 From 1b92aa03f32cf366196cc7642ba63e8b83a308b6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 24 Nov 2022 11:22:55 +0100 Subject: [PATCH 202/244] create dummy switcher --- bsp_hosted/ObjectFactory.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 89b83041..6db37e93 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -1,5 +1,6 @@ #include "ObjectFactory.h" +#include #include #include #include @@ -79,6 +80,7 @@ void ObjectFactory::produce(void* args) { ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel); DummyGpioIF* dummyGpioIF = new DummyGpioIF(); + auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); static_cast(dummyGpioIF); #ifdef PLATFORM_UNIX new SerialComIF(objects::UART_COM_IF); @@ -106,7 +108,7 @@ void ObjectFactory::produce(void* args) { #endif dummy::DummyCfg cfg; - dummy::createDummies(cfg); + dummy::createDummies(cfg, *dummySwitcher); new ThermalController(objects::THERMAL_CONTROLLER); new TestTask(objects::TEST_TASK); } From cf80ab01fdcbdd23ff9f27ad188c42f63c2a098f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 24 Nov 2022 11:22:55 +0100 Subject: [PATCH 203/244] create dummy switcher --- bsp_hosted/ObjectFactory.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp index 89b83041..6db37e93 100644 --- a/bsp_hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -1,5 +1,6 @@ #include "ObjectFactory.h" +#include #include #include #include @@ -79,6 +80,7 @@ void ObjectFactory::produce(void* args) { ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel); DummyGpioIF* dummyGpioIF = new DummyGpioIF(); + auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0); static_cast(dummyGpioIF); #ifdef PLATFORM_UNIX new SerialComIF(objects::UART_COM_IF); @@ -106,7 +108,7 @@ void ObjectFactory::produce(void* args) { #endif dummy::DummyCfg cfg; - dummy::createDummies(cfg); + dummy::createDummies(cfg, *dummySwitcher); new ThermalController(objects::THERMAL_CONTROLLER); new TestTask(objects::TEST_TASK); } From 63a8f31a45298eadc2c346480373a81387154541 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 24 Nov 2022 11:23:21 +0100 Subject: [PATCH 204/244] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index f5766ea4..70a1d492 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f5766ea419ff2213468dab7e19a81075aefd1ad9 +Subproject commit 70a1d49246b5bd5297c22d336e9dd8f58f019f90 From c7b90fb8e0d0a6bf1e2a20e36aee4e0e7882fbcb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 24 Nov 2022 11:58:41 +0100 Subject: [PATCH 205/244] something hangs up --- linux/devices/ploc/PlocSupervisorHandler.cpp | 4 +-- linux/devices/ploc/PlocSupvUartMan.cpp | 37 ++++++-------------- linux/devices/ploc/PlocSupvUartMan.h | 4 +-- tmtc | 2 +- 4 files changed, 15 insertions(+), 32 deletions(-) diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index 3a8565eb..b022f119 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -578,8 +578,8 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r // } tmReader.setData(start, remainingSize); - sif::debug << "PlocSupervisorHandler::scanForReply: Received Packet" << std::endl; - arrayprinter::print(start, remainingSize); + // sif::debug << "PlocSupervisorHandler::scanForReply: Received Packet" << std::endl; + // arrayprinter::print(start, remainingSize); uint16_t apid = tmReader.getModuleApid(); switch (apid) { diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 6d938efe..955484e5 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -68,8 +68,8 @@ ReturnValue_t PlocSupvUartManager::initializeInterface(CookieIF* cookie) { // Use non-canonical mode and clear echo flag tty.c_lflag &= ~(ICANON | ECHO); - // Blocking mode, 0.5 seconds timeout - tty.c_cc[VTIME] = 5; + // Blocking mode, 0.2 seconds timeout + tty.c_cc[VTIME] = 2; tty.c_cc[VMIN] = 0; uart::setBaudrate(tty, uartCookie->getBaudrate()); @@ -429,8 +429,7 @@ ReturnValue_t PlocSupvUartManager::writeUpdatePackets() { update.bytesWritten); return result; } - result = handlePacketTransmissionNoReply(packet); - // result = encodeAndSendPacket(packet.getFullPacket(), packet.getFullPacketLen()); + result = handlePacketTransmissionNoReply(packet, 2000); if (result != returnvalue::OK) { triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount), update.bytesWritten); @@ -494,19 +493,6 @@ uint32_t PlocSupvUartManager::buildProgParams1(uint8_t percent, uint16_t seqCoun // return result; // } -ReturnValue_t PlocSupvUartManager::handleRemainingExeReport(ploc::SpTmReader& reader) { - size_t remBytes = reader.getPacketDataLen() + 1; - ReturnValue_t result = handleTmReception(remBytes, tmBuf.data() + ccsds::HEADER_LEN); - if (result != returnvalue::OK) { - sif::warning << "Reading exe failure report failed" << std::endl; - } - result = exeReportHandling(); - if (result != returnvalue::OK) { - sif::warning << "Handling exe report failed" << std::endl; - } - return result; -} - ReturnValue_t PlocSupvUartManager::selectMemory() { ReturnValue_t result = returnvalue::OK; resetSpParams(); @@ -515,7 +501,7 @@ ReturnValue_t PlocSupvUartManager::selectMemory() { if (result != returnvalue::OK) { return result; } - result = handlePacketTransmissionNoReply(packet); + result = handlePacketTransmissionNoReply(packet, 2000); if (result != returnvalue::OK) { return result; } @@ -580,7 +566,6 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( if (not ackReceived) { retval = handleAckReception(packet, packetLen); if (retval == 1) { - sif::debug << "ACK received" << std::endl; ackReceived = true; packetWasHandled = true; } else if (retval == -1) { @@ -589,7 +574,6 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( } else { retval = handleExeAckReception(packet, packetLen); if (retval == 1) { - sif::debug << "EXE ACK received" << std::endl; break; } else if (retval == -1) { return returnvalue::FAILED; @@ -601,7 +585,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( decodedRingBuf.deleteData(packetLen); } } else { - TaskFactory::delayTask(50); + TaskFactory::delayTask(20); } if (countdown.hasTimedOut()) { return result::NO_REPLY_TIMEOUT; @@ -777,7 +761,6 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand(uint8_t failStep) { if (retval == 1) { packetWasHandled = true; exeReceived = true; - break; } else if (retval == -1) { return returnvalue::FAILED; } @@ -795,7 +778,6 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand(uint8_t failStep) { result = report.verifyCrc(update.crc); if (result == returnvalue::OK) { triggerEvent(SUPV_MEM_CHECK_OK, result); - return result; } else { sif::warning @@ -813,7 +795,7 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand(uint8_t failStep) { decodedRingBuf.deleteData(packetLen); } } else { - TaskFactory::delayTask(50); + TaskFactory::delayTask(20); } if (ackReceived and exeReceived and checkReplyReceived) { break; @@ -823,7 +805,6 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand(uint8_t failStep) { } } return returnvalue::OK; - return result; } uint32_t PlocSupvUartManager::getFileSize(std::string filename) { @@ -963,8 +944,10 @@ ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() { ReturnValue_t PlocSupvUartManager::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) { size_t encodedLen = 0; addHdlcFraming(sendData, sendLen, encodedSendBuf.data(), &encodedLen, encodedSendBuf.size()); - sif::debug << "Sending TC" << std::endl; - arrayprinter::print(encodedSendBuf.data(), encodedLen); + if (printTc) { + sif::debug << "Sending TC" << std::endl; + arrayprinter::print(encodedSendBuf.data(), encodedLen); + } size_t bytesWritten = write(serialPort, encodedSendBuf.data(), encodedLen); if (bytesWritten != encodedLen) { sif::warning diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index d36a9cd4..d9739e44 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -253,6 +253,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, std::array tmBuf{}; + bool printTc = false; bool debugMode = false; bool timestamping = true; @@ -275,7 +276,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, ReturnValue_t writeUpdatePackets(); // ReturnValue_t performEventBufferRequest(); ReturnValue_t handlePacketTransmissionNoReply(supv::TcBase& packet, - uint32_t timeoutExecutionReport = 60000); + uint32_t timeoutExecutionReport); int handleAckReception(supv::TcBase& tc, size_t packetLen); int handleExeAckReception(supv::TcBase& tc, size_t packetLen); @@ -310,7 +311,6 @@ class PlocSupvUartManager : public DeviceCommunicationIF, */ uint32_t getFileSize(std::string filename); ReturnValue_t handleEventBufferReception(ploc::SpTmReader& reader); - ReturnValue_t handleRemainingExeReport(ploc::SpTmReader& reader); void resetSpParams(); void pushIpcData(const uint8_t* data, size_t len); diff --git a/tmtc b/tmtc index 70a1d492..292bafa0 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 70a1d49246b5bd5297c22d336e9dd8f58f019f90 +Subproject commit 292bafa0918d386e7f64bf9ed0e114a887420899 From 6766395f4450a1c8c0513611dfbae3742ef3c6e5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 24 Nov 2022 14:42:17 +0100 Subject: [PATCH 206/244] works, but SW updates hangs at 21504 bytes --- .../devicedefinitions/PlocSupervisorDefinitions.h | 9 ++++++--- linux/devices/ploc/PlocSupvUartMan.cpp | 4 ++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 8c638c83..aad0a4c8 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -1154,8 +1154,11 @@ class WriteMemory : public TcBase { } spParams.creator.setSeqFlags(seqFlags); spParams.creator.setSeqCount(sequenceCount); - initPacket(memoryId, startAddress, length, updateData); - auto res = checkSizeAndSerializeHeader(); + auto res = initPacket(memoryId, startAddress, length, updateData); + if (res != returnvalue::OK) { + return res; + } + res = checkSizeAndSerializeHeader(); if (res != returnvalue::OK) { return res; } @@ -1182,7 +1185,7 @@ class WriteMemory : public TcBase { if (result != returnvalue::OK) { return result; } - size_t serializedSize = 6; + size_t serializedSize = MIN_TMTC_LEN; SerializeAdapter::serialize(&memoryId, &data, &serializedSize, spParams.maxSize, SerializeIF::Endianness::BIG); SerializeAdapter::serialize(&n, &data, &serializedSize, spParams.maxSize, diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 955484e5..919ac2aa 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -429,7 +429,7 @@ ReturnValue_t PlocSupvUartManager::writeUpdatePackets() { update.bytesWritten); return result; } - result = handlePacketTransmissionNoReply(packet, 2000); + result = handlePacketTransmissionNoReply(packet, 5000); if (result != returnvalue::OK) { triggerEvent(WRITE_MEMORY_FAILED, buildProgParams1(progPercent, update.sequenceCount), update.bytesWritten); @@ -443,7 +443,7 @@ ReturnValue_t PlocSupvUartManager::writeUpdatePackets() { #if OBSW_DEBUG_PLOC_SUPERVISOR == 1 progressPrinter.print(update.bytesWritten); #endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ - TaskFactory::delayTask(1); + // TaskFactory::delayTask(1); } return result; } From c731418b65a7d51697b0e4ae0eeb2f49db40f782 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 25 Nov 2022 10:28:20 +0100 Subject: [PATCH 207/244] disable PLOC SUPV for host build --- bsp_hosted/OBSWConfig.h.in | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bsp_hosted/OBSWConfig.h.in b/bsp_hosted/OBSWConfig.h.in index 9a80286d..f234e795 100644 --- a/bsp_hosted/OBSWConfig.h.in +++ b/bsp_hosted/OBSWConfig.h.in @@ -15,7 +15,7 @@ #define OBSW_ENABLE_TIMERS 1 #define OBSW_ADD_STAR_TRACKER 0 -#define OBSW_ADD_PLOC_SUPERVISOR 1 +#define OBSW_ADD_PLOC_SUPERVISOR 0 #define OBSW_ADD_PLOC_MPSOC 0 #define OBSW_ADD_SUN_SENSORS 0 #define OBSW_ADD_MGT 0 diff --git a/tmtc b/tmtc index 292bafa0..a54ae782 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 292bafa0918d386e7f64bf9ed0e114a887420899 +Subproject commit a54ae782d46f6f405df8e62ab80b556f971df369 From f771fefbc9d22324a1f48645924ba44ea10b1d4d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 25 Nov 2022 11:04:33 +0100 Subject: [PATCH 208/244] rename scheduling file --- bsp_q7s/core/CMakeLists.txt | 2 +- .../core/{InitMission.cpp => scheduling.cpp} | 42 +++++++++---------- bsp_q7s/core/{InitMission.h => scheduling.h} | 4 +- bsp_q7s/obsw.cpp | 4 +- 4 files changed, 25 insertions(+), 27 deletions(-) rename bsp_q7s/core/{InitMission.cpp => scheduling.cpp} (94%) rename bsp_q7s/core/{InitMission.h => scheduling.h} (92%) diff --git a/bsp_q7s/core/CMakeLists.txt b/bsp_q7s/core/CMakeLists.txt index b4050d73..66b65c62 100644 --- a/bsp_q7s/core/CMakeLists.txt +++ b/bsp_q7s/core/CMakeLists.txt @@ -1,4 +1,4 @@ -target_sources(${OBSW_NAME} PRIVATE CoreController.cpp InitMission.cpp +target_sources(${OBSW_NAME} PRIVATE CoreController.cpp scheduling.cpp ObjectFactory.cpp) target_sources(${SIMPLE_OBSW_NAME} PRIVATE InitMission.cpp) diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/scheduling.cpp similarity index 94% rename from bsp_q7s/core/InitMission.cpp rename to bsp_q7s/core/scheduling.cpp index 3b1eb357..f07eda32 100644 --- a/bsp_q7s/core/InitMission.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -1,4 +1,4 @@ -#include "bsp_q7s/core/InitMission.h" +#include "scheduling.h" #include #include @@ -35,13 +35,13 @@ ServiceInterfaceStream sif::error("ERROR", true, false, true); ObjectManagerIF* objectManager = nullptr; -void initmission::initMission() { +void scheduling::initMission() { sif::info << "Building global objects.." << std::endl; try { /* Instantiate global object manager and also create all objects */ ObjectManager::instance()->setObjectFactoryFunction(ObjectFactory::produce, nullptr); } catch (const std::invalid_argument& e) { - sif::error << "initmission::initMission: Object Construction failed with an " + sif::error << "scheduling::initMission: Object Construction failed with an " "invalid argument: " << e.what(); std::exit(1); @@ -54,7 +54,7 @@ void initmission::initMission() { initTasks(); } -void initmission::initTasks() { +void scheduling::initTasks() { TaskFactory* factory = TaskFactory::instance(); ReturnValue_t result = returnvalue::OK; if (factory == nullptr) { @@ -373,9 +373,8 @@ void initmission::initTasks() { sif::info << "Tasks started.." << std::endl; } -void initmission::createPstTasks(TaskFactory& factory, - TaskDeadlineMissedFunction missedDeadlineFunc, - std::vector& taskVec) { +void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, + std::vector& taskVec) { ReturnValue_t result = returnvalue::OK; /* Polling Sequence Table Default */ #if OBSW_ADD_SPI_TEST_CODE == 0 @@ -384,9 +383,9 @@ void initmission::createPstTasks(TaskFactory& factory, result = pst::pstSpi(spiPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { - sif::warning << "InitMission::initTasks: SPI PST is empty" << std::endl; + sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl; } else { - sif::error << "InitMission::initTasks: Creating SPI PST failed!" << std::endl; + sif::error << "scheduling::initTasks: Creating SPI PST failed!" << std::endl; } } else { taskVec.push_back(spiPst); @@ -399,9 +398,9 @@ void initmission::createPstTasks(TaskFactory& factory, result = pst::pstSpiRw(rwPstTask); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { - sif::warning << "InitMission::initTasks: SPI PST is empty" << std::endl; + sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl; } else { - sif::error << "InitMission::initTasks: Creating SPI PST failed!" << std::endl; + sif::error << "scheduling::initTasks: Creating SPI PST failed!" << std::endl; } } else { taskVec.push_back(rwPstTask); @@ -413,9 +412,9 @@ void initmission::createPstTasks(TaskFactory& factory, result = pst::pstUart(uartPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { - sif::warning << "InitMission::initTasks: UART PST is empty" << std::endl; + sif::warning << "scheduling::initTasks: UART PST is empty" << std::endl; } else { - sif::error << "InitMission::initTasks: Creating UART PST failed!" << std::endl; + sif::error << "scheduling::initTasks: Creating UART PST failed!" << std::endl; } } else { taskVec.push_back(uartPst); @@ -427,9 +426,9 @@ void initmission::createPstTasks(TaskFactory& factory, result = pst::pstI2c(i2cPst); if (result != returnvalue::OK) { if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { - sif::warning << "InitMission::initTasks: I2C PST is empty" << std::endl; + sif::warning << "scheduling::initTasks: I2C PST is empty" << std::endl; } else { - sif::error << "InitMission::initTasks: Creating I2C PST failed!" << std::endl; + sif::error << "scheduling::initTasks: Creating I2C PST failed!" << std::endl; } } else { taskVec.push_back(i2cPst); @@ -442,16 +441,15 @@ void initmission::createPstTasks(TaskFactory& factory, result = pst::pstGompaceCan(gomSpacePstTask); if (result != returnvalue::OK) { if (result != FixedTimeslotTaskIF::SLOT_LIST_EMPTY) { - sif::error << "InitMission::initTasks: GomSpace PST initialization failed!" << std::endl; + sif::error << "scheduling::initTasks: GomSpace PST initialization failed!" << std::endl; } } taskVec.push_back(gomSpacePstTask); #endif } -void initmission::createPusTasks(TaskFactory& factory, - TaskDeadlineMissedFunction missedDeadlineFunc, - std::vector& taskVec) { +void scheduling::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, + std::vector& taskVec) { ReturnValue_t result = returnvalue::OK; /* PUS Services */ PeriodicTaskIF* pusVerification = factory.createPeriodicTask( @@ -530,9 +528,9 @@ void initmission::createPusTasks(TaskFactory& factory, taskVec.push_back(pusLowPrio); } -void initmission::createTestTasks(TaskFactory& factory, - TaskDeadlineMissedFunction missedDeadlineFunc, - std::vector& taskVec) { +void scheduling::createTestTasks(TaskFactory& factory, + TaskDeadlineMissedFunction missedDeadlineFunc, + std::vector& taskVec) { #if OBSW_ADD_TEST_TASK == 1 && OBSW_ADD_TEST_CODE == 1 ReturnValue_t result = returnvalue::OK; static_cast(result); // supress warning in case it is not used diff --git a/bsp_q7s/core/InitMission.h b/bsp_q7s/core/scheduling.h similarity index 92% rename from bsp_q7s/core/InitMission.h rename to bsp_q7s/core/scheduling.h index e0b1d8f2..d43575dc 100644 --- a/bsp_q7s/core/InitMission.h +++ b/bsp_q7s/core/scheduling.h @@ -8,7 +8,7 @@ class PeriodicTaskIF; class TaskFactory; -namespace initmission { +namespace scheduling { void initMission(); void initTasks(); @@ -18,6 +18,6 @@ void createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadl std::vector& taskVec); void createTestTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc, std::vector& taskVec); -}; // namespace initmission +}; // namespace scheduling #endif /* BSP_Q7S_INITMISSION_H_ */ diff --git a/bsp_q7s/obsw.cpp b/bsp_q7s/obsw.cpp index 530c4904..d497eceb 100644 --- a/bsp_q7s/obsw.cpp +++ b/bsp_q7s/obsw.cpp @@ -5,7 +5,7 @@ #include "OBSWConfig.h" #include "commonConfig.h" -#include "core/InitMission.h" +#include "core/scheduling.h" #include "fsfw/tasks/TaskFactory.h" #include "fsfw/version.h" #include "q7sConfig.h" @@ -36,7 +36,7 @@ int obsw::obsw() { return OBSW_ALREADY_RUNNING; } #endif - initmission::initMission(); + scheduling::initMission(); for (;;) { /* Suspend main thread by sleeping it. */ From a20dce5a87ab0a9257732a448e94f8110836035d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 08:48:19 +0100 Subject: [PATCH 209/244] cmakelists fix, bump fsfw --- bsp_q7s/core/CMakeLists.txt | 2 +- fsfw | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/core/CMakeLists.txt b/bsp_q7s/core/CMakeLists.txt index 66b65c62..15d361fd 100644 --- a/bsp_q7s/core/CMakeLists.txt +++ b/bsp_q7s/core/CMakeLists.txt @@ -1,4 +1,4 @@ target_sources(${OBSW_NAME} PRIVATE CoreController.cpp scheduling.cpp ObjectFactory.cpp) -target_sources(${SIMPLE_OBSW_NAME} PRIVATE InitMission.cpp) +target_sources(${SIMPLE_OBSW_NAME} PRIVATE scheduling.cpp) diff --git a/fsfw b/fsfw index 160ff799..46a1c2ba 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 160ff799ace61e24708dcf1fdeaf5fafdf23a4ca +Subproject commit 46a1c2bacea142994666b2201acf0246ba0fd0b4 From bd3350c28a0bc6470856f8f5aab69bec89553a19 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 09:38:32 +0100 Subject: [PATCH 210/244] this might be a bug --- linux/devices/devicedefinitions/PlocSupervisorDefinitions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index aad0a4c8..68ac6447 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -1198,7 +1198,7 @@ class WriteMemory : public TcBase { if (updateDataLen % 2 != 0) { // The data field must be two bytes aligned. Thus, in case the number of bytes to write is odd // a value of zero is added here - data[updateDataLen + 1] = 0; + data[updateDataLen] = 0; } return returnvalue::OK; } From 2338d951c73eb2943af682edc578a4facc9e46f5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 11:21:12 +0100 Subject: [PATCH 211/244] some more robustness tweaks --- cmake/scripts/cmake-build-cfg.py | 66 +++++++++++++++++--------- linux/devices/ploc/PlocSupvUartMan.cpp | 5 ++ linux/devices/ploc/PlocSupvUartMan.h | 2 +- scripts/q7s-cp.py | 8 ++-- tmtc | 2 +- 5 files changed, 55 insertions(+), 28 deletions(-) diff --git a/cmake/scripts/cmake-build-cfg.py b/cmake/scripts/cmake-build-cfg.py index 4c5ee536..ea21fbdf 100755 --- a/cmake/scripts/cmake-build-cfg.py +++ b/cmake/scripts/cmake-build-cfg.py @@ -22,24 +22,38 @@ def main(): parser = argparse.ArgumentParser( description="Processing arguments for CMake build configuration." ) - parser.add_argument("-o", "--osal", type=str, choices=["freertos", "linux", "rtems", "host"], - help="FSFW OSAL. Valid arguments: host, linux, rtems, freertos") parser.add_argument( - "-b", "--buildtype", type=str, choices=["debug", "release", "size", "reldeb"], + "-o", + "--osal", + type=str, + choices=["freertos", "linux", "rtems", "host"], + help="FSFW OSAL. Valid arguments: host, linux, rtems, freertos", + ) + parser.add_argument( + "-b", + "--buildtype", + type=str, + choices=["debug", "release", "size", "reldeb"], help="CMake build type. Valid arguments: debug, release, size, reldeb (Release with Debug " - "Information)", default="debug" + "Information)", + default="debug", ) parser.add_argument("-l", "--builddir", type=str, help="Specify build directory.") parser.add_argument( - "-g", "--generator", type=str, help="CMake Generator", choices=['make', 'ninja'] + "-g", "--generator", type=str, help="CMake Generator", choices=["make", "ninja"] ) parser.add_argument( - "-d", "--defines", + "-d", + "--defines", help="Additional custom defines passed to CMake (supply without -D prefix!)", - nargs="*", type=str + nargs="*", + type=str, ) parser.add_argument( - "-t", "--target-bsp", type=str, help="Target BSP, combination of architecture and machine" + "-t", + "--target-bsp", + type=str, + help="Target BSP, combination of architecture and machine", ) args = parser.parse_args() @@ -59,13 +73,13 @@ def main(): if args.generator is None: generator_cmake_arg = "" else: - if args.generator == 'make': - if os.name == 'nt': + if args.generator == "make": + if os.name == "nt": generator_cmake_arg = '-G "MinGW Makefiles"' else: generator_cmake_arg = '-G "Unix Makefiles"' - elif args.generator == 'ninja': - generator_cmake_arg = '-G Ninja' + elif args.generator == "ninja": + generator_cmake_arg = "-G Ninja" else: generator_cmake_arg = args.generator @@ -79,7 +93,7 @@ def main(): cmake_build_type = "RelWithDebInfo" if args.target_bsp is not None: - cmake_target_cfg_cmd = f"-DTGT_BSP=\"{args.target_bsp}\"" + cmake_target_cfg_cmd = f'-DTGT_BSP="{args.target_bsp}"' else: cmake_target_cfg_cmd = "" @@ -92,10 +106,12 @@ def main(): build_folder = cmake_build_type if args.builddir is not None: build_folder = args.builddir - + build_path = source_location + os.path.sep + build_folder if os.path.isdir(build_path): - remove_old_dir = input(f"{build_folder} folder already exists. Remove old directory? [y/n]: ") + remove_old_dir = input( + f"{build_folder} folder already exists. Remove old directory? [y/n]: " + ) if str(remove_old_dir).lower() in ["yes", "y", 1]: remove_old_dir = True else: @@ -109,17 +125,19 @@ def main(): print(f"Navigating into build directory: {build_path}") os.chdir(build_folder) - cmake_command = f"cmake {generator_cmake_arg} -DFSFW_OSAL=\"{osal}\" " \ - f"-DCMAKE_BUILD_TYPE=\"{cmake_build_type}\" {cmake_target_cfg_cmd} " \ - f"{define_string} {source_location}" + cmake_command = ( + f'cmake {generator_cmake_arg} -DFSFW_OSAL="{osal}" ' + f'-DCMAKE_BUILD_TYPE="{cmake_build_type}" {cmake_target_cfg_cmd} ' + f"{define_string} {source_location}" + ) # Remove redundant spaces - cmake_command = ' '.join(cmake_command.split()) + cmake_command = " ".join(cmake_command.split()) print("Running CMake command: ") - print(f"\" {cmake_command} \"") + print(f'" {cmake_command} "') os.system(cmake_command) print("-- CMake configuration done. --") - - + + def rm_build_dir(path: str): # On windows the permissions of the build directory may have been set to read-only. If this # is the case the permissions are changed before trying to delete the directory. @@ -134,7 +152,9 @@ def determine_source_location() -> str: index += 1 os.chdir("..") if index >= 5: - print("Error: Could not find source directory (determined by looking for fsfw folder!)") + print( + "Error: Could not find source directory (determined by looking for fsfw folder!)" + ) sys.exit(1) return os.getcwd() diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 919ac2aa..890a1240 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -379,6 +379,11 @@ ReturnValue_t PlocSupvUartManager::writeUpdatePackets() { ProgressPrinter::HALF_PERCENT); #endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ uint8_t tempData[supv::WriteMemory::CHUNK_MAX + 1]{}; + if (not std::filesystem::exists(update.file)) { + sif::error << "PlocSupvUartManager::writeUpdatePackets: File " + << update.file << " does not exist" << std::endl; + return HasFileSystemIF::FILE_DOES_NOT_EXIST; + } std::ifstream file(update.file, std::ifstream::binary); uint16_t dataLength = 0; ccsds::SequenceFlags seqFlags; diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index d9739e44..9ebfac02 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -253,7 +253,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, std::array tmBuf{}; - bool printTc = false; + bool printTc = true; bool debugMode = false; bool timestamping = true; diff --git a/scripts/q7s-cp.py b/scripts/q7s-cp.py index 2890a46d..7a4f7405 100755 --- a/scripts/q7s-cp.py +++ b/scripts/q7s-cp.py @@ -17,7 +17,9 @@ def main(): def prompt_ssh_key_removal(): - do_remove_key = input("Do you want to remove problematic keys on localhost ([Y]/n)?: ") + do_remove_key = input( + "Do you want to remove problematic keys on localhost ([Y]/n)?: " + ) if not do_remove_key.lower() in ["y", "yes", "1", ""]: sys.exit(1) port = 0 @@ -65,7 +67,7 @@ def handle_args(): "--flatsat", default=False, action="store_true", - help="Copy to flatsat instead" + help="Copy to flatsat instead", ) # Positional argument(s) parser.add_argument( @@ -86,7 +88,7 @@ def build_cmd(args): address = "eive@flatsat.eive.absatvirt.lw" else: address = "root@localhost" - port_args=f"-P {args.port}" + port_args = f"-P {args.port}" if args.invert: if target == "": target = "." diff --git a/tmtc b/tmtc index a54ae782..a47ca5c9 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a54ae782d46f6f405df8e62ab80b556f971df369 +Subproject commit a47ca5c901e340f4c07bbb778dd63bb61554dbc4 From 003d37c490571144cc61e3abd3b2a3710437ff17 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 11:21:46 +0100 Subject: [PATCH 212/244] bump fsfw --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 46a1c2ba..8eb869e0 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 46a1c2bacea142994666b2201acf0246ba0fd0b4 +Subproject commit 8eb869e073d99e257c6cc62dbf26b4d99422e9b8 From 2c223712ff64db81bf8efd5c995d13becb139de0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 16:24:18 +0100 Subject: [PATCH 213/244] various fixes and improvements --- common/config/devConf.h | 2 +- .../PlocSupervisorDefinitions.h | 1 + linux/devices/ploc/PlocSupervisorHandler.cpp | 21 ++++++++++++--- linux/devices/ploc/PlocSupvUartMan.cpp | 26 ++++++++++++++----- linux/devices/ploc/PlocSupvUartMan.h | 3 ++- 5 files changed, 41 insertions(+), 12 deletions(-) diff --git a/common/config/devConf.h b/common/config/devConf.h index 931d1ad1..beb01282 100644 --- a/common/config/devConf.h +++ b/common/config/devConf.h @@ -60,7 +60,7 @@ static constexpr UartBaudRate SYRLINKS_BAUD = UartBaudRate::RATE_38400; static constexpr UartBaudRate SCEX_BAUD = UartBaudRate::RATE_115200; static constexpr UartBaudRate GNSS_BAUD = UartBaudRate::RATE_9600; static constexpr UartBaudRate PLOC_MPSOC_BAUD = UartBaudRate::RATE_115200; -static constexpr UartBaudRate PLOC_SUPV_BAUD = UartBaudRate::RATE_115200; +static constexpr UartBaudRate PLOC_SUPV_BAUD = UartBaudRate::RATE_921600; static constexpr UartBaudRate STAR_TRACKER_BAUD = UartBaudRate::RATE_921600; } // namespace uart diff --git a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h index 68ac6447..cb0639fa 100644 --- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h +++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h @@ -122,6 +122,7 @@ static const DeviceCommandId_t RESET_PL = 58; static const DeviceCommandId_t ENABLE_NVMS = 59; static const DeviceCommandId_t CONTINUE_UPDATE = 60; static const DeviceCommandId_t MEMORY_CHECK_WITH_FILE = 61; +static constexpr DeviceCommandId_t MEMORY_CHECK = 62; /** Reply IDs */ enum ReplyId : DeviceCommandId_t { diff --git a/linux/devices/ploc/PlocSupervisorHandler.cpp b/linux/devices/ploc/PlocSupervisorHandler.cpp index b022f119..08c63b1a 100644 --- a/linux/devices/ploc/PlocSupervisorHandler.cpp +++ b/linux/devices/ploc/PlocSupervisorHandler.cpp @@ -411,6 +411,7 @@ void PlocSupervisorHandler::fillCommandAndReplyMap() { insertInCommandMap(SET_GPIO); insertInCommandMap(READ_GPIO); insertInCommandMap(FACTORY_RESET); + insertInCommandMap(MEMORY_CHECK); insertInCommandMap(SET_SHUTDOWN_TIMEOUT); insertInCommandMap(FACTORY_FLASH); insertInCommandMap(SET_ADC_ENABLED_CHANNELS); @@ -421,6 +422,7 @@ void PlocSupervisorHandler::fillCommandAndReplyMap() { // ACK replies, use countdown for them insertInReplyMap(ACK_REPORT, 0, nullptr, SIZE_ACK_REPORT, false, &acknowledgementReportTimeout); insertInReplyMap(EXE_REPORT, 0, nullptr, SIZE_EXE_REPORT, false, &executionReportTimeout); + insertInReplyMap(MEMORY_CHECK, 5, nullptr, 0, false); // TM replies insertInReplyMap(HK_REPORT, 3, &hkset, SIZE_HK_REPORT); @@ -507,6 +509,16 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite } break; } + case MEMORY_CHECK: { + enabledReplies = 3; + result = + DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, MEMORY_CHECK); + if (result != returnvalue::OK) { + sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id " << MEMORY_CHECK + << " not in replyMap" << std::endl; + } + break; + } case START_MPSOC: case SHUTDOWN_MPSOC: case SEL_MPSOC_BOOT_IMAGE: @@ -625,10 +637,7 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r static_cast(supv::tm::MemManId::UPDATE_STATUS_REPORT)) { *foundLen = tmReader.getFullPacketLen(); *foundId = ReplyId::UPDATE_STATUS_REPORT; - // TODO: I think this will be handled by the uart manager? - // Actually, this is a bit tricky. Maybe the lower level will have two separate ring - // buffers, one for internally handled packets and one for packets which are handled - // here? + return OK; } } } @@ -667,6 +676,10 @@ ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id, result = handleExecutionReport(packet); break; } + case (UPDATE_STATUS_REPORT): { + // TODO: handle status report here + break; + } default: { sif::debug << "PlocSupervisorHandler::interpretDeviceReply: Unknown device reply id" << std::endl; diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 890a1240..f2ce3b3b 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -116,7 +116,9 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) { break; } case InternalState::DEDICATED_REQUEST: { + updateVtime(1); handleRunningLongerRequest(); + updateVtime(2); MutexGuard mg(lock); state = InternalState::DEFAULT; break; @@ -380,9 +382,9 @@ ReturnValue_t PlocSupvUartManager::writeUpdatePackets() { #endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */ uint8_t tempData[supv::WriteMemory::CHUNK_MAX + 1]{}; if (not std::filesystem::exists(update.file)) { - sif::error << "PlocSupvUartManager::writeUpdatePackets: File " - << update.file << " does not exist" << std::endl; - return HasFileSystemIF::FILE_DOES_NOT_EXIST; + sif::error << "PlocSupvUartManager::writeUpdatePackets: File " << update.file + << " does not exist" << std::endl; + return HasFileSystemIF::FILE_DOES_NOT_EXIST; } std::ifstream file(update.file, std::ifstream::binary); uint16_t dataLength = 0; @@ -553,6 +555,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( return result; } Countdown countdown(timeoutExecutionReport); + dur_millis_t currentDelay = 5; bool ackReceived = false; bool packetWasHandled = false; while (true) { @@ -590,7 +593,10 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( decodedRingBuf.deleteData(packetLen); } } else { - TaskFactory::delayTask(20); + TaskFactory::delayTask(currentDelay); + if (currentDelay < 20) { + currentDelay *= 2; + } } if (countdown.hasTimedOut()) { return result::NO_REPLY_TIMEOUT; @@ -1095,9 +1101,11 @@ void PlocSupvUartManager::addHdlcFraming(const uint8_t* src, size_t slen, uint8_ } size_t dummy = 0; + uint8_t crcRaw[2]; // hdlc crc16 is in little endian format - SerializeAdapter::serialize(&crc16, dst + tlen, &dummy, maxDest, SerializeIF::Endianness::LITTLE); - tlen += dummy; + SerializeAdapter::serialize(&crc16, crcRaw, &dummy, maxDest, SerializeIF::Endianness::LITTLE); + hdlc_add_byte(crcRaw[0], dst, &tlen); + hdlc_add_byte(crcRaw[1], dst, &tlen); dst[tlen++] = 0x7C; *dlen = tlen; @@ -1141,3 +1149,9 @@ bool PlocSupvUartManager::longerRequestActive() const { MutexGuard mg(lock); return state == InternalState::DEDICATED_REQUEST; } + +void PlocSupvUartManager::updateVtime(uint8_t vtime) { + tcgetattr(serialPort, &tty); + tty.c_cc[VTIME] = vtime; + tcsetattr(serialPort, TCSANOW, &tty); +} diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 9ebfac02..e1539c97 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -253,7 +253,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, std::array tmBuf{}; - bool printTc = true; + bool printTc = false; bool debugMode = false; bool timestamping = true; @@ -368,6 +368,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF, ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; void performUartShutdown(); + void updateVtime(uint8_t vtime); }; #endif /* BSP_Q7S_DEVICES_PLOCSUPVHELPER_H_ */ From 425e7c1e520a778f8cc896f06d0a03b8872e172a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 16:27:33 +0100 Subject: [PATCH 214/244] increase regular delay to up to 80 ms --- linux/devices/ploc/PlocSupvUartMan.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index f2ce3b3b..ee232f47 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -594,7 +594,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply( } } else { TaskFactory::delayTask(currentDelay); - if (currentDelay < 20) { + if (currentDelay < 80) { currentDelay *= 2; } } From acc77399587144974c21544c5b04a5fe397248ba Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 16:33:50 +0100 Subject: [PATCH 215/244] bump changelog --- CHANGELOG.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6dd1f9da..ba0e0641 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,7 +13,9 @@ list yields a list of all related PRs for each release. ## Added - PLOC Supervisor Update: Update SW to use newest PLOC SUPV version by TAS - PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/316 + PR 1: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/316 + PR 2: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/324 + PR 3: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/326 # [v1.16.0] 18.11.2022 From 0763f11998d9796697d4ca808369187b4db2e3da Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 18:21:53 +0100 Subject: [PATCH 216/244] revert baud rate --- common/config/devConf.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/config/devConf.h b/common/config/devConf.h index beb01282..931d1ad1 100644 --- a/common/config/devConf.h +++ b/common/config/devConf.h @@ -60,7 +60,7 @@ static constexpr UartBaudRate SYRLINKS_BAUD = UartBaudRate::RATE_38400; static constexpr UartBaudRate SCEX_BAUD = UartBaudRate::RATE_115200; static constexpr UartBaudRate GNSS_BAUD = UartBaudRate::RATE_9600; static constexpr UartBaudRate PLOC_MPSOC_BAUD = UartBaudRate::RATE_115200; -static constexpr UartBaudRate PLOC_SUPV_BAUD = UartBaudRate::RATE_921600; +static constexpr UartBaudRate PLOC_SUPV_BAUD = UartBaudRate::RATE_115200; static constexpr UartBaudRate STAR_TRACKER_BAUD = UartBaudRate::RATE_921600; } // namespace uart From 87ab84c649559f4bfcfcfb2cf78d69ccbe7f2edd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 18:23:55 +0100 Subject: [PATCH 217/244] bump changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index ba0e0641..52b2d1a0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,12 @@ list yields a list of all related PRs for each release. # [unreleased] +## Changed + +- PLOC Supervisor: Changes baudrate to 921600 + +# [v1.17.0] 28.11.2022 + ## Added - PLOC Supervisor Update: Update SW to use newest PLOC SUPV version by TAS From 5ba5e69273223726d3286db17c2c1afcc5275d36 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 18:25:08 +0100 Subject: [PATCH 218/244] re-run generators --- generators/bsp_q7s_events.csv | 2 +- generators/bsp_q7s_returnvalues.csv | 660 +++++++++--------- generators/events/translateEvents.cpp | 6 +- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 6 +- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 7 files changed, 340 insertions(+), 340 deletions(-) diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 9a89eca9..e56b0ed6 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -129,7 +129,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 12005;0x2ee5;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report P1: ID of command for which the execution failed P2: Status code sent by the supervisor handler;linux/devices/ploc/PlocSupervisorHandler.h 12006;0x2ee6;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;linux/devices/ploc/PlocSupervisorHandler.h 12007;0x2ee7;SUPV_HELPER_EXECUTING;LOW;Supervisor helper currently executing a command;linux/devices/ploc/PlocSupervisorHandler.h -12008;0x2ee8;SUPV_MPSOC_SHUWDOWN_BUILD_FAILED;LOW;Failed to build the command to shutdown the MPSoC;linux/devices/ploc/PlocSupervisorHandler.h +12008;0x2ee8;SUPV_MPSOC_SHUTDOWN_BUILD_FAILED;LOW;Failed to build the command to shutdown the MPSoC;linux/devices/ploc/PlocSupervisorHandler.h 12100;0x2f44;SANITIZATION_FAILED;LOW;;bsp_q7s/fs/SdCardManager.h 12101;0x2f45;MOUNTED_SD_CARD;INFO;;bsp_q7s/fs/SdCardManager.h 12300;0x300c;SEND_MRAM_DUMP_FAILED;LOW;Failed to send mram dump command to supervisor handler P1: Return value of commandAction function P2: Start address of MRAM to dump with this command;linux/devices/ploc/PlocMemoryDumper.h diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 72c805b5..e64eed14 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -1,22 +1,13 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h -0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h -0x58a0;SUSS_ErrorUnlockMutex;;160;SUS_HANDLER;mission/devices/SusHandler.h -0x58a1;SUSS_ErrorLockMutex;;161;SUS_HANDLER;mission/devices/SusHandler.h -0x66a0;SADPL_CommandNotSupported;;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a1;SADPL_DeploymentAlreadyExecuting;;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a2;SADPL_MainSwitchTimeoutFailure;;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a3;SADPL_SwitchingDeplSa1Failed;;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a4;SADPL_SwitchingDeplSa2Failed;;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x51a0;IMTQ_InvalidCommandCode;;160;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a1;IMTQ_ParameterMissing;;161;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a2;IMTQ_ParameterInvalid;;162;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a3;IMTQ_CcUnavailable;;163;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a4;IMTQ_InternalProcessingError;;164;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a5;IMTQ_RejectedWithoutReason;;165;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a6;IMTQ_CmdErrUnknown;;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h -0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h +0x5d00;GOMS_PacketTooLong;;0;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d01;GOMS_InvalidTableId;;1;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d02;GOMS_InvalidAddress;;2;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d03;GOMS_InvalidParamSize;;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d04;GOMS_InvalidPayloadSize;;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d05;GOMS_UnknownReplyId;;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h 0x52b0;RWHA_SpiWriteFailure;;176;RW_HANDLER;mission/devices/RwHandler.h 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/RwHandler.h 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/RwHandler.h @@ -29,12 +20,21 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x52a2;RWHA_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;RW_HANDLER;mission/devices/RwHandler.h 0x52a3;RWHA_ExecutionFailed;Command execution failed;163;RW_HANDLER;mission/devices/RwHandler.h 0x52a4;RWHA_CrcError;Reaction wheel reply has invalid crc;164;RW_HANDLER;mission/devices/RwHandler.h -0x5d00;GOMS_PacketTooLong;;0;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d01;GOMS_InvalidTableId;;1;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d02;GOMS_InvalidAddress;;2;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d03;GOMS_InvalidParamSize;;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d04;GOMS_InvalidPayloadSize;;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d05;GOMS_UnknownReplyId;;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x4fa1;HEATER_CommandNotSupported;;161;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa2;HEATER_InitFailed;;162;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa3;HEATER_InvalidSwitchNr;;163;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa4;HEATER_MainSwitchSetTimeout;;164;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x4fa5;HEATER_CommandAlreadyWaiting;;165;HEATER_HANDLER;mission/devices/HeaterHandler.h +0x58a0;SUSS_ErrorUnlockMutex;;160;SUS_HANDLER;mission/devices/SusHandler.h +0x58a1;SUSS_ErrorLockMutex;;161;SUS_HANDLER;mission/devices/SusHandler.h +0x51a0;IMTQ_InvalidCommandCode;;160;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a1;IMTQ_ParameterMissing;;161;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a2;IMTQ_ParameterInvalid;;162;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a3;IMTQ_CcUnavailable;;163;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a4;IMTQ_InternalProcessingError;;164;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a5;IMTQ_RejectedWithoutReason;;165;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a6;IMTQ_CmdErrUnknown;;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h +0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.h 0x50a0;SYRLINKS_CrcFailure;;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h 0x50a2;SYRLINKS_BadCharacterAck;;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h @@ -44,111 +44,130 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x50a6;SYRLINKS_BadCrcAck;;166;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h 0x50a7;SYRLINKS_ReplyWrongSize;;167;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h 0x50a8;SYRLINKS_MissingStartFrameCharacter;;168;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h -0x4fa1;HEATER_CommandNotSupported;;161;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa2;HEATER_InitFailed;;162;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa3;HEATER_InvalidSwitchNr;;163;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa4;HEATER_MainSwitchSetTimeout;;164;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x4fa5;HEATER_CommandAlreadyWaiting;;165;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h -0x4500;HSPI_OpeningFileFailed;;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4501;HSPI_FullDuplexTransferFailed;;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4502;HSPI_HalfDuplexTransferFailed;;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h -0x4801;HGIO_UnknownGpioId;;1;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4802;HGIO_DriveGpioFailure;;2;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4803;HGIO_GpioTypeFailure;;3;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4804;HGIO_GpioInvalidInstance;;4;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4805;HGIO_GpioDuplicateDetected;;5;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4806;HGIO_GpioInitFailed;;6;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4807;HGIO_GpioGetValueFailed;;7;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h -0x4601;HURT_UartReadFailure;;1;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h -0x4602;HURT_UartReadSizeMissmatch;;2;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h -0x4603;HURT_UartRxBufferTooSmall;;3;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h -0x4400;UXOS_ExecutionFinished;Execution of the current command has finished;0;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4401;UXOS_CommandPending;Command is pending. This will also be returned if the user tries to load another command but a command is still pending;1;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4402;UXOS_BytesRead;Some bytes have been read from the executing process;2;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4403;UXOS_CommandError;Command execution failed;3;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4404;UXOS_NoCommandLoadedOrPending;;4;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x4406;UXOS_PcloseCallError;;6;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x2801;SM_DataTooLarge;;1;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2802;SM_DataStorageFull;;2;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2803;SM_IllegalStorageId;;3;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2804;SM_DataDoesNotExist;;4;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2805;SM_IllegalAddress;;5;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2806;SM_PoolTooLarge;;6;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x0601;PP_DoItMyself;;1;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0602;PP_PointsToVariable;;2;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0603;PP_PointsToMemory;;3;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0604;PP_ActivityCompleted;;4;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0605;PP_PointsToVectorUint8;;5;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0606;PP_PointsToVectorUint16;;6;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0607;PP_PointsToVectorUint32;;7;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x0608;PP_PointsToVectorFloat;;8;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06a0;PP_DumpNotSupported;;160;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e0;PP_InvalidSize;;224;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e1;PP_InvalidAddress;;225;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e2;PP_InvalidContent;;226;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e3;PP_UnalignedAccess;;227;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x06e4;PP_WriteProtected;;228;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x13e0;MH_UnknownCmd;;224;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e1;MH_InvalidAddress;;225;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e2;MH_InvalidSize;;226;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e3;MH_StateMismatch;;227;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x38a1;SGP4_InvalidEccentricity;;161;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a2;SGP4_InvalidMeanMotion;;162;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a3;SGP4_InvalidPerturbationElements;;163;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a4;SGP4_InvalidSemiLatusRectum;;164;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a5;SGP4_InvalidEpochElements;;165;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38a6;SGP4_SatelliteHasDecayed;;166;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38b1;SGP4_TleTooOld;;177;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x38b2;SGP4_TleNotInitialized;;178;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x1801;FF_Full;;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h -0x1802;FF_Empty;;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h -0x1601;FMM_MapFull;;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h -0x1602;FMM_KeyDoesNotExist;;2;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h -0x3901;MUX_NotEnoughResources;;1;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3902;MUX_InsufficientMemory;;2;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3903;MUX_NoPrivilege;;3;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3904;MUX_WrongAttributeSetting;;4;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3905;MUX_MutexAlreadyLocked;;5;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3906;MUX_MutexNotFound;;6;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3907;MUX_MutexMaxLocks;;7;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3908;MUX_CurrThreadAlreadyOwnsMutex;;8;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3909;MUX_CurrThreadDoesNotOwnMutex;;9;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390a;MUX_MutexTimeout;;10;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390b;MUX_MutexInvalidId;;11;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390c;MUX_MutexDestroyedWhileWaiting;;12;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3a01;MQI_Empty;;1;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a02;MQI_Full;No space left for more messages;2;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a03;MQI_NoReplyPartner;Returned if a reply method was called without partner;3;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a04;MQI_DestinationInvalid;Returned if the target destination is invalid.;4;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x0f01;CM_UnknownCommand;;1;COMMAND_MESSAGE;fsfw/src/fsfw/ipc/CommandMessageIF.h +0x66a0;SADPL_CommandNotSupported;;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a1;SADPL_DeploymentAlreadyExecuting;;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a2;SADPL_MainSwitchTimeoutFailure;;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a3;SADPL_SwitchingDeplSa1Failed;;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a4;SADPL_SwitchingDeplSa2Failed;;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h +0x2c01;CCS_BcIsSetVrCommand;;1;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2c02;CCS_BcIsUnlockCommand;;2;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cb0;CCS_BcIllegalCommand;;176;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cb1;CCS_BoardReadingNotFinished;;177;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf0;CCS_NsPositiveW;;240;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf1;CCS_NsNegativeW;;241;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf2;CCS_NsLockout;;242;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf3;CCS_FarmInLockout;;243;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf4;CCS_FarmInWait;;244;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce0;CCS_WrongSymbol;;224;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce1;CCS_DoubleStart;;225;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce2;CCS_StartSymbolMissed;;226;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce3;CCS_EndWithoutStart;;227;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce4;CCS_TooLarge;;228;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce5;CCS_TooShort;;229;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce6;CCS_WrongTfVersion;;230;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce7;CCS_WrongSpacecraftId;;231;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce8;CCS_NoValidFrameType;;232;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce9;CCS_CrcFailed;;233;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cea;CCS_VcNotFound;;234;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ceb;CCS_ForwardingFailed;;235;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cec;CCS_ContentTooLarge;;236;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ced;CCS_ResidualData;;237;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cee;CCS_DataCorrupted;;238;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cef;CCS_IllegalSegmentationFlag;;239;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd0;CCS_IllegalFlagCombination;;208;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd1;CCS_ShorterThanHeader;;209;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd2;CCS_TooShortBlockedPacket;;210;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd3;CCS_TooShortMapExtraction;;211;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x3b00;SPH_ConnBroken;;0;SEMAPHORE_IF;fsfw/src/fsfw/osal/common/TcpTmTcServer.h +0x2a01;IEC_NoConfigurationTable;;1;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a02;IEC_NoCpuTable;;2;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a03;IEC_InvalidWorkspaceAddress;;3;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a04;IEC_TooLittleWorkspace;;4;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a05;IEC_WorkspaceAllocation;;5;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a06;IEC_InterruptStackTooSmall;;6;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a07;IEC_ThreadExitted;;7;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a08;IEC_InconsistentMpInformation;;8;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a09;IEC_InvalidNode;;9;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0a;IEC_NoMpci;;10;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0b;IEC_BadPacket;;11;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0c;IEC_OutOfPackets;;12;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0d;IEC_OutOfGlobalObjects;;13;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0e;IEC_OutOfProxies;;14;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a0f;IEC_InvalidGlobalId;;15;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a10;IEC_BadStackHook;;16;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a11;IEC_BadAttributes;;17;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a12;IEC_ImplementationKeyCreateInconsistency;;18;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a13;IEC_ImplementationBlockingOperationCancel;;19;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a14;IEC_MutexObtainFromBadState;;20;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h +0x2a15;IEC_UnlimitedAndMaximumIs0;;21;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h 0x0e01;HM_InvalidMode;;1;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e02;HM_TransNotAllowed;;2;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e03;HM_InTransition;;3;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h 0x0e04;HM_InvalidSubmode;;4;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h -0x0c02;MS_InvalidEntry;;2;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0c03;MS_TooManyElements;;3;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0c04;MS_CantStoreEmpty;;4;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0b01;SB_ChildNotFound;;1;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b02;SB_ChildInfoUpdated;;2;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b03;SB_ChildDoesntHaveModes;;3;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b04;SB_CouldNotInsertChild;;4;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b05;SB_TableContainsInvalidObjectId;;5;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0d01;SS_SequenceAlreadyExists;;1;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d02;SS_TableAlreadyExists;;2;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d03;SS_TableDoesNotExist;;3;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d04;SS_TableOrSequenceLengthInvalid;;4;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d05;SS_SequenceDoesNotExist;;5;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d06;SS_TableContainsInvalidObjectId;;6;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d07;SS_FallbackSequenceDoesNotExist;;7;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d08;SS_NoTargetTable;;8;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d09;SS_SequenceOrTableTooLong;;9;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0b;SS_IsFallbackSequence;;11;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0c;SS_AccessDenied;;12;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0e;SS_TableInUse;;14;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0da1;SS_TargetTableNotReached;;161;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0da2;SS_TableCheckFailed;;162;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x2501;EV_ListenerNotFound;;1;EVENT_MANAGER_IF;fsfw/src/fsfw/events/EventManagerIF.h +0x2e01;HPA_InvalidIdentifierId;;1;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e02;HPA_InvalidDomainId;;2;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e03;HPA_InvalidValue;;3;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e05;HPA_ReadOnly;;5;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2d01;PAW_UnknownDatatype;;1;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d02;PAW_DatatypeMissmatch;;2;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d03;PAW_Readonly;;3;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d04;PAW_TooBig;;4;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d05;PAW_SourceNotSet;;5;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d06;PAW_OutOfBounds;;6;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d07;PAW_NotSet;;7;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d08;PAW_ColumnOrRowsZero;;8;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x3201;CF_ObjectHasNoFunctions;;1;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h +0x3202;CF_AlreadyCommanding;;2;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h +0x3301;HF_IsBusy;;1;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3302;HF_InvalidParameters;;2;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3303;HF_ExecutionFinished;;3;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3304;HF_InvalidActionId;;4;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x0201;OM_InsertionFailed;;1;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0202;OM_NotFound;;2;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0203;OM_ChildInitFailed;;3;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0204;OM_InternalErrReporterUninit;;4;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x2600;FDI_YourFault;;0;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2601;FDI_MyFault;;1;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2602;FDI_ConfirmLater;;2;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h +0x2201;TMF_Busy;;1;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2202;TMF_LastPacketFound;;2;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2203;TMF_StopFetch;;3;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2204;TMF_Timeout;;4;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2205;TMF_TmChannelFull;;5;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2206;TMF_NotStored;;6;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2207;TMF_AllDeleted;;7;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2208;TMF_InvalidData;;8;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2209;TMF_NotReady;;9;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2101;TMB_Busy;;1;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2102;TMB_Full;;2;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2103;TMB_Empty;;3;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2104;TMB_NullRequested;;4;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2105;TMB_TooLarge;;5;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2106;TMB_NotReady;;6;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2107;TMB_DumpError;;7;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2108;TMB_CrcError;;8;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2109;TMB_Timeout;;9;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210a;TMB_IdlePacketFound;;10;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210b;TMB_TelecommandFound;;11;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210c;TMB_NoPusATm;;12;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210d;TMB_TooSmall;;13;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210e;TMB_BlockNotFound;;14;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210f;TMB_InvalidRequest;;15;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x1c01;TCD_PacketLost;;1;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1c02;TCD_DestinationNotFound;;2;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1c03;TCD_ServiceIdAlreadyExists;;3;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1b00;TCC_NoDestinationFound;;0;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b01;TCC_InvalidCcsdsVersion;;1;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b02;TCC_InvalidApid;;2;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b03;TCC_InvalidPacketType;;3;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b04;TCC_InvalidSecHeaderField;;4;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b05;TCC_IncorrectPrimaryHeader;;5;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b07;TCC_IncompletePacket;;7;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b08;TCC_InvalidPusVersion;;8;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b09;TCC_IncorrectChecksum;;9;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b0a;TCC_IllegalPacketSubtype;;10;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b0b;TCC_IncorrectSecondaryHeader;;11;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h 0x04e1;RMP_CommandNoDescriptorsAvailable;;225;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x04e2;RMP_CommandBufferFull;;226;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x04e3;RMP_CommandChannelOutOfRange;;227;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h @@ -189,9 +208,95 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x040a;RMP_ReplyCommandNotImplementedOrNotAuthorised;;10;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x040b;RMP_ReplyRmwDataLengthError;;11;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x040c;RMP_ReplyInvalidTargetLogicalAddress;;12;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h -0x1401;SE_BufferTooShort;;1;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x1402;SE_StreamTooShort;;2;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x1403;SE_TooManyElements;;3;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x2801;SM_DataTooLarge;;1;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2802;SM_DataStorageFull;;2;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2803;SM_IllegalStorageId;;3;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2804;SM_DataDoesNotExist;;4;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2805;SM_IllegalAddress;;5;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2806;SM_PoolTooLarge;;6;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x38a1;SGP4_InvalidEccentricity;;161;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a2;SGP4_InvalidMeanMotion;;162;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a3;SGP4_InvalidPerturbationElements;;163;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a4;SGP4_InvalidSemiLatusRectum;;164;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a5;SGP4_InvalidEpochElements;;165;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38a6;SGP4_SatelliteHasDecayed;;166;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38b1;SGP4_TleTooOld;;177;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x38b2;SGP4_TleNotInitialized;;178;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h +0x2401;MT_NoPacketFound;;1;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/DleParser.h +0x2402;MT_PossiblePacketLoss;;2;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/DleParser.h +0x2403;MT_NoMatch;;3;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2404;MT_Full;;4;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2405;MT_NewNodeCreated;;5;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x3f01;DLEE_StreamTooShort;;1;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h +0x3f02;DLEE_DecodingError;;2;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h +0x2f01;ASC_TooLongForTargetType;;1;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x2f02;ASC_InvalidCharacters;;2;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x2f03;ASC_BufferTooSmall;;3;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x0f01;CM_UnknownCommand;;1;COMMAND_MESSAGE;fsfw/src/fsfw/ipc/CommandMessageIF.h +0x3a01;MQI_Empty;;1;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a02;MQI_Full;No space left for more messages;2;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a03;MQI_NoReplyPartner;Returned if a reply method was called without partner;3;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a04;MQI_DestinationInvalid;Returned if the target destination is invalid.;4;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3901;MUX_NotEnoughResources;;1;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3902;MUX_InsufficientMemory;;2;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3903;MUX_NoPrivilege;;3;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3904;MUX_WrongAttributeSetting;;4;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3905;MUX_MutexAlreadyLocked;;5;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3906;MUX_MutexNotFound;;6;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3907;MUX_MutexMaxLocks;;7;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3908;MUX_CurrThreadAlreadyOwnsMutex;;8;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3909;MUX_CurrThreadDoesNotOwnMutex;;9;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390a;MUX_MutexTimeout;;10;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390b;MUX_MutexInvalidId;;11;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390c;MUX_MutexDestroyedWhileWaiting;;12;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3b01;SPH_SemaphoreTimeout;;1;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x3b02;SPH_SemaphoreNotOwned;;2;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x3b03;SPH_SemaphoreInvalid;;3;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x1e00;PUS_InvalidPusVersion;;0;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h +0x1e01;PUS_InvalidCrc16;;1;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h +0x3601;CFDP_InvalidTlvType;;1;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3602;CFDP_InvalidDirectiveField;;2;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3603;CFDP_InvalidPduDatafieldLen;;3;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3604;CFDP_InvalidAckDirectiveFields;;4;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3605;CFDP_MetadataCantParseOptions;;5;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3606;CFDP_NakCantParseOptions;;6;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3607;CFDP_FinishedCantParseFsResponses;;7;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3608;CFDP_FilestoreRequiresSecondFile;;8;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3609;CFDP_FilestoreResponseCantParseFsMessage;;9;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x360a;CFDP_InvalidPduFormat;;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x2901;TC_InvalidTargetState;;1;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x29f1;TC_AboveOperationalLimit;;241;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x29f2;TC_BelowOperationalLimit;;242;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x0c02;MS_InvalidEntry;;2;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0c03;MS_TooManyElements;;3;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0c04;MS_CantStoreEmpty;;4;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0d01;SS_SequenceAlreadyExists;;1;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d02;SS_TableAlreadyExists;;2;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d03;SS_TableDoesNotExist;;3;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d04;SS_TableOrSequenceLengthInvalid;;4;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d05;SS_SequenceDoesNotExist;;5;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d06;SS_TableContainsInvalidObjectId;;6;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d07;SS_FallbackSequenceDoesNotExist;;7;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d08;SS_NoTargetTable;;8;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d09;SS_SequenceOrTableTooLong;;9;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0b;SS_IsFallbackSequence;;11;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0c;SS_AccessDenied;;12;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0e;SS_TableInUse;;14;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0da1;SS_TargetTableNotReached;;161;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0da2;SS_TableCheckFailed;;162;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0b01;SB_ChildNotFound;;1;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b02;SB_ChildInfoUpdated;;2;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b03;SB_ChildDoesntHaveModes;;3;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b04;SB_CouldNotInsertChild;;4;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b05;SB_TableContainsInvalidObjectId;;5;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x3e00;HKM_QueueOrDestinationInvalid;;0;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e01;HKM_WrongHkPacketType;;1;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e02;HKM_ReportingStatusUnchanged;;2;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e03;HKM_PeriodicHelperInvalid;;3;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e04;HKM_PoolobjectNotFound;;4;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e05;HKM_DatasetNotFound;;5;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3c00;LPIF_PoolEntryNotFound;;0;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h +0x3c01;LPIF_PoolEntryTypeConflict;;1;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h 0x3da0;PVA_InvalidReadWriteMode;;160;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h 0x3da1;PVA_InvalidPoolEntry;;161;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h 0x0801;DPS_InvalidParameterDefinition;;1;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h @@ -200,20 +305,35 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0804;DPS_DataSetUninitialised;;4;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h 0x0805;DPS_DataSetFull;;5;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h 0x0806;DPS_PoolVarNull;;6;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x1c01;TCD_PacketLost;;1;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1c02;TCD_DestinationNotFound;;2;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1c03;TCD_ServiceIdAlreadyExists;;3;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1b00;TCC_NoDestinationFound;;0;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b01;TCC_InvalidCcsdsVersion;;1;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b02;TCC_InvalidApid;;2;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b03;TCC_InvalidPacketType;;3;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b04;TCC_InvalidSecHeaderField;;4;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b05;TCC_IncorrectPrimaryHeader;;5;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b07;TCC_IncompletePacket;;7;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b08;TCC_InvalidPusVersion;;8;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b09;TCC_IncorrectChecksum;;9;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b0a;TCC_IllegalPacketSubtype;;10;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b0b;TCC_IncorrectSecondaryHeader;;11;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1000;TIM_UnsupportedTimeFormat;;0;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1001;TIM_NotEnoughInformationForTargetFormat;;1;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1002;TIM_LengthMismatch;;2;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1003;TIM_InvalidTimeFormat;;3;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1004;TIM_InvalidDayOfYear;;4;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1005;TIM_TimeDoesNotFitFormat;;5;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x3701;TSI_BadTimestamp;;1;TIME_STAMPER_IF;fsfw/src/fsfw/timemanager/TimeStampIF.h +0x1d01;ATC_ActivityStarted;;1;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d02;ATC_InvalidSubservice;;2;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d03;ATC_IllegalApplicationData;;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d04;ATC_SendTmFailed;;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d05;ATC_Timeout;;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x4c00;SPPA_NoPacketFound;;0;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h +0x4c01;SPPA_SplitPacket;;1;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h +0x2001;CSB_ExecutionComplete;;1;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2002;CSB_NoStepMessage;;2;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2003;CSB_ObjectBusy;;3;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2004;CSB_Busy;;4;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2005;CSB_InvalidTc;;5;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2006;CSB_InvalidObject;;6;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2007;CSB_InvalidReply;;7;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x1801;FF_Full;;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h +0x1802;FF_Empty;;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h +0x1601;FMM_MapFull;;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h +0x1602;FMM_KeyDoesNotExist;;2;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h +0x2501;EV_ListenerNotFound;;1;EVENT_MANAGER_IF;fsfw/src/fsfw/events/EventManagerIF.h +0x1701;HHI_ObjectNotHealthy;;1;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x1702;HHI_InvalidHealthState;;2;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x1703;HHI_IsExternallyControlled;;3;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h 0x3001;POS_InPowerTransition;;1;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h 0x3002;POS_SwitchStateMismatch;;2;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h 0x0501;PS_SwitchOn;;1;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h @@ -221,76 +341,23 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0502;PS_SwitchTimeout;;2;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h 0x0503;PS_FuseOn;;3;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h 0x0504;PS_FuseOff;;4;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h -0x3b00;SPH_ConnBroken;;0;SEMAPHORE_IF;fsfw/src/fsfw/osal/common/TcpTmTcServer.h -0x2a01;IEC_NoConfigurationTable;;1;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a02;IEC_NoCpuTable;;2;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a03;IEC_InvalidWorkspaceAddress;;3;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a04;IEC_TooLittleWorkspace;;4;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a05;IEC_WorkspaceAllocation;;5;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a06;IEC_InterruptStackTooSmall;;6;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a07;IEC_ThreadExitted;;7;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a08;IEC_InconsistentMpInformation;;8;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a09;IEC_InvalidNode;;9;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0a;IEC_NoMpci;;10;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0b;IEC_BadPacket;;11;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0c;IEC_OutOfPackets;;12;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0d;IEC_OutOfGlobalObjects;;13;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0e;IEC_OutOfProxies;;14;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a0f;IEC_InvalidGlobalId;;15;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a10;IEC_BadStackHook;;16;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a11;IEC_BadAttributes;;17;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a12;IEC_ImplementationKeyCreateInconsistency;;18;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a13;IEC_ImplementationBlockingOperationCancel;;19;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a14;IEC_MutexObtainFromBadState;;20;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2a15;IEC_UnlimitedAndMaximumIs0;;21;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x2600;FDI_YourFault;;0;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x2601;FDI_MyFault;;1;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x2602;FDI_ConfirmLater;;2;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x1e00;PUS_InvalidPusVersion;;0;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h -0x1e01;PUS_InvalidCrc16;;1;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h -0x0201;OM_InsertionFailed;;1;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0202;OM_NotFound;;2;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0203;OM_ChildInitFailed;;3;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0204;OM_InternalErrReporterUninit;;4;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x2201;TMF_Busy;;1;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2202;TMF_LastPacketFound;;2;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2203;TMF_StopFetch;;3;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2204;TMF_Timeout;;4;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2205;TMF_TmChannelFull;;5;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2206;TMF_NotStored;;6;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2207;TMF_AllDeleted;;7;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2208;TMF_InvalidData;;8;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2209;TMF_NotReady;;9;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2101;TMB_Busy;;1;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2102;TMB_Full;;2;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2103;TMB_Empty;;3;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2104;TMB_NullRequested;;4;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2105;TMB_TooLarge;;5;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2106;TMB_NotReady;;6;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2107;TMB_DumpError;;7;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2108;TMB_CrcError;;8;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2109;TMB_Timeout;;9;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210a;TMB_IdlePacketFound;;10;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210b;TMB_TelecommandFound;;11;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210c;TMB_NoPusATm;;12;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210d;TMB_TooSmall;;13;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210e;TMB_BlockNotFound;;14;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210f;TMB_InvalidRequest;;15;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2d01;PAW_UnknownDatatype;;1;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d02;PAW_DatatypeMissmatch;;2;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d03;PAW_Readonly;;3;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d04;PAW_TooBig;;4;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d05;PAW_SourceNotSet;;5;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d06;PAW_OutOfBounds;;6;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d07;PAW_NotSet;;7;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d08;PAW_ColumnOrRowsZero;;8;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2e01;HPA_InvalidIdentifierId;;1;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e02;HPA_InvalidDomainId;;2;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e03;HPA_InvalidValue;;3;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e05;HPA_ReadOnly;;5;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x3b01;SPH_SemaphoreTimeout;;1;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x3b02;SPH_SemaphoreNotOwned;;2;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x3b03;SPH_SemaphoreInvalid;;3;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x4300;FILS_GenericFileError;;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4301;FILS_GenericDirError;;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4302;FILS_FilesystemInactive;;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4303;FILS_GenericRenameError;;3;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4304;FILS_IsBusy;;4;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4305;FILS_InvalidParameters;;5;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430a;FILS_FileDoesNotExist;;10;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430b;FILS_FileAlreadyExists;;11;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430c;FILS_NotAFile;;12;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430d;FILS_FileLocked;;13;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x430e;FILS_PermissionDenied;;14;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4315;FILS_DirectoryDoesNotExist;;21;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4316;FILS_DirectoryAlreadyExists;;22;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4317;FILS_NotADirectory;;23;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x4318;FILS_DirectoryNotEmpty;;24;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x431e;FILS_SequencePacketMissingWrite;;30;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h +0x431f;FILS_SequencePacketMissingRead;;31;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x1a01;TRC_NotEnoughSensors;;1;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h 0x1a02;TRC_LowestValueOol;;2;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h 0x1a03;TRC_HighestValueOol;;3;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h @@ -309,72 +376,34 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x31e2;LIM_WrongPid;;226;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h 0x31e3;LIM_WrongLimitId;;227;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h 0x31ee;LIM_MonitorNotFound;;238;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3601;CFDP_InvalidTlvType;;1;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3602;CFDP_InvalidDirectiveField;;2;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3603;CFDP_InvalidPduDatafieldLen;;3;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3604;CFDP_InvalidAckDirectiveFields;;4;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3605;CFDP_MetadataCantParseOptions;;5;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3606;CFDP_NakCantParseOptions;;6;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3607;CFDP_FinishedCantParseFsResponses;;7;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3608;CFDP_FilestoreRequiresSecondFile;;8;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3609;CFDP_FilestoreResponseCantParseFsMessage;;9;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x360a;CFDP_InvalidPduFormat;;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x4300;FILS_GenericFileError;;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4301;FILS_GenericDirError;;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4302;FILS_FilesystemInactive;;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4303;FILS_GenericRenameError;;3;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4304;FILS_IsBusy;;4;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4305;FILS_InvalidParameters;;5;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430a;FILS_FileDoesNotExist;;10;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430b;FILS_FileAlreadyExists;;11;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430c;FILS_NotAFile;;12;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430d;FILS_FileLocked;;13;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x430e;FILS_PermissionDenied;;14;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4315;FILS_DirectoryDoesNotExist;;21;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4316;FILS_DirectoryAlreadyExists;;22;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4317;FILS_NotADirectory;;23;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x4318;FILS_DirectoryNotEmpty;;24;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x431e;FILS_SequencePacketMissingWrite;;30;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x431f;FILS_SequencePacketMissingRead;;31;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x2c01;CCS_BcIsSetVrCommand;;1;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2c02;CCS_BcIsUnlockCommand;;2;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cb0;CCS_BcIllegalCommand;;176;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cb1;CCS_BoardReadingNotFinished;;177;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf0;CCS_NsPositiveW;;240;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf1;CCS_NsNegativeW;;241;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf2;CCS_NsLockout;;242;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf3;CCS_FarmInLockout;;243;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf4;CCS_FarmInWait;;244;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce0;CCS_WrongSymbol;;224;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce1;CCS_DoubleStart;;225;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce2;CCS_StartSymbolMissed;;226;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce3;CCS_EndWithoutStart;;227;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce4;CCS_TooLarge;;228;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce5;CCS_TooShort;;229;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce6;CCS_WrongTfVersion;;230;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce7;CCS_WrongSpacecraftId;;231;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce8;CCS_NoValidFrameType;;232;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce9;CCS_CrcFailed;;233;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cea;CCS_VcNotFound;;234;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ceb;CCS_ForwardingFailed;;235;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cec;CCS_ContentTooLarge;;236;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ced;CCS_ResidualData;;237;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cee;CCS_DataCorrupted;;238;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cef;CCS_IllegalSegmentationFlag;;239;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd0;CCS_IllegalFlagCombination;;208;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd1;CCS_ShorterThanHeader;;209;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd2;CCS_TooShortBlockedPacket;;210;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd3;CCS_TooShortMapExtraction;;211;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h 0x4201;PUS11_InvalidTypeTimeWindow;;1;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4202;PUS11_InvalidTimeWindow;;2;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4203;PUS11_TimeshiftingNotPossible;;3;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h 0x4204;PUS11_InvalidRelativeTime;;4;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h -0x3401;DC_NoReplyReceived;;1;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3402;DC_ProtocolError;;2;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3403;DC_Nullpointer;;3;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3404;DC_InvalidCookieType;;4;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3405;DC_NotActive;;5;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3406;DC_TooMuchData;;6;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x0601;PP_DoItMyself;;1;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0602;PP_PointsToVariable;;2;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0603;PP_PointsToMemory;;3;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0604;PP_ActivityCompleted;;4;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0605;PP_PointsToVectorUint8;;5;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0606;PP_PointsToVectorUint16;;6;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0607;PP_PointsToVectorUint32;;7;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x0608;PP_PointsToVectorFloat;;8;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06a0;PP_DumpNotSupported;;160;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e0;PP_InvalidSize;;224;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e1;PP_InvalidAddress;;225;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e2;PP_InvalidContent;;226;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e3;PP_UnalignedAccess;;227;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x06e4;PP_WriteProtected;;228;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h +0x13e0;MH_UnknownCmd;;224;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e1;MH_InvalidAddress;;225;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e2;MH_InvalidSize;;226;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e3;MH_StateMismatch;;227;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x1201;AB_NeedSecondStep;;1;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1202;AB_NeedToReconfigure;;2;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1203;AB_ModeFallback;;3;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1204;AB_ChildNotCommandable;;4;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1205;AB_NeedToChangeHealth;;5;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x12a1;AB_NotEnoughChildrenInCorrectState;;161;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h 0x03a0;DHB_InvalidChannel;;160;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03b0;DHB_AperiodicReply;;176;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03b1;DHB_IgnoreReplyData;;177;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -384,12 +413,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x03d0;DHB_NoSwitch;;208;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03e0;DHB_ChildTimeout;;224;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03e1;DHB_SwitchFailed;;225;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h -0x1201;AB_NeedSecondStep;;1;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1202;AB_NeedToReconfigure;;2;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1203;AB_ModeFallback;;3;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1204;AB_ChildNotCommandable;;4;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1205;AB_NeedToChangeHealth;;5;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x12a1;AB_NotEnoughChildrenInCorrectState;;161;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x3401;DC_NoReplyReceived;;1;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3402;DC_ProtocolError;;2;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3403;DC_Nullpointer;;3;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3404;DC_InvalidCookieType;;4;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3405;DC_NotActive;;5;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3406;DC_TooMuchData;;6;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h 0x27a0;DHI_NoCommandData;;160;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27a1;DHI_CommandNotSupported;;161;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27a2;DHI_CommandAlreadySent;;162;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h @@ -411,58 +440,28 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x27c3;DHI_DeviceReplyInvalid;;195;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27d0;DHI_InvalidCommandParameter;;208;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27d1;DHI_InvalidNumberOrLengthOfParameters;;209;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h -0x2401;MT_TooDetailedRequest;;1;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2402;MT_TooGeneralRequest;;2;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2403;MT_NoMatch;;3;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2404;MT_Full;;4;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2405;MT_NewNodeCreated;;5;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x3f01;DLEE_NoPacketFound;;1;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleParser.h -0x3f02;DLEE_PossiblePacketLoss;;2;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleParser.h -0x2f01;ASC_TooLongForTargetType;;1;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x2f02;ASC_InvalidCharacters;;2;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x2f03;ASC_BufferTooSmall;;3;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x1701;HHI_ObjectNotHealthy;;1;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x1702;HHI_InvalidHealthState;;2;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x1703;HHI_IsExternallyControlled;;3;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x3201;CF_ObjectHasNoFunctions;;1;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h -0x3202;CF_AlreadyCommanding;;2;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h -0x3301;HF_IsBusy;;1;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3302;HF_InvalidParameters;;2;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3303;HF_ExecutionFinished;;3;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3304;HF_InvalidActionId;;4;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x1000;TIM_UnsupportedTimeFormat;;0;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1001;TIM_NotEnoughInformationForTargetFormat;;1;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1002;TIM_LengthMismatch;;2;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1003;TIM_InvalidTimeFormat;;3;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1004;TIM_InvalidDayOfYear;;4;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1005;TIM_TimeDoesNotFitFormat;;5;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x3701;TSI_BadTimestamp;;1;TIME_STAMPER_IF;fsfw/src/fsfw/timemanager/TimeStampIF.h -0x3c00;LPIF_PoolEntryNotFound;;0;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h -0x3c01;LPIF_PoolEntryTypeConflict;;1;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h -0x3e00;HKM_QueueOrDestinationInvalid;;0;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e01;HKM_WrongHkPacketType;;1;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e02;HKM_ReportingStatusUnchanged;;2;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e03;HKM_PeriodicHelperInvalid;;3;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e04;HKM_PoolobjectNotFound;;4;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e05;HKM_DatasetNotFound;;5;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x2901;TC_InvalidTargetState;;1;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x29f1;TC_AboveOperationalLimit;;241;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x29f2;TC_BelowOperationalLimit;;242;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x2001;CSB_ExecutionComplete;;1;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2002;CSB_NoStepMessage;;2;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2003;CSB_ObjectBusy;;3;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2004;CSB_Busy;;4;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2005;CSB_InvalidTc;;5;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2006;CSB_InvalidObject;;6;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2007;CSB_InvalidReply;;7;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x4c00;SPPA_NoPacketFound;;0;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h -0x4c01;SPPA_SplitPacket;;1;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h -0x1d01;ATC_ActivityStarted;;1;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d02;ATC_InvalidSubservice;;2;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d03;ATC_IllegalApplicationData;;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d04;ATC_SendTmFailed;;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d05;ATC_Timeout;;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x6b00;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h +0x1401;SE_BufferTooShort;;1;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x1402;SE_StreamTooShort;;2;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x1403;SE_TooManyElements;;3;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x4500;HSPI_HalTimeoutRetval;;0;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4501;HSPI_HalBusyRetval;;1;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4502;HSPI_HalErrorRetval;;2;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x4601;HURT_UartReadFailure;;1;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4602;HURT_UartReadSizeMissmatch;;2;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4603;HURT_UartRxBufferTooSmall;;3;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h +0x4801;HGIO_UnknownGpioId;;1;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4802;HGIO_DriveGpioFailure;;2;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4803;HGIO_GpioTypeFailure;;3;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4804;HGIO_GpioInvalidInstance;;4;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4805;HGIO_GpioDuplicateDetected;;5;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4806;HGIO_GpioInitFailed;;6;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4807;HGIO_GpioGetValueFailed;;7;HAL_GPIO;fsfw/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +0x4400;UXOS_ExecutionFinished;Execution of the current command has finished;0;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4401;UXOS_CommandPending;Command is pending. This will also be returned if the user tries to load another command but a command is still pending;1;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4402;UXOS_BytesRead;Some bytes have been read from the executing process;2;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4403;UXOS_CommandError;Command execution failed;3;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4404;UXOS_NoCommandLoadedOrPending;;4;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h +0x4406;UXOS_PcloseCallError;;6;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h 0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x6a00;SDMA_OpOngoing;;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h @@ -475,3 +474,4 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a0d;SDMA_UnmountError;;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6a0e;SDMA_SystemCallError;;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6a0f;SDMA_PopenCallError;;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b00;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index ac00745c..fdcc4488 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 239 translations. * @details - * Generated on: 2022-11-16 15:25:08 + * Generated on: 2022-11-28 18:24:37 */ #include "translateEvents.h" @@ -135,7 +135,7 @@ const char *SUPV_ACK_FAILURE_STRING = "SUPV_ACK_FAILURE"; const char *SUPV_EXE_FAILURE_STRING = "SUPV_EXE_FAILURE"; const char *SUPV_CRC_FAILURE_EVENT_STRING = "SUPV_CRC_FAILURE_EVENT"; const char *SUPV_HELPER_EXECUTING_STRING = "SUPV_HELPER_EXECUTING"; -const char *SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUWDOWN_BUILD_FAILED"; +const char *SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUTDOWN_BUILD_FAILED"; const char *SANITIZATION_FAILED_STRING = "SANITIZATION_FAILED"; const char *MOUNTED_SD_CARD_STRING = "MOUNTED_SD_CARD"; const char *SEND_MRAM_DUMP_FAILED_STRING = "SEND_MRAM_DUMP_FAILED"; @@ -504,7 +504,7 @@ const char *translateEvents(Event event) { case (12007): return SUPV_HELPER_EXECUTING_STRING; case (12008): - return SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING; + return SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING; case (12100): return SANITIZATION_FAILED_STRING; case (12101): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 0890725e..f6e58aab 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2022-11-16 15:25:08 + * Generated on: 2022-11-28 18:24:37 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index ac00745c..fdcc4488 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 239 translations. * @details - * Generated on: 2022-11-16 15:25:08 + * Generated on: 2022-11-28 18:24:37 */ #include "translateEvents.h" @@ -135,7 +135,7 @@ const char *SUPV_ACK_FAILURE_STRING = "SUPV_ACK_FAILURE"; const char *SUPV_EXE_FAILURE_STRING = "SUPV_EXE_FAILURE"; const char *SUPV_CRC_FAILURE_EVENT_STRING = "SUPV_CRC_FAILURE_EVENT"; const char *SUPV_HELPER_EXECUTING_STRING = "SUPV_HELPER_EXECUTING"; -const char *SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUWDOWN_BUILD_FAILED"; +const char *SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUTDOWN_BUILD_FAILED"; const char *SANITIZATION_FAILED_STRING = "SANITIZATION_FAILED"; const char *MOUNTED_SD_CARD_STRING = "MOUNTED_SD_CARD"; const char *SEND_MRAM_DUMP_FAILED_STRING = "SEND_MRAM_DUMP_FAILED"; @@ -504,7 +504,7 @@ const char *translateEvents(Event event) { case (12007): return SUPV_HELPER_EXECUTING_STRING; case (12008): - return SUPV_MPSOC_SHUWDOWN_BUILD_FAILED_STRING; + return SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING; case (12100): return SANITIZATION_FAILED_STRING; case (12101): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 0890725e..f6e58aab 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2022-11-16 15:25:08 + * Generated on: 2022-11-28 18:24:37 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index a47ca5c9..12f7b5bb 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a47ca5c901e340f4c07bbb778dd63bb61554dbc4 +Subproject commit 12f7b5bb258f183b62a2ed27776d2385a728306e From 44ef698c3520a9251fb5745702bfb0256b2d9ca3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 18:27:59 +0100 Subject: [PATCH 219/244] bump version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 765f1fc9..5111b9ed 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR_IF_GIT_FAILS 1) -set(OBSW_VERSION_MINOR_IF_GIT_FAILS 16) +set(OBSW_VERSION_MINOR_IF_GIT_FAILS 17) set(OBSW_VERSION_REVISION_IF_GIT_FAILS 0) # set(CMAKE_VERBOSE TRUE) From b0f000a42f3f6b44aa0ababf205d94ed3cd4b312 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 18:31:44 +0100 Subject: [PATCH 220/244] new PLOC SUPV baudrate 921600 --- common/config/devConf.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/config/devConf.h b/common/config/devConf.h index 931d1ad1..beb01282 100644 --- a/common/config/devConf.h +++ b/common/config/devConf.h @@ -60,7 +60,7 @@ static constexpr UartBaudRate SYRLINKS_BAUD = UartBaudRate::RATE_38400; static constexpr UartBaudRate SCEX_BAUD = UartBaudRate::RATE_115200; static constexpr UartBaudRate GNSS_BAUD = UartBaudRate::RATE_9600; static constexpr UartBaudRate PLOC_MPSOC_BAUD = UartBaudRate::RATE_115200; -static constexpr UartBaudRate PLOC_SUPV_BAUD = UartBaudRate::RATE_115200; +static constexpr UartBaudRate PLOC_SUPV_BAUD = UartBaudRate::RATE_921600; static constexpr UartBaudRate STAR_TRACKER_BAUD = UartBaudRate::RATE_921600; } // namespace uart From ffbf7df55792738c10666b0e0ebe43406d6b3980 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 29 Nov 2022 11:05:19 +0100 Subject: [PATCH 221/244] rename ploc supv and i2c dev symlinks --- bsp_q7s/boardconfig/busConf.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/bsp_q7s/boardconfig/busConf.h b/bsp_q7s/boardconfig/busConf.h index 7b50af4e..a094c029 100644 --- a/bsp_q7s/boardconfig/busConf.h +++ b/bsp_q7s/boardconfig/busConf.h @@ -8,12 +8,14 @@ static constexpr uint32_t SPI_MAIN_BUS_LOCK_TIMEOUT = 50; static constexpr char SPI_RW_DEV[] = "/dev/spi_rw"; -static constexpr char I2C_PL_EIVE[] = "/dev/i2c_eive"; -static constexpr char I2C_PS_EIVE[] = "/dev/i2c-2"; +//! I2C bus using an I2C IP core in the programmable logic (PL) +static constexpr char I2C_PL_EIVE[] = "/dev/i2c_pl"; +//! I2C bus using the I2C peripheral of the ARM processing system (PS) +static constexpr char I2C_PS_EIVE[] = "/dev/i2c_ps"; static constexpr char UART_GNSS_DEV[] = "/dev/gps0"; static constexpr char UART_PLOC_MPSOC_DEV[] = "/dev/ul_plmpsoc"; -static constexpr char UART_PLOC_SUPERVSIOR_DEV[] = "/dev/ul_plsv"; +static constexpr char UART_PLOC_SUPERVSIOR_DEV[] = "/dev/ploc_supv"; static constexpr char UART_SYRLINKS_DEV[] = "/dev/ul_syrlinks"; static constexpr char UART_STAR_TRACKER_DEV[] = "/dev/ul_str"; static constexpr char UART_SCEX_DEV[] = "/dev/scex"; From d4acdb67bb4a7464a9479fa27a0bd41fa6c2b4ad Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 29 Nov 2022 11:07:02 +0100 Subject: [PATCH 222/244] bump changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 52b2d1a0..b9675d21 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,8 @@ list yields a list of all related PRs for each release. ## Changed - PLOC Supervisor: Changes baudrate to 921600 +- Renamed `/dev/ul-plsv` to `/dev/ploc_supv`, is not a UART lite anymore +- Renamed `/dev/i2c_eive` to `/dev/i2c_pl` and `/dev/i2c-2` to `/dev/i2c_ps`. # [v1.17.0] 28.11.2022 From a1c9fece1e1887305b4f28e42adc3a0454580dcc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 29 Nov 2022 11:08:34 +0100 Subject: [PATCH 223/244] bump changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b9675d21..a4df75d5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,7 +15,7 @@ list yields a list of all related PRs for each release. - PLOC Supervisor: Changes baudrate to 921600 - Renamed `/dev/ul-plsv` to `/dev/ploc_supv`, is not a UART lite anymore - Renamed `/dev/i2c_eive` to `/dev/i2c_pl` and `/dev/i2c-2` to `/dev/i2c_ps`. - + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/328 # [v1.17.0] 28.11.2022 ## Added From fbf8f5ef6cdded72548fe6bac45b503d24dc1fd5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 29 Nov 2022 16:55:55 +0100 Subject: [PATCH 224/244] bump tmtc --- generators/events/event_parser.py | 2 +- generators/objects/objects.py | 2 +- generators/returnvalues/returnvalues_parser.py | 2 +- tmtc | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/generators/events/event_parser.py b/generators/events/event_parser.py index 8ab88a1b..8c5ab35b 100644 --- a/generators/events/event_parser.py +++ b/generators/events/event_parser.py @@ -46,7 +46,7 @@ BSP_DIR_NAME = BSP_SELECT.value # Store this file in the root of the generators folder CSV_FILENAME = Path(f"{ROOT_DIR}/{BSP_SELECT.value}_events.csv") -CSV_COPY_DEST = Path(f"{OBSW_ROOT_DIR}/tmtc/config/events.csv") +CSV_COPY_DEST = Path(f"{OBSW_ROOT_DIR}/tmtc/eive_tmtc/config/events.csv") if BSP_SELECT == BspType.BSP_Q7S or BSP_SELECT == BspType.BSP_LINUX_BOARD: FSFW_CONFIG_ROOT = Path(f"{OBSW_ROOT_DIR}/linux/fsfwconfig") diff --git a/generators/objects/objects.py b/generators/objects/objects.py index c64a61ed..af8fec4b 100644 --- a/generators/objects/objects.py +++ b/generators/objects/objects.py @@ -43,7 +43,7 @@ CPP_COPY_DESTINATION = f"{FSFW_CONFIG_ROOT}/objects/" CPP_FILENAME = f"{os.path.dirname(os.path.realpath(__file__))}//translateObjects.cpp" CPP_H_FILENAME = f"{os.path.dirname(os.path.realpath(__file__))}//translateObjects.h" CSV_OBJECT_FILENAME = f"{ROOT_DIR}/{BSP_SELECT.value}_objects.csv" -CSV_COPY_DEST = f"{OBSW_ROOT_DIR}/tmtc/config/objects.csv" +CSV_COPY_DEST = f"{OBSW_ROOT_DIR}/tmtc/eive_tmtc/config/objects.csv" FILE_SEPARATOR = ";" diff --git a/generators/returnvalues/returnvalues_parser.py b/generators/returnvalues/returnvalues_parser.py index e79f79de..4f6f30e1 100644 --- a/generators/returnvalues/returnvalues_parser.py +++ b/generators/returnvalues/returnvalues_parser.py @@ -31,7 +31,7 @@ MAX_STRING_LENGTH = 32 BSP_SELECT = BspType.BSP_Q7S BSP_DIR_NAME = BSP_SELECT.value CSV_RETVAL_FILENAME = Path(f"{ROOT_DIR}/{BSP_SELECT.value}_returnvalues.csv") -CSV_COPY_DEST = Path(f"{OBSW_ROOT_DIR}/tmtc/config/returnvalues.csv") +CSV_COPY_DEST = Path(f"{OBSW_ROOT_DIR}/tmtc/eive_tmtc/config/returnvalues.csv") ADD_LINUX_FOLDER = False if BSP_SELECT == BspType.BSP_Q7S or BSP_SELECT == BspType.BSP_LINUX_BOARD: diff --git a/tmtc b/tmtc index 12f7b5bb..1483dc4d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 12f7b5bb258f183b62a2ed27776d2385a728306e +Subproject commit 1483dc4d93898826b1cee645148f88995bbdc7a5 From 2b327bf3a7f6a12867ef565271f56d16eb4bea76 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 29 Nov 2022 23:09:55 +0100 Subject: [PATCH 225/244] initialize a variable --- linux/devices/ploc/PlocSupvUartMan.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index ee232f47..a246d97c 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -1132,7 +1132,7 @@ int PlocSupvUartManager::removeHdlcFramingWithCrcCheck(const uint8_t* src, size_ } // calc crc16 uint16_t calcCrc = calc_crc16_buff_reflected(dst, tlen - 2); - uint16_t crc; + uint16_t crc = 0; size_t dummy; SerializeAdapter::deSerialize(&crc, dst + tlen - 2, &dummy, SerializeIF::Endianness::LITTLE); if (calcCrc != crc) { From 84a3466b4d6dafa1b5526bc2f653df16ada91de5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 29 Nov 2022 23:24:45 +0100 Subject: [PATCH 226/244] fixed array list min len 1 --- fsfw | 2 +- mission/system/tree/acsModeTree.cpp | 2 +- mission/system/tree/payloadModeTree.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/fsfw b/fsfw index 8eb869e0..5b0ea912 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 8eb869e073d99e257c6cc62dbf26b4d99422e9b8 +Subproject commit 5b0ea91222a6b8efb2f4562cfecbcb735dfeedd5 diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp index 4e390985..d91c59fc 100644 --- a/mission/system/tree/acsModeTree.cpp +++ b/mission/system/tree/acsModeTree.cpp @@ -30,7 +30,7 @@ static const auto NML = DeviceHandlerIF::MODE_NORMAL; auto ACS_SEQUENCE_OFF = std::make_pair(acs::CtrlModes::OFF << 24, FixedArrayList()); auto ACS_TABLE_OFF_TGT = - std::make_pair((acs::CtrlModes::OFF << 24) | 1, FixedArrayList()); + std::make_pair((acs::CtrlModes::OFF << 24) | 1, FixedArrayList()); auto ACS_TABLE_OFF_TRANS = std::make_pair((acs::CtrlModes::OFF << 24) | 2, FixedArrayList()); diff --git a/mission/system/tree/payloadModeTree.cpp b/mission/system/tree/payloadModeTree.cpp index 6cbc4004..0396cfc6 100644 --- a/mission/system/tree/payloadModeTree.cpp +++ b/mission/system/tree/payloadModeTree.cpp @@ -28,7 +28,7 @@ static const auto ON = HasModesIF::MODE_ON; static const auto NML = DeviceHandlerIF::MODE_NORMAL; auto PL_SEQUENCE_OFF = std::make_pair(OFF << 24, FixedArrayList()); -auto PL_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList()); +auto PL_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList()); auto PL_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList()); auto PL_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 3, FixedArrayList()); From 2d939a289407a1a0e870b06e4543d1f1718f1c70 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 30 Nov 2022 10:35:30 +0100 Subject: [PATCH 227/244] update --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 603b7e85..4cfd2d68 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 603b7e8574d74ba60692115133cde3cd8b8bd423 +Subproject commit 4cfd2d684a531b66ff1b366ef87ac086ba993a96 From 5f929f6cf805fff498a5d4d49ef4c65f765a1059 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 30 Nov 2022 17:26:33 +0100 Subject: [PATCH 228/244] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 1483dc4d..c421f3f5 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1483dc4d93898826b1cee645148f88995bbdc7a5 +Subproject commit c421f3f5d78d77d23e42d1ccae79d7c6fc308431 From f73d293cdd72653bef7b3ffe82bb815f9ff3676a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 30 Nov 2022 18:08:16 +0100 Subject: [PATCH 229/244] schedule PL task --- bsp_q7s/core/scheduling.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index f07eda32..5a8c3827 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -365,6 +365,7 @@ void scheduling::initTasks() { #if OBSW_ADD_PLOC_SUPERVISOR == 1 supvHelperTask->startTask(); #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */ + plTask->startTask(); #if OBSW_ADD_TEST_CODE == 1 taskStarter(testTasks, "Test task vector"); From b2e0ef24f3825bdd03e1739c93d3fd1444423379 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 29 Nov 2022 11:45:58 +0100 Subject: [PATCH 230/244] first version for gauss-jordan matrix inversion --- mission/controller/acs/util/MathOperations.h | 577 +++++++++++-------- 1 file changed, 323 insertions(+), 254 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 6a534880..c37dbde8 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -1,298 +1,367 @@ -/* - * MathOperations.h - * - * Created on: 3 Mar 2022 - * Author: rooob - */ - #ifndef MATH_MATHOPERATIONS_H_ #define MATH_MATHOPERATIONS_H_ -#include -#include -#include -#include #include #include +#include +#include +#include +#include + +#include using namespace Math; -template +template class MathOperations { -public: - static void skewMatrix(const T1 vector[], T2 *result) { - // Input Dimension [3], Output [3][3] - result[0] = 0; - result[1] = -vector[2]; - result[2] = vector[1]; - result[3] = vector[2]; - result[4] = 0; - result[5] = -vector[0]; - result[6] = -vector[1]; - result[7] = vector[0]; - result[8] = 0; - } - static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], - T2 *result, uint8_t size = 3) { - // Looks like MatrixOpertions::multiply is able to do the same thing - for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) { - for (uint8_t resultRow = 0; resultRow < size; resultRow++) { - result[resultColumn + size * resultRow] = vector1[resultRow] - * transposeVector2[resultColumn]; + public: + static void skewMatrix(const T1 vector[], T2 *result) { + // Input Dimension [3], Output [3][3] + result[0] = 0; + result[1] = -vector[2]; + result[2] = vector[1]; + result[3] = vector[2]; + result[4] = 0; + result[5] = -vector[0]; + result[6] = -vector[1]; + result[7] = vector[0]; + result[8] = 0; + } + static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result, + uint8_t size = 3) { + // Looks like MatrixOpertions::multiply is able to do the same thing + for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) { + for (uint8_t resultRow = 0; resultRow < size; resultRow++) { + result[resultColumn + size * resultRow] = + vector1[resultRow] * transposeVector2[resultColumn]; + } + } + /*matrixSun[i][j] = sunEstB[i] * sunEstB[j]; + matrixMag[i][j] = magEstB[i] * magEstB[j]; + matrixSunMag[i][j] = sunEstB[i] * magEstB[j]; + matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/ + } - } - } - /*matrixSun[i][j] = sunEstB[i] * sunEstB[j]; - matrixMag[i][j] = magEstB[i] * magEstB[j]; - matrixSunMag[i][j] = sunEstB[i] * magEstB[j]; - matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/ - } + static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) { + int min_idx; + T1 temp; + memcpy(result, matrix, rowSize * colSize * sizeof(*result)); + // One by one move boundary of unsorted subarray + for (int k = 0; k < rowSize; k++) { + for (int i = 0; i < colSize - 1; i++) { + // Find the minimum element in unsorted array + min_idx = i; + for (int j = i + 1; j < colSize; j++) { + if (result[j + k * colSize] < result[min_idx + k * colSize]) { + min_idx = j; + } + } + // Swap the found minimum element with the first element + temp = result[i + k * colSize]; + result[i + k * colSize] = result[min_idx + k * colSize]; + result[min_idx + k * colSize] = temp; + } + } + } - static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, - uint8_t colSize) { - int min_idx; - T1 temp; - memcpy(result, matrix, rowSize * colSize * sizeof(*result)); - // One by one move boundary of unsorted subarray - for (int k = 0; k < rowSize; k++) { - for (int i = 0; i < colSize - 1; i++) { - // Find the minimum element in unsorted array - min_idx = i; - for (int j = i + 1; j < colSize; j++) { - if (result[j + k * colSize] - < result[min_idx + k * colSize]) { - min_idx = j; - } - } - // Swap the found minimum element with the first element - temp = result[i + k * colSize]; - result[i + k * colSize] = result[min_idx + k * colSize]; - result[min_idx + k * colSize] = temp; - } - } - } + 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){ + static T1 convertUnixToJD2000(timeval time) { + // time = {{s},{us}} + T1 julianDate2000; + julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545; + return julianDate2000; + } - // 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 dcmFromQuat(const T1 vector[], T1 *outputDcm) { + // convention q = [qx,qy,qz, qw] + outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2); + outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]); + outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]); - static T1 convertUnixToJD2000(timeval time){ - //time = {{s},{us}} - T1 julianDate2000; - julianDate2000 = (time.tv_sec/86400.0)+2440587.5-2451545; - return julianDate2000; - } + outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]); + outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2); + outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]); - static void dcmFromQuat(const T1 vector[], T1 *outputDcm){ - // convention q = [qx,qy,qz, qw] - outputDcm[0] = pow(vector[0],2) - pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2); - outputDcm[1] = 2*(vector[0]*vector[1] + vector[2]*vector[3]); - outputDcm[2] = 2*(vector[0]*vector[2] - vector[1]*vector[3]); + outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]); + outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]); + outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2); + } - outputDcm[3] = 2*(vector[1]*vector[0] - vector[2]*vector[3]); - outputDcm[4] = -pow(vector[0],2) + pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2); - outputDcm[5] = 2*(vector[1]*vector[2] + vector[0]*vector[3]); + static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, + T2 *cartesianOutput) { + double radiusPolar = 6378137; + double radiusEqua = 6356752.314; - outputDcm[6] = 2*(vector[2]*vector[0] + vector[1]*vector[3]); - outputDcm[7] = 2*(vector[2]*vector[1] - vector[0]*vector[3]); - outputDcm[8] = -pow(vector[0],2) - pow(vector[1],2) + pow(vector[2],2) + pow(vector[3],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 cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){ + /* @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) { + 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 radiusPolar = 6378137; - double radiusEqua = 6356752.314; + double JC2000 = JD2000Floor / 36525; + double sec = (JD2000 - JD2000Floor) * 86400; + double gmst = 0; // greenwich mean sidereal time + gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) - + 0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec; + double rest = gmst / 86400; + double FloorRest = floor(rest); + double secOfDay = rest - FloorRest; + secOfDay *= 86400; + gmst = secOfDay / 240 * PI / 180; - double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2)); - double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2)); + outputDcmEJ[0] = cos(gmst); + outputDcmEJ[1] = sin(gmst); + outputDcmEJ[2] = 0; + outputDcmEJ[3] = -sin(gmst); + outputDcmEJ[4] = cos(gmst); + outputDcmEJ[5] = 0; + outputDcmEJ[6] = 0; + outputDcmEJ[7] = 0; + outputDcmEJ[8] = 1; + } - 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); + /* @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) - } + double JD2000UTC1 = convertUnixToJD2000(unixTime); - /* @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){ + // 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 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; - } + //------------------------------------------------------------------------------------- + // 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; - double JC2000 = JD2000Floor / 36525; - double sec = (JD2000 - JD2000Floor) * 86400; - double gmst = 0; //greenwich mean sidereal time - gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000,2) - - 0.0000062 * pow(JC2000,3) + 1.002737909350795 * sec; - double rest = gmst / 86400; - double FloorRest = floor(rest); - double secOfDay = rest-FloorRest; - secOfDay *= 86400; - gmst = secOfDay / 240 * PI / 180; + 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; - outputDcmEJ[0] = cos(gmst); - outputDcmEJ[1] = sin(gmst); - outputDcmEJ[2] = 0; - outputDcmEJ[3] = -sin(gmst); - outputDcmEJ[4] = cos(gmst); - outputDcmEJ[5] = 0; - outputDcmEJ[6] = 0; - outputDcmEJ[7] = 0; - outputDcmEJ[8] = 1; + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Precession P + //------------------------------------------------------------------------------------- + double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - } + 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); - /* @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 ) { + zeta *= arcsecFactor; + theta2 *= arcsecFactor; + ze *= arcsecFactor; -// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 (initial Offset) -// International Atomic Time (TAI) + 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); - double JD2000UTC1 = convertUnixToJD2000(unixTime); + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Nutation size + //------------------------------------------------------------------------------------- + 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); -// 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; + // delta eps approx + double de = 9.203 * arcsecFactor * cos(Om); -//------------------------------------------------------------------------------------- -// 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; + // % true obliquity of the ecliptic eps p.71 (simplified) + double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; + ; - 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; + 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 from earth Precession P -//------------------------------------------------------------------------------------- - double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + //------------------------------------------------------------------------------------- + // 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); - 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 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); - zeta *= arcsecFactor; - theta2 *= arcsecFactor; - ze *= arcsecFactor; + MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 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); + 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 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); + 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<<"\size\ndeterminant: "<::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); - MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); - -//------------------------------------------------------------------------------------- -// 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: "<::matrixDeterminant(*submatrix, size - 1)); + } + } + return det; + } + static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { + if (MathOperations::matrixDeterminant(*inputMatrix, size) == 0) { + return 0; // Matrix is singular and not invertible + } + T1 matrix[size][size], identity[size][size] = {0}; + // reformat array to matrix + for (uint8_t row = 0; row < size; row++) { + for (uint8_t col = 0; col < size; col++) { + matrix[row][col] = inputMatrix[row * size + col]; + } + } + // init identity matrix + for (uint8_t diag = 0; diag < size; diag++) { + identity[diag][diag] = 1; + } + // gauss-jordan algo + for (uint8_t row = 0; row < size; row++) { + uint8_t rowIndex = row; + // check if diag entry is 0 + // in case it is, find next row whose diag entry is not 0 + while (matrix[rowIndex][row] == 0) { + if (rowIndex < size) { + rowIndex++; + } else { + return 0; // Matrix is not invertible + } + } + // swap rows if needed + if (rowIndex != row) { + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); + std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); + } + } + // normalize line + for (uint8_t colIndex = row; colIndex < size; colIndex++) { + matrix[row][colIndex] = matrix[row][colIndex] / matrix[row][row]; + identity[row][colIndex] = identity[row][colIndex] / matrix[row][row]; + } + // make elements of the same col in following rows to 0 + for (uint8_t rowIndex = row + 1; rowIndex < size; rowIndex++) { + for (uint8_t colIndex = row; colIndex < size; colIndex++) { + matrix[rowIndex][colIndex] -= matrix[row][colIndex] * matrix[rowIndex][row]; + identity[rowIndex][colIndex] -= identity[row][colIndex] * matrix[rowIndex][row]; + } + } + } + std::memcpy(inverse, identity, size * size * sizeof(T1)); + return 1; // successful inversion + } }; #endif /* ACS_MATH_MATHOPERATIONS_H_ */ From b2442041f0c74581c5003e589d7cea68c0eee778 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 30 Nov 2022 09:07:09 +0100 Subject: [PATCH 231/244] gauss-jordan v2 that hopefully noone ever sees --- .../acs/MultiplicativeKalmanFilter.cpp | 10 ++-- mission/controller/acs/util/MathOperations.h | 53 +++++++++++++++---- 2 files changed, 49 insertions(+), 14 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5d66bd8d..4717dc79 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -888,10 +888,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // (H * P * H' + R) MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); // <> - double invResidualCov1[MDF] = {0}; + // double invResidualCov1[MDF] = {0}; double invResidualCov[MDF][MDF] = {{0}}; - int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov, *invResidualCov, - invResidualCov1, MDF); + // int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov, + // *invResidualCov, + // invResidualCov1, MDF); + int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); + double test[MDF][MDF]; + MatrixOperations::multiply(*residualCov, *invResidualCov, *test, MDF, MDF, MDF); if (inversionFailed) { { PoolReadGuard pg(mekfData); diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index c37dbde8..8279ec40 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -313,10 +313,11 @@ class MathOperations { } static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { - if (MathOperations::matrixDeterminant(*inputMatrix, size) == 0) { - return 0; // Matrix is singular and not invertible + std::cout << MathOperations::matrixDeterminant(inputMatrix, size) << std::endl; + if (MathOperations::matrixDeterminant(inputMatrix, size) == 0) { + return 1; // Matrix is singular and not invertible } - T1 matrix[size][size], identity[size][size] = {0}; + T1 matrix[size][size], identity[size][size]; // reformat array to matrix for (uint8_t row = 0; row < size; row++) { for (uint8_t col = 0; col < size; col++) { @@ -324,10 +325,12 @@ class MathOperations { } } // init identity matrix + std::memset(identity, 0.0, sizeof(identity)); for (uint8_t diag = 0; diag < size; diag++) { identity[diag][diag] = 1; } // gauss-jordan algo + // start with gauss for (uint8_t row = 0; row < size; row++) { uint8_t rowIndex = row; // check if diag entry is 0 @@ -336,7 +339,7 @@ class MathOperations { if (rowIndex < size) { rowIndex++; } else { - return 0; // Matrix is not invertible + return 1; // Matrix is not invertible } } // swap rows if needed @@ -347,20 +350,48 @@ class MathOperations { } } // normalize line + double normFactor = matrix[row][row]; for (uint8_t colIndex = row; colIndex < size; colIndex++) { - matrix[row][colIndex] = matrix[row][colIndex] / matrix[row][row]; - identity[row][colIndex] = identity[row][colIndex] / matrix[row][row]; + matrix[row][colIndex] /= normFactor; + identity[row][colIndex] /= normFactor; } // make elements of the same col in following rows to 0 + std::cout << "C++ sucks" << std::endl; for (uint8_t rowIndex = row + 1; rowIndex < size; rowIndex++) { - for (uint8_t colIndex = row; colIndex < size; colIndex++) { - matrix[rowIndex][colIndex] -= matrix[row][colIndex] * matrix[rowIndex][row]; - identity[rowIndex][colIndex] -= identity[row][colIndex] * matrix[rowIndex][row]; + double elimFactor = matrix[rowIndex][row]; + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor; + identity[rowIndex][colIndex] -= identity[row][colIndex] * elimFactor; } } } - std::memcpy(inverse, identity, size * size * sizeof(T1)); - return 1; // successful inversion + // finish with jordan + for (uint8_t row = size - 1; row > 0; row--) { + for (int16_t rowIndex = row - 1; rowIndex >= 0; rowIndex--) { + double elimFactor = matrix[rowIndex][row]; + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor; + identity[rowIndex][row] -= identity[row][colIndex] * elimFactor; + } + } + } + T1 test[size][size]; + MatrixOperations::multiply(inputMatrix, *identity, *test, size, size, size); + std::cout << "[\n" + << test[0][0] << " " << test[0][1] << " " << test[0][2] << " " << test[0][3] << " " + << test[0][4] << " " << test[0][5] << "\n" + << test[1][0] << " " << test[1][1] << " " << test[1][2] << " " << test[1][3] << " " + << test[1][4] << " " << test[1][5] << "\n" + << test[2][0] << " " << test[2][1] << " " << test[2][2] << " " << test[2][3] << " " + << test[2][4] << " " << test[2][5] << "\n" + << test[3][0] << " " << test[3][1] << " " << test[3][2] << " " << test[3][3] << " " + << test[3][4] << " " << test[3][5] << "\n" + << test[4][0] << " " << test[4][1] << " " << test[4][2] << " " << test[4][3] << " " + << test[4][4] << " " << test[4][5] << "\n" + << test[5][0] << " " << test[5][1] << " " << test[5][2] << " " << test[5][3] << " " + << test[5][4] << " " << test[5][5] << "\n]" << std::endl; + std::memcpy(inverse, identity, sizeof(identity)); + return 0; // successful inversion } }; From ce83b64ca212dc4c624bb82ce1733f1dd0964e1d Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 1 Dec 2022 09:15:42 +0100 Subject: [PATCH 232/244] working versions of inverse calc and determinant calc --- .../acs/MultiplicativeKalmanFilter.cpp | 6 - mission/controller/acs/util/MathOperations.h | 115 +++++++++--------- 2 files changed, 56 insertions(+), 65 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 4717dc79..199440d5 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -888,14 +888,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // (H * P * H' + R) MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); // <> - // double invResidualCov1[MDF] = {0}; double invResidualCov[MDF][MDF] = {{0}}; - // int inversionFailed = CholeskyDecomposition::invertCholesky(*residualCov, - // *invResidualCov, - // invResidualCov1, MDF); int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); - double test[MDF][MDF]; - MatrixOperations::multiply(*residualCov, *invResidualCov, *test, MDF, MDF, MDF); if (inversionFailed) { { PoolReadGuard pg(mekfData); diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 8279ec40..6484a72d 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -285,7 +285,7 @@ class MathOperations { static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) { float det = 0; - T1 matrix[size][size], submatrix[size][size]; + T1 matrix[size][size], submatrix[size - 1][size - 1]; for (uint8_t row = 0; row < size; row++) { for (uint8_t col = 0; col < size; col++) { matrix[row][col] = inputMatrix[row * size + col]; @@ -294,26 +294,25 @@ class MathOperations { if (size == 2) return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1])); else { - for (uint8_t x = 0; x < size; x++) { - int subi = 0; - for (uint8_t i = 1; i < size; i++) { - int subj = 0; - for (uint8_t j = 0; j < size; j++) { - if (j == x) continue; - submatrix[subi][subj] = matrix[i][j]; - subj++; + for (uint8_t col = 0; col < size; col++) { + int subRow = 0; + for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) { + int subCol = 0; + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + if (colIndex == col) continue; + submatrix[subRow][subCol] = matrix[rowIndex][colIndex]; + subCol++; } - subi++; + subRow++; } - det = det + (pow(-1, x) * matrix[0][x] * - MathOperations::matrixDeterminant(*submatrix, size - 1)); + det += (pow(-1, col) * matrix[0][col] * + MathOperations::matrixDeterminant(*submatrix, size - 1)); } } return det; } static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { - std::cout << MathOperations::matrixDeterminant(inputMatrix, size) << std::endl; if (MathOperations::matrixDeterminant(inputMatrix, size) == 0) { return 1; // Matrix is singular and not invertible } @@ -330,66 +329,64 @@ class MathOperations { identity[diag][diag] = 1; } // gauss-jordan algo - // start with gauss + // sort matrix such as no diag entry shall be 0 + // should not be needed as such a matrix has a det=0 for (uint8_t row = 0; row < size; row++) { - uint8_t rowIndex = row; - // check if diag entry is 0 - // in case it is, find next row whose diag entry is not 0 - while (matrix[rowIndex][row] == 0) { - if (rowIndex < size) { + if (matrix[row][row] == 0.0) { + bool swaped = false; + uint8_t rowIndex = 0; + while ((rowIndex < size) && !swaped) { + if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) { + for (uint8_t colIndex = 0; colIndex < size; colIndex++) { + std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); + std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); + } + swaped = true; + } rowIndex++; - } else { - return 1; // Matrix is not invertible + } + if (!swaped) { + return 1; // matrix not invertible } } - // swap rows if needed - if (rowIndex != row) { + } + + for (int row = 0; row < size; row++) { + if (matrix[row][row] == 0.0) { + uint8_t rowIndex; + if (row == 0) { + rowIndex = size - 1; + } else { + rowIndex = row - 1; + } for (uint8_t colIndex = 0; colIndex < size; colIndex++) { std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); } - } - // normalize line - double normFactor = matrix[row][row]; - for (uint8_t colIndex = row; colIndex < size; colIndex++) { - matrix[row][colIndex] /= normFactor; - identity[row][colIndex] /= normFactor; - } - // make elements of the same col in following rows to 0 - std::cout << "C++ sucks" << std::endl; - for (uint8_t rowIndex = row + 1; rowIndex < size; rowIndex++) { - double elimFactor = matrix[rowIndex][row]; - for (uint8_t colIndex = 0; colIndex < size; colIndex++) { - matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor; - identity[rowIndex][colIndex] -= identity[row][colIndex] * elimFactor; + row--; + if (row < 0) { + return 1; // Matrix is not invertible } } } - // finish with jordan - for (uint8_t row = size - 1; row > 0; row--) { - for (int16_t rowIndex = row - 1; rowIndex >= 0; rowIndex--) { - double elimFactor = matrix[rowIndex][row]; - for (uint8_t colIndex = 0; colIndex < size; colIndex++) { - matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor; - identity[rowIndex][row] -= identity[row][colIndex] * elimFactor; + // remove non diag elements in matrix (jordan) + for (int row = 0; row < size; row++) { + for (int rowIndex = 0; rowIndex < size; rowIndex++) { + if (row != rowIndex) { + double ratio = matrix[rowIndex][row] / matrix[row][row]; + for (int colIndex = 0; colIndex < size; colIndex++) { + matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex]; + identity[rowIndex][colIndex] -= ratio * identity[row][colIndex]; + } } } } - T1 test[size][size]; - MatrixOperations::multiply(inputMatrix, *identity, *test, size, size, size); - std::cout << "[\n" - << test[0][0] << " " << test[0][1] << " " << test[0][2] << " " << test[0][3] << " " - << test[0][4] << " " << test[0][5] << "\n" - << test[1][0] << " " << test[1][1] << " " << test[1][2] << " " << test[1][3] << " " - << test[1][4] << " " << test[1][5] << "\n" - << test[2][0] << " " << test[2][1] << " " << test[2][2] << " " << test[2][3] << " " - << test[2][4] << " " << test[2][5] << "\n" - << test[3][0] << " " << test[3][1] << " " << test[3][2] << " " << test[3][3] << " " - << test[3][4] << " " << test[3][5] << "\n" - << test[4][0] << " " << test[4][1] << " " << test[4][2] << " " << test[4][3] << " " - << test[4][4] << " " << test[4][5] << "\n" - << test[5][0] << " " << test[5][1] << " " << test[5][2] << " " << test[5][3] << " " - << test[5][4] << " " << test[5][5] << "\n]" << std::endl; + // normalize rows in matrix (gauss) + for (int row = 0; row < size; row++) { + for (int col = 0; col < size; col++) { + identity[row][col] = identity[row][col] / matrix[row][row]; + } + } std::memcpy(inverse, identity, sizeof(identity)); return 0; // successful inversion } From 377a672cd204e5319928c888af48a98431f7a8a4 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 1 Dec 2022 13:59:26 +0100 Subject: [PATCH 233/244] detumblecounter init --- mission/controller/acs/AcsParameters.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 813851dc..828e4495 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -895,9 +895,10 @@ class AcsParameters /*: public HasParametersIF*/ { } magnetorquesParameter; struct DetumbleParameter { - uint8_t detumblecounter; - double omegaDetumbleStart; - double omegaDetumbleEnd; + 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); } detumbleParameter; }; From 08bdc875055eff86a281c43eb8cbb840df6a3a45 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Thu, 1 Dec 2022 14:19:42 +0100 Subject: [PATCH 234/244] removed hardcoded modes --- mission/controller/AcsController.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d00bb816..f9ea5926 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -82,11 +82,6 @@ void AcsController::performControlOperation() { copyGyrData(); } } - - // DEBUG : REMOVE AFTER COMPLETION - mode = MODE_ON; - submode = SUBMODE_SAFE; - // DEBUG END } void AcsController::performSafe() { From d3252434aa88fe5d0aead95acee7bfcf9e799e8a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Dec 2022 14:47:21 +0100 Subject: [PATCH 235/244] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 1483dc4d..6308bbb3 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1483dc4d93898826b1cee645148f88995bbdc7a5 +Subproject commit 6308bbb3f0e4387508371bd3d18c52f404c4e18b From a3a919437e9d6d7f8a49de7adba75c4c7951ff54 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Dec 2022 15:56:40 +0100 Subject: [PATCH 236/244] bump submodules --- fsfw | 2 +- tmtc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/fsfw b/fsfw index 672fca51..5b0ea912 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 672fca5169b017387e58e2ff864913d932c59aa1 +Subproject commit 5b0ea91222a6b8efb2f4562cfecbcb735dfeedd5 diff --git a/tmtc b/tmtc index 4cfd2d68..6308bbb3 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 4cfd2d684a531b66ff1b366ef87ac086ba993a96 +Subproject commit 6308bbb3f0e4387508371bd3d18c52f404c4e18b From 828738ba0e46f22666531c224c740c44436cae26 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Dec 2022 15:56:55 +0100 Subject: [PATCH 237/244] afmt --- dummies/GyroL3GD20Dummy.cpp | 1 - dummies/ImtqDummy.cpp | 2 +- dummies/SusDummy.cpp | 8 +- mission/controller/AcsController.h | 2 +- mission/controller/CMakeLists.txt | 2 +- mission/controller/acs/CMakeLists.txt | 18 +- mission/controller/acs/SusConverter.cpp | 10 +- mission/controller/acs/SusConverter.h | 8 +- mission/controller/acs/config/classIds.h | 15 +- mission/controller/acs/control/CMakeLists.txt | 7 +- mission/controller/acs/control/Detumble.cpp | 22 +-- mission/controller/acs/control/Detumble.h | 46 +++-- mission/controller/acs/control/PtgCtrl.cpp | 160 +++++++++--------- .../acs/util/CholeskyDecomposition.h | 156 +++++++++-------- 14 files changed, 217 insertions(+), 240 deletions(-) diff --git a/dummies/GyroL3GD20Dummy.cpp b/dummies/GyroL3GD20Dummy.cpp index a800cd62..20309c63 100644 --- a/dummies/GyroL3GD20Dummy.cpp +++ b/dummies/GyroL3GD20Dummy.cpp @@ -38,7 +38,6 @@ void GyroL3GD20Dummy::fillCommandAndReplyMap() {} uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; } - ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({1.2}, true)); diff --git a/dummies/ImtqDummy.cpp b/dummies/ImtqDummy.cpp index 7f64d977..fdc7b009 100644 --- a/dummies/ImtqDummy.cpp +++ b/dummies/ImtqDummy.cpp @@ -39,7 +39,7 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0,0.0,0.0})); + localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry({0.0, 0.0, 0.0})); localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry({0.12, 0.76, -0.45}, true)); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); diff --git a/dummies/SusDummy.cpp b/dummies/SusDummy.cpp index 28f85969..c0aed6dd 100644 --- a/dummies/SusDummy.cpp +++ b/dummies/SusDummy.cpp @@ -35,9 +35,9 @@ uint32_t SusDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); - localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, - new PoolEntry({0, 0, 0, 0, 0, 0},true)); + localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry({0}, 1, true)); + localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC, + new PoolEntry({0, 0, 0, 0, 0, 0}, true)); - return returnvalue::OK; + return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index ed896d8e..1c4996b3 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -70,7 +70,7 @@ class AcsController : public ExtendedControllerBase { void announceMode(bool recursive); /* ACS Datasets */ -IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); + IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); // MGMs acsctrl::MgmDataRaw mgmDataRaw; PoolEntry mgm0VecRaw = PoolEntry(3); diff --git a/mission/controller/CMakeLists.txt b/mission/controller/CMakeLists.txt index 463e6a5d..6177a14b 100644 --- a/mission/controller/CMakeLists.txt +++ b/mission/controller/CMakeLists.txt @@ -3,4 +3,4 @@ if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "") AcsController.cpp) endif() -add_subdirectory(acs) \ No newline at end of file +add_subdirectory(acs) diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index a379197d..3c4a3475 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -1,13 +1,13 @@ target_sources( ${LIB_EIVE_MISSION} PRIVATE AcsParameters.cpp - ActuatorCmd.cpp - Guidance.cpp - Igrf13Model.cpp - MultiplicativeKalmanFilter.cpp - Navigation.cpp - SensorProcessing.cpp - SensorValues.cpp - SusConverter.cpp) - + ActuatorCmd.cpp + Guidance.cpp + Igrf13Model.cpp + MultiplicativeKalmanFilter.cpp + Navigation.cpp + SensorProcessing.cpp + SensorValues.cpp + SusConverter.cpp) + add_subdirectory(control) diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 1dc3a2e2..7020742a 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -108,14 +108,14 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB float* SusConverter::calculateSunVector() { // Calculate the normalized Sun Vector sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) / - (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + - powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) + + powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) / - (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + - powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + + powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); sunVectorSensorFrame[2] = -(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) + - powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); + powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1)))); return sunVectorSensorFrame; } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 10079e90..b3829827 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -27,10 +27,10 @@ class SusConverter { const float coeffBeta[9][10]); private: - float alphaBetaRaw[2]; //[°] - // float coeffAlpha[9][10]; - // float coeffBeta[9][10]; - float alphaBetaCalibrated[2]; //[°] + float alphaBetaRaw[2]; //[°] + // float coeffAlpha[9][10]; + // float coeffBeta[9][10]; + float alphaBetaCalibrated[2]; //[°] float sunVectorSensorFrame[3]; //[-] bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK, diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h index e714ff33..ccf6b616 100644 --- a/mission/controller/acs/config/classIds.h +++ b/mission/controller/acs/config/classIds.h @@ -5,15 +5,14 @@ #include namespace CLASS_ID { -enum eiveclassIds: uint8_t { - EIVE_CLASS_ID_START = COMMON_CLASS_ID_END, - KALMAN, - SAFE, - PTG, - DETUMBLE, - EIVE_CLASS_ID_END // [EXPORT] : [END] +enum eiveclassIds : uint8_t { + EIVE_CLASS_ID_START = COMMON_CLASS_ID_END, + KALMAN, + SAFE, + PTG, + DETUMBLE, + EIVE_CLASS_ID_END // [EXPORT] : [END] }; } - #endif /* ACS_CONFIG_CLASSIDS_H_ */ diff --git a/mission/controller/acs/control/CMakeLists.txt b/mission/controller/acs/control/CMakeLists.txt index dc540435..2d40ecc5 100644 --- a/mission/controller/acs/control/CMakeLists.txt +++ b/mission/controller/acs/control/CMakeLists.txt @@ -1,5 +1,2 @@ -target_sources( - ${LIB_EIVE_MISSION} - PRIVATE Detumble.cpp - PtgCtrl.cpp - SafeCtrl.cpp) \ No newline at end of file +target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp + SafeCtrl.cpp) diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 7421226d..4c8217dc 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -6,30 +6,24 @@ * Author: Robin Marquardt */ - #include "Detumble.h" -#include "../util/MathOperations.h" -#include + #include #include #include #include #include +#include +#include "../util/MathOperations.h" -Detumble::Detumble(AcsParameters *acsParameters_){ - loadAcsParameters(acsParameters_); -} +Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } -Detumble::~Detumble(){ - -} - -void Detumble::loadAcsParameters(AcsParameters *acsParameters_){ - - detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); - magnetorquesParameter = &(acsParameters_->magnetorquesParameter); +Detumble::~Detumble() {} +void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { + detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters); + magnetorquesParameter = &(acsParameters_->magnetorquesParameter); } ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 375f67aa..bea9b1bf 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -8,38 +8,36 @@ #ifndef ACS_CONTROL_DETUMBLE_H_ #define ACS_CONTROL_DETUMBLE_H_ -#include "../SensorValues.h" -#include "../AcsParameters.h" -#include "../config/classIds.h" -#include -#include -#include #include +#include +#include +#include +#include "../AcsParameters.h" +#include "../SensorValues.h" +#include "../config/classIds.h" -class Detumble{ +class Detumble { + public: + Detumble(AcsParameters *acsParameters_); + virtual ~Detumble(); -public: - Detumble(AcsParameters *acsParameters_); - virtual ~Detumble(); + static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE; + static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); - 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 + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void loadAcsParameters(AcsParameters *acsParameters_); - /* @brief: Load AcsParameters für this class - * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters - */ - void loadAcsParameters(AcsParameters *acsParameters_); + ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField, + const bool magFieldValid, double *magMom); - ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, - const double *magField, const bool magFieldValid, double *magMom); + ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); - ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); - - private: - AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters; - AcsParameters::MagnetorquesParameter* magnetorquesParameter; + private: + AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters; + 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 e06b576f..07b595ec 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -5,10 +5,8 @@ * Author: Robin Marquardt */ - - #include "PtgCtrl.h" -#include "../util/MathOperations.h" + #include #include #include @@ -16,100 +14,96 @@ #include #include -PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ - loadAcsParameters(acsParameters_); +#include "../util/MathOperations.h" + +PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } + +PtgCtrl::~PtgCtrl() {} + +void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { + pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); + inertiaEIVE = &(acsParameters_->inertiaEIVE); + rwHandlingParameters = &(acsParameters_->rwHandlingParameters); + rwMatrices = &(acsParameters_->rwMatrices); } -PtgCtrl::~PtgCtrl(){ +void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate, + const double *rwPseudoInv, double *torqueRws) { + //------------------------------------------------------------------------------------------------ + // Compute gain matrix K and P matrix + //------------------------------------------------------------------------------------------------ + double om = pointingModeControllerParameters->om; + double zeta = pointingModeControllerParameters->zeta; + double qErrorMin = pointingModeControllerParameters->qiMin; + double omMax = pointingModeControllerParameters->omMax; -} + double cInt = 2 * om * zeta; + double kInt = 2 * pow(om, 2); -void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){ - pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); - inertiaEIVE = &(acsParameters_->inertiaEIVE); - rwHandlingParameters = &(acsParameters_->rwHandlingParameters); - rwMatrices =&(acsParameters_->rwMatrices); -} + double qErrorLaw[3] = {0, 0, 0}; -void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){ + for (int i = 0; i < 3; i++) { + if (abs(qError[i]) < qErrorMin) { + qErrorLaw[i] = qErrorMin; + } else { + qErrorLaw[i] = abs(qError[i]); + } + } + double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); - //------------------------------------------------------------------------------------------------ - // Compute gain matrix K and P matrix - //------------------------------------------------------------------------------------------------ - double om = pointingModeControllerParameters->om; - double zeta = pointingModeControllerParameters->zeta; - double qErrorMin = pointingModeControllerParameters->qiMin; - double omMax = pointingModeControllerParameters->omMax; + double gain1 = cInt * omMax / qErrorLawNorm; + double gainVector[3] = {0, 0, 0}; + VectorOperations::mulScalar(qErrorLaw, gain1, gainVector, 3); - double cInt = 2 * om * zeta; - double kInt = 2 * pow(om,2); + double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + gainMatrixDiagonal[0][0] = gainVector[0]; + gainMatrixDiagonal[1][1] = gainVector[1]; + gainMatrixDiagonal[2][2] = gainVector[2]; + MatrixOperations::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), + *gainMatrix, 3, 3, 3); - double qErrorLaw[3] = {0, 0, 0}; + // Inverse of gainMatrix + double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; + gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; + gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; - for (int i = 0; i < 3; i++) { - if (abs(qError[i]) < qErrorMin) { - qErrorLaw[i] = qErrorMin; - } - else { - qErrorLaw[i] = abs(qError[i]); - } - } - double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); + double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, + 3, 3); + MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); - double gain1 = cInt * omMax / qErrorLawNorm; - double gainVector[3] = {0, 0, 0}; - VectorOperations::mulScalar(qErrorLaw, gain1, gainVector, 3); + //------------------------------------------------------------------------------------------------ + // Torque Calculations for the reaction wheels + //------------------------------------------------------------------------------------------------ - double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - gainMatrixDiagonal[0][0] = gainVector[0]; - gainMatrixDiagonal[1][1] = gainVector[1]; - gainMatrixDiagonal[2][2] = gainVector[2]; - MatrixOperations::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3); + double pError[3] = {0, 0, 0}; + MatrixOperations::multiply(*pMatrix, qError, pError, 3, 3, 1); + double pErrorSign[3] = {0, 0, 0}; - // Inverse of gainMatrix - double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; - gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; - gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; + 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); - double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3); - MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); - - //------------------------------------------------------------------------------------------------ - // Torque Calculations for the reaction wheels - //------------------------------------------------------------------------------------------------ - - double pError[3] = {0, 0, 0}; - MatrixOperations::multiply(*pMatrix, qError, pError, 3, 3, 1); - 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); - -// 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); + // 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); } void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, diff --git a/mission/controller/acs/util/CholeskyDecomposition.h b/mission/controller/acs/util/CholeskyDecomposition.h index 38ce0bcc..667f9f63 100644 --- a/mission/controller/acs/util/CholeskyDecomposition.h +++ b/mission/controller/acs/util/CholeskyDecomposition.h @@ -8,95 +8,91 @@ #ifndef CHOLESKYDECOMPOSITION_H_ #define CHOLESKYDECOMPOSITION_H_ #include -//typedef unsigned int uint8_t; +// typedef unsigned int uint8_t; -template +template class CholeskyDecomposition { -public: - static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) - { - // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c - return cholsl(matrix, result, tempMatrix, dimension); - } -private: - // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c - static uint8_t choldc1(double * a, double * p, uint8_t n) { - int8_t i,j,k; - double sum; + public: + static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) { + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + return cholsl(matrix, result, tempMatrix, dimension); + } - for (i = 0; i < n; i++) { - for (j = i; j < n; j++) { - sum = a[i*n+j]; - for (k = i - 1; k >= 0; k--) { - sum -= a[i*n+k] * a[j*n+k]; - } - if (i == j) { - if (sum <= 0) { - return 1; /* error */ - } - p[i] = sqrt(sum); - } - else { - a[j*n+i] = sum / p[i]; - } - } - } + private: + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t choldc1(double *a, double *p, uint8_t n) { + int8_t i, j, k; + double sum; - return 0; /* success */ - } + for (i = 0; i < n; i++) { + for (j = i; j < n; j++) { + sum = a[i * n + j]; + for (k = i - 1; k >= 0; k--) { + sum -= a[i * n + k] * a[j * n + k]; + } + if (i == j) { + if (sum <= 0) { + return 1; /* error */ + } + p[i] = sqrt(sum); + } else { + a[j * n + i] = sum / p[i]; + } + } + } - // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c - static uint8_t choldcsl(double * A, double * a, double * p, uint8_t n) - { - uint8_t i,j,k; double sum; - for (i = 0; i < n; i++) - for (j = 0; j < n; j++) - a[i*n+j] = A[i*n+j]; - if (choldc1(a, p, n)) return 1; - for (i = 0; i < n; i++) { - a[i*n+i] = 1 / p[i]; - for (j = i + 1; j < n; j++) { - sum = 0; - for (k = i; k < j; k++) { - sum -= a[j*n+k] * a[k*n+i]; - } - a[j*n+i] = sum / p[j]; - } - } + return 0; /* success */ + } - return 0; /* success */ - } + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) { + uint8_t i, j, k; + double sum; + for (i = 0; i < n; i++) + for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j]; + if (choldc1(a, p, n)) return 1; + for (i = 0; i < n; i++) { + a[i * n + i] = 1 / p[i]; + for (j = i + 1; j < n; j++) { + sum = 0; + for (k = i; k < j; k++) { + sum -= a[j * n + k] * a[k * n + i]; + } + a[j * n + i] = sum / p[j]; + } + } + return 0; /* success */ + } - // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c - static uint8_t cholsl(double * A,double * a,double * p, uint8_t n) - { - uint8_t i,j,k; - if (choldcsl(A,a,p,n)) return 1; - for (i = 0; i < n; i++) { - for (j = i + 1; j < n; j++) { - a[i*n+j] = 0.0; - } - } - for (i = 0; i < n; i++) { - a[i*n+i] *= a[i*n+i]; - for (k = i + 1; k < n; k++) { - a[i*n+i] += a[k*n+i] * a[k*n+i]; - } - for (j = i + 1; j < n; j++) { - for (k = j; k < n; k++) { - a[i*n+j] += a[k*n+i] * a[k*n+j]; - } - } - } - for (i = 0; i < n; i++) { - for (j = 0; j < i; j++) { - a[i*n+j] = a[j*n+i]; - } - } + // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c + static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) { + uint8_t i, j, k; + if (choldcsl(A, a, p, n)) return 1; + for (i = 0; i < n; i++) { + for (j = i + 1; j < n; j++) { + a[i * n + j] = 0.0; + } + } + for (i = 0; i < n; i++) { + a[i * n + i] *= a[i * n + i]; + for (k = i + 1; k < n; k++) { + a[i * n + i] += a[k * n + i] * a[k * n + i]; + } + for (j = i + 1; j < n; j++) { + for (k = j; k < n; k++) { + a[i * n + j] += a[k * n + i] * a[k * n + j]; + } + } + } + for (i = 0; i < n; i++) { + for (j = 0; j < i; j++) { + a[i * n + j] = a[j * n + i]; + } + } - return 0; /* success */ - } + return 0; /* success */ + } }; #endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */ From 0bbdd21d8176a2bb2c37c7aed6bf10fad5e45bc6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Dec 2022 15:58:39 +0100 Subject: [PATCH 238/244] bump changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a4df75d5..7a0258d1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,12 @@ list yields a list of all related PRs for each release. - Renamed `/dev/ul-plsv` to `/dev/ploc_supv`, is not a UART lite anymore - Renamed `/dev/i2c_eive` to `/dev/i2c_pl` and `/dev/i2c-2` to `/dev/i2c_ps`. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/328 + +## Added + +- First version of ACS controller + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/329 + # [v1.17.0] 28.11.2022 ## Added From b38afa5df3ce79833c0cc91c02b6dc6096c4becf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Dec 2022 16:01:50 +0100 Subject: [PATCH 239/244] repoint json dependency --- thirdparty/json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thirdparty/json b/thirdparty/json index fb1ee4f9..bc889afb 160000 --- a/thirdparty/json +++ b/thirdparty/json @@ -1 +1 @@ -Subproject commit fb1ee4f94b426a398969b2c96df9784be8e007e6 +Subproject commit bc889afb4c5bf1c0d8ee29ef35eaaf4c8bef8a5d From a726f7535def0e8e82480b190b3b650619f79500 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Dec 2022 16:21:29 +0100 Subject: [PATCH 240/244] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 6308bbb3..6e152818 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6308bbb3f0e4387508371bd3d18c52f404c4e18b +Subproject commit 6e152818707c7a12be8900bc239a0073053c0bec From 7d99d8e80507ad3e26d884d27ea5d331ef0f9b86 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Dec 2022 16:30:39 +0100 Subject: [PATCH 241/244] bump minor version and changelog --- CHANGELOG.md | 3 ++- CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a4df75d5..b52806a0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,7 +8,7 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/). The [milestone](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/milestones) list yields a list of all related PRs for each release. -# [unreleased] +# [v1.18.0] 01.12.2022 ## Changed @@ -16,6 +16,7 @@ list yields a list of all related PRs for each release. - Renamed `/dev/ul-plsv` to `/dev/ploc_supv`, is not a UART lite anymore - Renamed `/dev/i2c_eive` to `/dev/i2c_pl` and `/dev/i2c-2` to `/dev/i2c_ps`. PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/328 + # [v1.17.0] 28.11.2022 ## Added diff --git a/CMakeLists.txt b/CMakeLists.txt index 5111b9ed..d87c2695 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR_IF_GIT_FAILS 1) -set(OBSW_VERSION_MINOR_IF_GIT_FAILS 17) +set(OBSW_VERSION_MINOR_IF_GIT_FAILS 18) set(OBSW_VERSION_REVISION_IF_GIT_FAILS 0) # set(CMAKE_VERBOSE TRUE) From 1d0a13a79d0e481815642af59267dd3be7ab7f8b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Dec 2022 17:08:03 +0100 Subject: [PATCH 242/244] update generated files --- generators/bsp_q7s_returnvalues.csv | 556 +++++++++--------- generators/events/translateEvents.cpp | 2 +- generators/objects/translateObjects.cpp | 2 +- linux/fsfwconfig/events/translateEvents.cpp | 2 +- linux/fsfwconfig/objects/translateObjects.cpp | 2 +- tmtc | 2 +- 6 files changed, 283 insertions(+), 283 deletions(-) diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index e64eed14..939d3ede 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -2,12 +2,13 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h 0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h -0x5d00;GOMS_PacketTooLong;;0;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d01;GOMS_InvalidTableId;;1;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d02;GOMS_InvalidAddress;;2;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d03;GOMS_InvalidParamSize;;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d04;GOMS_InvalidPayloadSize;;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h -0x5d05;GOMS_UnknownReplyId;;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x66a0;SADPL_CommandNotSupported;;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a1;SADPL_DeploymentAlreadyExecuting;;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a2;SADPL_MainSwitchTimeoutFailure;;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a3;SADPL_SwitchingDeplSa1Failed;;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x66a4;SADPL_SwitchingDeplSa2Failed;;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h +0x58a0;SUSS_ErrorUnlockMutex;;160;SUS_HANDLER;mission/devices/SusHandler.h +0x58a1;SUSS_ErrorLockMutex;;161;SUS_HANDLER;mission/devices/SusHandler.h 0x52b0;RWHA_SpiWriteFailure;;176;RW_HANDLER;mission/devices/RwHandler.h 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/RwHandler.h 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/RwHandler.h @@ -25,8 +26,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x4fa3;HEATER_InvalidSwitchNr;;163;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa4;HEATER_MainSwitchSetTimeout;;164;HEATER_HANDLER;mission/devices/HeaterHandler.h 0x4fa5;HEATER_CommandAlreadyWaiting;;165;HEATER_HANDLER;mission/devices/HeaterHandler.h -0x58a0;SUSS_ErrorUnlockMutex;;160;SUS_HANDLER;mission/devices/SusHandler.h -0x58a1;SUSS_ErrorLockMutex;;161;SUS_HANDLER;mission/devices/SusHandler.h +0x5d00;GOMS_PacketTooLong;;0;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d01;GOMS_InvalidTableId;;1;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d02;GOMS_InvalidAddress;;2;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d03;GOMS_InvalidParamSize;;3;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d04;GOMS_InvalidPayloadSize;;4;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h +0x5d05;GOMS_UnknownReplyId;;5;GOM_SPACE_HANDLER;mission/devices/GomspaceDeviceHandler.h 0x51a0;IMTQ_InvalidCommandCode;;160;IMTQ_HANDLER;mission/devices/ImtqHandler.h 0x51a1;IMTQ_ParameterMissing;;161;IMTQ_HANDLER;mission/devices/ImtqHandler.h 0x51a2;IMTQ_ParameterInvalid;;162;IMTQ_HANDLER;mission/devices/ImtqHandler.h @@ -44,42 +49,133 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x50a6;SYRLINKS_BadCrcAck;;166;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h 0x50a7;SYRLINKS_ReplyWrongSize;;167;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h 0x50a8;SYRLINKS_MissingStartFrameCharacter;;168;SYRLINKS_HANDLER;mission/devices/SyrlinksHkHandler.h -0x66a0;SADPL_CommandNotSupported;;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a1;SADPL_DeploymentAlreadyExecuting;;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a2;SADPL_MainSwitchTimeoutFailure;;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a3;SADPL_SwitchingDeplSa1Failed;;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h -0x66a4;SADPL_SwitchingDeplSa2Failed;;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h -0x2c01;CCS_BcIsSetVrCommand;;1;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2c02;CCS_BcIsUnlockCommand;;2;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cb0;CCS_BcIllegalCommand;;176;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cb1;CCS_BoardReadingNotFinished;;177;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf0;CCS_NsPositiveW;;240;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf1;CCS_NsNegativeW;;241;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf2;CCS_NsLockout;;242;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf3;CCS_FarmInLockout;;243;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cf4;CCS_FarmInWait;;244;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce0;CCS_WrongSymbol;;224;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce1;CCS_DoubleStart;;225;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce2;CCS_StartSymbolMissed;;226;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce3;CCS_EndWithoutStart;;227;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce4;CCS_TooLarge;;228;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce5;CCS_TooShort;;229;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce6;CCS_WrongTfVersion;;230;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce7;CCS_WrongSpacecraftId;;231;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce8;CCS_NoValidFrameType;;232;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ce9;CCS_CrcFailed;;233;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cea;CCS_VcNotFound;;234;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ceb;CCS_ForwardingFailed;;235;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cec;CCS_ContentTooLarge;;236;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2ced;CCS_ResidualData;;237;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cee;CCS_DataCorrupted;;238;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cef;CCS_IllegalSegmentationFlag;;239;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd0;CCS_IllegalFlagCombination;;208;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd1;CCS_ShorterThanHeader;;209;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd2;CCS_TooShortBlockedPacket;;210;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x2cd3;CCS_TooShortMapExtraction;;211;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h -0x3b00;SPH_ConnBroken;;0;SEMAPHORE_IF;fsfw/src/fsfw/osal/common/TcpTmTcServer.h +0x3c00;LPIF_PoolEntryNotFound;;0;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h +0x3c01;LPIF_PoolEntryTypeConflict;;1;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h +0x3e00;HKM_QueueOrDestinationInvalid;;0;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e01;HKM_WrongHkPacketType;;1;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e02;HKM_ReportingStatusUnchanged;;2;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e03;HKM_PeriodicHelperInvalid;;3;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e04;HKM_PoolobjectNotFound;;4;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3e05;HKM_DatasetNotFound;;5;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h +0x3601;CFDP_InvalidTlvType;;1;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3602;CFDP_InvalidDirectiveField;;2;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3603;CFDP_InvalidPduDatafieldLen;;3;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3604;CFDP_InvalidAckDirectiveFields;;4;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3605;CFDP_MetadataCantParseOptions;;5;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3606;CFDP_NakCantParseOptions;;6;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3607;CFDP_FinishedCantParseFsResponses;;7;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3608;CFDP_FilestoreRequiresSecondFile;;8;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3609;CFDP_FilestoreResponseCantParseFsMessage;;9;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x360a;CFDP_InvalidPduFormat;;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h +0x3201;CF_ObjectHasNoFunctions;;1;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h +0x3202;CF_AlreadyCommanding;;2;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h +0x3301;HF_IsBusy;;1;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3302;HF_InvalidParameters;;2;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3303;HF_ExecutionFinished;;3;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x3304;HF_InvalidActionId;;4;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h +0x1e00;PUS_InvalidPusVersion;;0;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h +0x1e01;PUS_InvalidCrc16;;1;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h +0x2e01;HPA_InvalidIdentifierId;;1;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e02;HPA_InvalidDomainId;;2;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e03;HPA_InvalidValue;;3;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2e05;HPA_ReadOnly;;5;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h +0x2d01;PAW_UnknownDatatype;;1;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d02;PAW_DatatypeMissmatch;;2;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d03;PAW_Readonly;;3;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d04;PAW_TooBig;;4;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d05;PAW_SourceNotSet;;5;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d06;PAW_OutOfBounds;;6;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d07;PAW_NotSet;;7;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x2d08;PAW_ColumnOrRowsZero;;8;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h +0x1701;HHI_ObjectNotHealthy;;1;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x1702;HHI_InvalidHealthState;;2;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x1703;HHI_IsExternallyControlled;;3;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h +0x2801;SM_DataTooLarge;;1;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2802;SM_DataStorageFull;;2;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2803;SM_IllegalStorageId;;3;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2804;SM_DataDoesNotExist;;4;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2805;SM_IllegalAddress;;5;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2806;SM_PoolTooLarge;;6;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2401;MT_TooDetailedRequest;;1;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2402;MT_TooGeneralRequest;;2;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2403;MT_NoMatch;;3;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2404;MT_Full;;4;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x2405;MT_NewNodeCreated;;5;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h +0x3f01;DLEE_StreamTooShort;;1;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h +0x3f02;DLEE_DecodingError;;2;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h +0x2f01;ASC_NoPacketFound;;1;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/DleParser.h +0x2f02;ASC_PossiblePacketLoss;;2;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/DleParser.h +0x2f03;ASC_BufferTooSmall;;3;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h +0x4201;PUS11_InvalidTypeTimeWindow;;1;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h +0x4202;PUS11_InvalidTimeWindow;;2;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h +0x4203;PUS11_TimeshiftingNotPossible;;3;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h +0x4204;PUS11_InvalidRelativeTime;;4;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h +0x1c01;TCD_PacketLost;;1;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1c02;TCD_DestinationNotFound;;2;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1c03;TCD_ServiceIdAlreadyExists;;3;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h +0x1b00;TCC_NoDestinationFound;;0;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b01;TCC_InvalidCcsdsVersion;;1;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b02;TCC_InvalidApid;;2;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b03;TCC_InvalidPacketType;;3;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b04;TCC_InvalidSecHeaderField;;4;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b05;TCC_IncorrectPrimaryHeader;;5;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b07;TCC_IncompletePacket;;7;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b08;TCC_InvalidPusVersion;;8;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b09;TCC_IncorrectChecksum;;9;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b0a;TCC_IllegalPacketSubtype;;10;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x1b0b;TCC_IncorrectSecondaryHeader;;11;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h +0x3a01;MQI_Empty;;1;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a02;MQI_Full;No space left for more messages;2;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a03;MQI_NoReplyPartner;Returned if a reply method was called without partner;3;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x3a04;MQI_DestinationInvalid;Returned if the target destination is invalid.;4;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h +0x0f01;CM_UnknownCommand;;1;COMMAND_MESSAGE;fsfw/src/fsfw/ipc/CommandMessageIF.h +0x3901;MUX_NotEnoughResources;;1;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3902;MUX_InsufficientMemory;;2;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3903;MUX_NoPrivilege;;3;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3904;MUX_WrongAttributeSetting;;4;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3905;MUX_MutexAlreadyLocked;;5;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3906;MUX_MutexNotFound;;6;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3907;MUX_MutexMaxLocks;;7;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3908;MUX_CurrThreadAlreadyOwnsMutex;;8;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x3909;MUX_CurrThreadDoesNotOwnMutex;;9;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390a;MUX_MutexTimeout;;10;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390b;MUX_MutexInvalidId;;11;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x390c;MUX_MutexDestroyedWhileWaiting;;12;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h +0x2901;TC_InvalidTargetState;;1;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x29f1;TC_AboveOperationalLimit;;241;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x29f2;TC_BelowOperationalLimit;;242;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h +0x0801;DPS_InvalidParameterDefinition;;1;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h +0x0802;DPS_SetWasAlreadyRead;;2;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h +0x0803;DPS_CommitingWithoutReading;;3;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h +0x0804;DPS_DataSetUninitialised;;4;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h +0x0805;DPS_DataSetFull;;5;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h +0x0806;DPS_PoolVarNull;;6;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h +0x3da0;PVA_InvalidReadWriteMode;;160;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h +0x3da1;PVA_InvalidPoolEntry;;161;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h +0x0c02;MS_InvalidEntry;;2;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0c03;MS_TooManyElements;;3;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0c04;MS_CantStoreEmpty;;4;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h +0x0d01;SS_SequenceAlreadyExists;;1;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d02;SS_TableAlreadyExists;;2;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d03;SS_TableDoesNotExist;;3;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d04;SS_TableOrSequenceLengthInvalid;;4;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d05;SS_SequenceDoesNotExist;;5;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d06;SS_TableContainsInvalidObjectId;;6;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d07;SS_FallbackSequenceDoesNotExist;;7;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d08;SS_NoTargetTable;;8;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d09;SS_SequenceOrTableTooLong;;9;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0b;SS_IsFallbackSequence;;11;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0c;SS_AccessDenied;;12;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0d0e;SS_TableInUse;;14;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0da1;SS_TargetTableNotReached;;161;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0da2;SS_TableCheckFailed;;162;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h +0x0b01;SB_ChildNotFound;;1;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b02;SB_ChildInfoUpdated;;2;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b03;SB_ChildDoesntHaveModes;;3;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b04;SB_CouldNotInsertChild;;4;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b05;SB_TableContainsInvalidObjectId;;5;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h +0x0b00;SB_ConnBroken;;0;SUBSYSTEM_BASE;fsfw/src/fsfw/osal/common/TcpTmTcServer.h 0x2a01;IEC_NoConfigurationTable;;1;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h 0x2a02;IEC_NoCpuTable;;2;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h 0x2a03;IEC_InvalidWorkspaceAddress;;3;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h @@ -101,73 +197,26 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x2a13;IEC_ImplementationBlockingOperationCancel;;19;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h 0x2a14;IEC_MutexObtainFromBadState;;20;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h 0x2a15;IEC_UnlimitedAndMaximumIs0;;21;INTERNAL_ERROR_CODES;fsfw/src/fsfw/osal/InternalErrorCodes.h -0x0e01;HM_InvalidMode;;1;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h -0x0e02;HM_TransNotAllowed;;2;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h -0x0e03;HM_InTransition;;3;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h -0x0e04;HM_InvalidSubmode;;4;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h -0x2e01;HPA_InvalidIdentifierId;;1;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e02;HPA_InvalidDomainId;;2;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e03;HPA_InvalidValue;;3;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2e05;HPA_ReadOnly;;5;HAS_PARAMETERS_IF;fsfw/src/fsfw/parameters/HasParametersIF.h -0x2d01;PAW_UnknownDatatype;;1;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d02;PAW_DatatypeMissmatch;;2;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d03;PAW_Readonly;;3;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d04;PAW_TooBig;;4;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d05;PAW_SourceNotSet;;5;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d06;PAW_OutOfBounds;;6;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d07;PAW_NotSet;;7;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x2d08;PAW_ColumnOrRowsZero;;8;PARAMETER_WRAPPER;fsfw/src/fsfw/parameters/ParameterWrapper.h -0x3201;CF_ObjectHasNoFunctions;;1;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h -0x3202;CF_AlreadyCommanding;;2;COMMANDS_ACTIONS_IF;fsfw/src/fsfw/action/CommandsActionsIF.h -0x3301;HF_IsBusy;;1;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3302;HF_InvalidParameters;;2;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3303;HF_ExecutionFinished;;3;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x3304;HF_InvalidActionId;;4;HAS_ACTIONS_IF;fsfw/src/fsfw/action/HasActionsIF.h -0x0201;OM_InsertionFailed;;1;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0202;OM_NotFound;;2;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0203;OM_ChildInitFailed;;3;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h -0x0204;OM_InternalErrReporterUninit;;4;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x1401;SE_BufferTooShort;;1;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x1402;SE_StreamTooShort;;2;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x1403;SE_TooManyElements;;3;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h +0x4c00;SPPA_NoPacketFound;;0;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h +0x4c01;SPPA_SplitPacket;;1;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h +0x1d01;ATC_ActivityStarted;;1;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d02;ATC_InvalidSubservice;;2;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d03;ATC_IllegalApplicationData;;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d04;ATC_SendTmFailed;;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x1d05;ATC_Timeout;;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h +0x2001;CSB_ExecutionComplete;;1;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2002;CSB_NoStepMessage;;2;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2003;CSB_ObjectBusy;;3;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2004;CSB_Busy;;4;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2005;CSB_InvalidTc;;5;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2006;CSB_InvalidObject;;6;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h +0x2007;CSB_InvalidReply;;7;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h 0x2600;FDI_YourFault;;0;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h 0x2601;FDI_MyFault;;1;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h 0x2602;FDI_ConfirmLater;;2;HANDLES_FAILURES_IF;fsfw/src/fsfw/fdir/ConfirmsFailuresIF.h -0x2201;TMF_Busy;;1;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2202;TMF_LastPacketFound;;2;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2203;TMF_StopFetch;;3;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2204;TMF_Timeout;;4;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2205;TMF_TmChannelFull;;5;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2206;TMF_NotStored;;6;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2207;TMF_AllDeleted;;7;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2208;TMF_InvalidData;;8;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2209;TMF_NotReady;;9;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h -0x2101;TMB_Busy;;1;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2102;TMB_Full;;2;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2103;TMB_Empty;;3;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2104;TMB_NullRequested;;4;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2105;TMB_TooLarge;;5;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2106;TMB_NotReady;;6;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2107;TMB_DumpError;;7;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2108;TMB_CrcError;;8;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x2109;TMB_Timeout;;9;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210a;TMB_IdlePacketFound;;10;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210b;TMB_TelecommandFound;;11;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210c;TMB_NoPusATm;;12;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210d;TMB_TooSmall;;13;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210e;TMB_BlockNotFound;;14;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x210f;TMB_InvalidRequest;;15;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h -0x1c01;TCD_PacketLost;;1;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1c02;TCD_DestinationNotFound;;2;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1c03;TCD_ServiceIdAlreadyExists;;3;PACKET_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/TcDistributorBase.h -0x1b00;TCC_NoDestinationFound;;0;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b01;TCC_InvalidCcsdsVersion;;1;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b02;TCC_InvalidApid;;2;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b03;TCC_InvalidPacketType;;3;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b04;TCC_InvalidSecHeaderField;;4;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b05;TCC_IncorrectPrimaryHeader;;5;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b07;TCC_IncompletePacket;;7;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b08;TCC_InvalidPusVersion;;8;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b09;TCC_IncorrectChecksum;;9;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b0a;TCC_IllegalPacketSubtype;;10;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h -0x1b0b;TCC_IncorrectSecondaryHeader;;11;TMTC_DISTRIBUTION;fsfw/src/fsfw/tcdistribution/definitions.h 0x04e1;RMP_CommandNoDescriptorsAvailable;;225;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x04e2;RMP_CommandBufferFull;;226;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x04e3;RMP_CommandChannelOutOfRange;;227;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h @@ -208,12 +257,35 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x040a;RMP_ReplyCommandNotImplementedOrNotAuthorised;;10;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x040b;RMP_ReplyRmwDataLengthError;;11;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h 0x040c;RMP_ReplyInvalidTargetLogicalAddress;;12;RMAP_CHANNEL;fsfw/src/fsfw/rmap/RMAP.h -0x2801;SM_DataTooLarge;;1;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2802;SM_DataStorageFull;;2;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2803;SM_IllegalStorageId;;3;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2804;SM_DataDoesNotExist;;4;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2805;SM_IllegalAddress;;5;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h -0x2806;SM_PoolTooLarge;;6;STORAGE_MANAGER_IF;fsfw/src/fsfw/storagemanager/StorageManagerIF.h +0x2c01;CCS_BcIsSetVrCommand;;1;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2c02;CCS_BcIsUnlockCommand;;2;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cb0;CCS_BcIllegalCommand;;176;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cb1;CCS_BoardReadingNotFinished;;177;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf0;CCS_NsPositiveW;;240;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf1;CCS_NsNegativeW;;241;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf2;CCS_NsLockout;;242;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf3;CCS_FarmInLockout;;243;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cf4;CCS_FarmInWait;;244;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce0;CCS_WrongSymbol;;224;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce1;CCS_DoubleStart;;225;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce2;CCS_StartSymbolMissed;;226;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce3;CCS_EndWithoutStart;;227;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce4;CCS_TooLarge;;228;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce5;CCS_TooShort;;229;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce6;CCS_WrongTfVersion;;230;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce7;CCS_WrongSpacecraftId;;231;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce8;CCS_NoValidFrameType;;232;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ce9;CCS_CrcFailed;;233;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cea;CCS_VcNotFound;;234;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ceb;CCS_ForwardingFailed;;235;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cec;CCS_ContentTooLarge;;236;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2ced;CCS_ResidualData;;237;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cee;CCS_DataCorrupted;;238;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cef;CCS_IllegalSegmentationFlag;;239;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd0;CCS_IllegalFlagCombination;;208;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd1;CCS_ShorterThanHeader;;209;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd2;CCS_TooShortBlockedPacket;;210;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h +0x2cd3;CCS_TooShortMapExtraction;;211;CCSDS_HANDLER_IF;fsfw/src/fsfw/datalinklayer/CCSDSReturnValuesIF.h 0x38a1;SGP4_InvalidEccentricity;;161;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h 0x38a2;SGP4_InvalidMeanMotion;;162;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h 0x38a3;SGP4_InvalidPerturbationElements;;163;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h @@ -222,125 +294,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x38a6;SGP4_SatelliteHasDecayed;;166;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h 0x38b1;SGP4_TleTooOld;;177;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h 0x38b2;SGP4_TleNotInitialized;;178;SGP4PROPAGATOR_CLASS;fsfw/src/fsfw/coordinates/Sgp4Propagator.h -0x2401;MT_NoPacketFound;;1;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/DleParser.h -0x2402;MT_PossiblePacketLoss;;2;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/DleParser.h -0x2403;MT_NoMatch;;3;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2404;MT_Full;;4;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x2405;MT_NewNodeCreated;;5;MATCH_TREE_CLASS;fsfw/src/fsfw/globalfunctions/matching/MatchTree.h -0x3f01;DLEE_StreamTooShort;;1;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h -0x3f02;DLEE_DecodingError;;2;DLE_ENCODER;fsfw/src/fsfw/globalfunctions/DleEncoder.h -0x2f01;ASC_TooLongForTargetType;;1;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x2f02;ASC_InvalidCharacters;;2;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x2f03;ASC_BufferTooSmall;;3;ASCII_CONVERTER;fsfw/src/fsfw/globalfunctions/AsciiConverter.h -0x0f01;CM_UnknownCommand;;1;COMMAND_MESSAGE;fsfw/src/fsfw/ipc/CommandMessageIF.h -0x3a01;MQI_Empty;;1;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a02;MQI_Full;No space left for more messages;2;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a03;MQI_NoReplyPartner;Returned if a reply method was called without partner;3;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3a04;MQI_DestinationInvalid;Returned if the target destination is invalid.;4;MESSAGE_QUEUE_IF;fsfw/src/fsfw/ipc/MessageQueueIF.h -0x3901;MUX_NotEnoughResources;;1;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3902;MUX_InsufficientMemory;;2;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3903;MUX_NoPrivilege;;3;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3904;MUX_WrongAttributeSetting;;4;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3905;MUX_MutexAlreadyLocked;;5;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3906;MUX_MutexNotFound;;6;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3907;MUX_MutexMaxLocks;;7;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3908;MUX_CurrThreadAlreadyOwnsMutex;;8;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3909;MUX_CurrThreadDoesNotOwnMutex;;9;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390a;MUX_MutexTimeout;;10;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390b;MUX_MutexInvalidId;;11;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x390c;MUX_MutexDestroyedWhileWaiting;;12;MUTEX_IF;fsfw/src/fsfw/ipc/MutexIF.h -0x3b01;SPH_SemaphoreTimeout;;1;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x3b02;SPH_SemaphoreNotOwned;;2;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x3b03;SPH_SemaphoreInvalid;;3;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h -0x1e00;PUS_InvalidPusVersion;;0;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h -0x1e01;PUS_InvalidCrc16;;1;PUS_IF;fsfw/src/fsfw/tmtcpacket/pus/PusIF.h -0x3601;CFDP_InvalidTlvType;;1;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3602;CFDP_InvalidDirectiveField;;2;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3603;CFDP_InvalidPduDatafieldLen;;3;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3604;CFDP_InvalidAckDirectiveFields;;4;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3605;CFDP_MetadataCantParseOptions;;5;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3606;CFDP_NakCantParseOptions;;6;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3607;CFDP_FinishedCantParseFsResponses;;7;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3608;CFDP_FilestoreRequiresSecondFile;;8;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x3609;CFDP_FilestoreResponseCantParseFsMessage;;9;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x360a;CFDP_InvalidPduFormat;;10;CFDP;fsfw/src/fsfw/cfdp/definitions.h -0x2901;TC_InvalidTargetState;;1;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x29f1;TC_AboveOperationalLimit;;241;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x29f2;TC_BelowOperationalLimit;;242;THERMAL_COMPONENT_IF;fsfw/src/fsfw/thermal/ThermalComponentIF.h -0x0c02;MS_InvalidEntry;;2;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0c03;MS_TooManyElements;;3;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0c04;MS_CantStoreEmpty;;4;MODE_STORE_IF;fsfw/src/fsfw/subsystem/modes/ModeStoreIF.h -0x0d01;SS_SequenceAlreadyExists;;1;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d02;SS_TableAlreadyExists;;2;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d03;SS_TableDoesNotExist;;3;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d04;SS_TableOrSequenceLengthInvalid;;4;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d05;SS_SequenceDoesNotExist;;5;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d06;SS_TableContainsInvalidObjectId;;6;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d07;SS_FallbackSequenceDoesNotExist;;7;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d08;SS_NoTargetTable;;8;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d09;SS_SequenceOrTableTooLong;;9;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0b;SS_IsFallbackSequence;;11;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0c;SS_AccessDenied;;12;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0d0e;SS_TableInUse;;14;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0da1;SS_TargetTableNotReached;;161;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0da2;SS_TableCheckFailed;;162;SUBSYSTEM;fsfw/src/fsfw/subsystem/Subsystem.h -0x0b01;SB_ChildNotFound;;1;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b02;SB_ChildInfoUpdated;;2;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b03;SB_ChildDoesntHaveModes;;3;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b04;SB_CouldNotInsertChild;;4;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x0b05;SB_TableContainsInvalidObjectId;;5;SUBSYSTEM_BASE;fsfw/src/fsfw/subsystem/SubsystemBase.h -0x3e00;HKM_QueueOrDestinationInvalid;;0;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e01;HKM_WrongHkPacketType;;1;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e02;HKM_ReportingStatusUnchanged;;2;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e03;HKM_PeriodicHelperInvalid;;3;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e04;HKM_PoolobjectNotFound;;4;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3e05;HKM_DatasetNotFound;;5;HOUSEKEEPING_MANAGER;fsfw/src/fsfw/datapoollocal/LocalDataPoolManager.h -0x3c00;LPIF_PoolEntryNotFound;;0;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h -0x3c01;LPIF_PoolEntryTypeConflict;;1;LOCAL_POOL_OWNER_IF;fsfw/src/fsfw/datapoollocal/localPoolDefinitions.h -0x3da0;PVA_InvalidReadWriteMode;;160;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h -0x3da1;PVA_InvalidPoolEntry;;161;POOL_VARIABLE_IF;fsfw/src/fsfw/datapool/PoolVariableIF.h -0x0801;DPS_InvalidParameterDefinition;;1;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x0802;DPS_SetWasAlreadyRead;;2;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x0803;DPS_CommitingWithoutReading;;3;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x0804;DPS_DataSetUninitialised;;4;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x0805;DPS_DataSetFull;;5;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x0806;DPS_PoolVarNull;;6;DATA_SET_CLASS;fsfw/src/fsfw/datapool/DataSetIF.h -0x1000;TIM_UnsupportedTimeFormat;;0;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1001;TIM_NotEnoughInformationForTargetFormat;;1;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1002;TIM_LengthMismatch;;2;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1003;TIM_InvalidTimeFormat;;3;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1004;TIM_InvalidDayOfYear;;4;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x1005;TIM_TimeDoesNotFitFormat;;5;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h -0x3701;TSI_BadTimestamp;;1;TIME_STAMPER_IF;fsfw/src/fsfw/timemanager/TimeStampIF.h -0x1d01;ATC_ActivityStarted;;1;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d02;ATC_InvalidSubservice;;2;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d03;ATC_IllegalApplicationData;;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d04;ATC_SendTmFailed;;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x1d05;ATC_Timeout;;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h -0x4c00;SPPA_NoPacketFound;;0;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h -0x4c01;SPPA_SplitPacket;;1;SPACE_PACKET_PARSER;fsfw/src/fsfw/tmtcservices/SpacePacketParser.h -0x2001;CSB_ExecutionComplete;;1;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2002;CSB_NoStepMessage;;2;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2003;CSB_ObjectBusy;;3;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2004;CSB_Busy;;4;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2005;CSB_InvalidTc;;5;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2006;CSB_InvalidObject;;6;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x2007;CSB_InvalidReply;;7;COMMAND_SERVICE_BASE;fsfw/src/fsfw/tmtcservices/CommandingServiceBase.h -0x1801;FF_Full;;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h -0x1802;FF_Empty;;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h -0x1601;FMM_MapFull;;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h -0x1602;FMM_KeyDoesNotExist;;2;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h -0x2501;EV_ListenerNotFound;;1;EVENT_MANAGER_IF;fsfw/src/fsfw/events/EventManagerIF.h -0x1701;HHI_ObjectNotHealthy;;1;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x1702;HHI_InvalidHealthState;;2;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x1703;HHI_IsExternallyControlled;;3;HAS_HEALTH_IF;fsfw/src/fsfw/health/HasHealthIF.h -0x3001;POS_InPowerTransition;;1;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h -0x3002;POS_SwitchStateMismatch;;2;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h -0x0501;PS_SwitchOn;;1;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h -0x0500;PS_SwitchOff;;0;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h -0x0502;PS_SwitchTimeout;;2;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h -0x0503;PS_FuseOn;;3;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h -0x0504;PS_FuseOff;;4;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h 0x4300;FILS_GenericFileError;;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x4301;FILS_GenericDirError;;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x4302;FILS_FilesystemInactive;;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h @@ -358,28 +311,10 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x4318;FILS_DirectoryNotEmpty;;24;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x431e;FILS_SequencePacketMissingWrite;;30;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h 0x431f;FILS_SequencePacketMissingRead;;31;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h -0x1a01;TRC_NotEnoughSensors;;1;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h -0x1a02;TRC_LowestValueOol;;2;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h -0x1a03;TRC_HighestValueOol;;3;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h -0x1a04;TRC_BothValuesOol;;4;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h -0x1a05;TRC_DuplexOol;;5;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h -0x3101;LIM_Unchecked;;1;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3102;LIM_Invalid;;2;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3103;LIM_Unselected;;3;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3104;LIM_BelowLowLimit;;4;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3105;LIM_AboveHighLimit;;5;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3106;LIM_UnexpectedValue;;6;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x3107;LIM_OutOfRange;;7;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x31a0;LIM_FirstSample;;160;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x31e0;LIM_InvalidSize;;224;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x31e1;LIM_WrongType;;225;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x31e2;LIM_WrongPid;;226;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x31e3;LIM_WrongLimitId;;227;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x31ee;LIM_MonitorNotFound;;238;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h -0x4201;PUS11_InvalidTypeTimeWindow;;1;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h -0x4202;PUS11_InvalidTimeWindow;;2;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h -0x4203;PUS11_TimeshiftingNotPossible;;3;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h -0x4204;PUS11_InvalidRelativeTime;;4;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h +0x13e0;MH_UnknownCmd;;224;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e1;MH_InvalidAddress;;225;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e2;MH_InvalidSize;;226;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h +0x13e3;MH_StateMismatch;;227;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h 0x0601;PP_DoItMyself;;1;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h 0x0602;PP_PointsToVariable;;2;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h 0x0603;PP_PointsToMemory;;3;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h @@ -394,16 +329,56 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x06e2;PP_InvalidContent;;226;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h 0x06e3;PP_UnalignedAccess;;227;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h 0x06e4;PP_WriteProtected;;228;HAS_MEMORY_IF;fsfw/src/fsfw/memory/HasMemoryIF.h -0x13e0;MH_UnknownCmd;;224;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e1;MH_InvalidAddress;;225;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e2;MH_InvalidSize;;226;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x13e3;MH_StateMismatch;;227;MEMORY_HELPER;fsfw/src/fsfw/memory/MemoryHelper.h -0x1201;AB_NeedSecondStep;;1;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1202;AB_NeedToReconfigure;;2;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1203;AB_ModeFallback;;3;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1204;AB_ChildNotCommandable;;4;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x1205;AB_NeedToChangeHealth;;5;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h -0x12a1;AB_NotEnoughChildrenInCorrectState;;161;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x0201;OM_InsertionFailed;;1;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0202;OM_NotFound;;2;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0203;OM_ChildInitFailed;;3;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0204;OM_InternalErrReporterUninit;;4;OBJECT_MANAGER_IF;fsfw/src/fsfw/objectmanager/ObjectManagerIF.h +0x0501;PS_SwitchOn;;1;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h +0x0500;PS_SwitchOff;;0;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h +0x0502;PS_SwitchTimeout;;2;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h +0x0503;PS_FuseOn;;3;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h +0x0504;PS_FuseOff;;4;POWER_SWITCH_IF;fsfw/src/fsfw/power/PowerSwitchIF.h +0x3001;POS_InPowerTransition;;1;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h +0x3002;POS_SwitchStateMismatch;;2;POWER_SWITCHER;fsfw/src/fsfw/power/PowerSwitcher.h +0x1000;TIM_UnsupportedTimeFormat;;0;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1001;TIM_NotEnoughInformationForTargetFormat;;1;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1002;TIM_LengthMismatch;;2;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1003;TIM_InvalidTimeFormat;;3;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1004;TIM_InvalidDayOfYear;;4;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x1005;TIM_TimeDoesNotFitFormat;;5;CCSDS_TIME_HELPER_CLASS;fsfw/src/fsfw/timemanager/CCSDSTime.h +0x3701;TSI_BadTimestamp;;1;TIME_STAMPER_IF;fsfw/src/fsfw/timemanager/TimeStampIF.h +0x2201;TMF_Busy;;1;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2202;TMF_LastPacketFound;;2;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2203;TMF_StopFetch;;3;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2204;TMF_Timeout;;4;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2205;TMF_TmChannelFull;;5;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2206;TMF_NotStored;;6;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2207;TMF_AllDeleted;;7;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2208;TMF_InvalidData;;8;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2209;TMF_NotReady;;9;TM_STORE_FRONTEND_IF;fsfw/src/fsfw/tmstorage/TmStoreFrontendIF.h +0x2101;TMB_Busy;;1;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2102;TMB_Full;;2;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2103;TMB_Empty;;3;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2104;TMB_NullRequested;;4;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2105;TMB_TooLarge;;5;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2106;TMB_NotReady;;6;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2107;TMB_DumpError;;7;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2108;TMB_CrcError;;8;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x2109;TMB_Timeout;;9;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210a;TMB_IdlePacketFound;;10;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210b;TMB_TelecommandFound;;11;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210c;TMB_NoPusATm;;12;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210d;TMB_TooSmall;;13;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210e;TMB_BlockNotFound;;14;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x210f;TMB_InvalidRequest;;15;TM_STORE_BACKEND_IF;fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h +0x0e01;HM_InvalidMode;;1;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h +0x0e02;HM_TransNotAllowed;;2;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h +0x0e03;HM_InTransition;;3;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h +0x0e04;HM_InvalidSubmode;;4;HAS_MODES_IF;fsfw/src/fsfw/modes/HasModesIF.h +0x1601;FMM_MapFull;;1;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h +0x1602;FMM_KeyDoesNotExist;;2;FIXED_MULTIMAP;fsfw/src/fsfw/container/FixedOrderedMultimap.h +0x1801;FF_Full;;1;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h +0x1802;FF_Empty;;2;FIFO_CLASS;fsfw/src/fsfw/container/FIFOBase.h 0x03a0;DHB_InvalidChannel;;160;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03b0;DHB_AperiodicReply;;176;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03b1;DHB_IgnoreReplyData;;177;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -413,12 +388,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x03d0;DHB_NoSwitch;;208;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03e0;DHB_ChildTimeout;;224;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h 0x03e1;DHB_SwitchFailed;;225;DEVICE_HANDLER_BASE;fsfw/src/fsfw/devicehandlers/DeviceHandlerBase.h -0x3401;DC_NoReplyReceived;;1;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3402;DC_ProtocolError;;2;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3403;DC_Nullpointer;;3;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3404;DC_InvalidCookieType;;4;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3405;DC_NotActive;;5;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h -0x3406;DC_TooMuchData;;6;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x1201;AB_NeedSecondStep;;1;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1202;AB_NeedToReconfigure;;2;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1203;AB_ModeFallback;;3;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1204;AB_ChildNotCommandable;;4;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x1205;AB_NeedToChangeHealth;;5;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h +0x12a1;AB_NotEnoughChildrenInCorrectState;;161;ASSEMBLY_BASE;fsfw/src/fsfw/devicehandlers/AssemblyBase.h 0x27a0;DHI_NoCommandData;;160;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27a1;DHI_CommandNotSupported;;161;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27a2;DHI_CommandAlreadySent;;162;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h @@ -440,12 +415,37 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x27c3;DHI_DeviceReplyInvalid;;195;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27d0;DHI_InvalidCommandParameter;;208;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h 0x27d1;DHI_InvalidNumberOrLengthOfParameters;;209;DEVICE_HANDLER_IF;fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h -0x1401;SE_BufferTooShort;;1;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x1402;SE_StreamTooShort;;2;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x1403;SE_TooManyElements;;3;SERIALIZE_IF;fsfw/src/fsfw/serialize/SerializeIF.h -0x4500;HSPI_HalTimeoutRetval;;0;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h -0x4501;HSPI_HalBusyRetval;;1;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h -0x4502;HSPI_HalErrorRetval;;2;HAL_SPI;fsfw/src/fsfw_hal/stm32h7/spi/spiDefinitions.h +0x3401;DC_NoReplyReceived;;1;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3402;DC_ProtocolError;;2;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3403;DC_Nullpointer;;3;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3404;DC_InvalidCookieType;;4;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3405;DC_NotActive;;5;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x3406;DC_TooMuchData;;6;DEVICE_COMMUNICATION_IF;fsfw/src/fsfw/devicehandlers/DeviceCommunicationIF.h +0x2501;EV_ListenerNotFound;;1;EVENT_MANAGER_IF;fsfw/src/fsfw/events/EventManagerIF.h +0x1a01;TRC_NotEnoughSensors;;1;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h +0x1a02;TRC_LowestValueOol;;2;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h +0x1a03;TRC_HighestValueOol;;3;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h +0x1a04;TRC_BothValuesOol;;4;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h +0x1a05;TRC_DuplexOol;;5;TRIPLE_REDUNDACY_CHECK;fsfw/src/fsfw/monitoring/TriplexMonitor.h +0x3101;LIM_Unchecked;;1;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x3102;LIM_Invalid;;2;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x3103;LIM_Unselected;;3;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x3104;LIM_BelowLowLimit;;4;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x3105;LIM_AboveHighLimit;;5;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x3106;LIM_UnexpectedValue;;6;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x3107;LIM_OutOfRange;;7;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x31a0;LIM_FirstSample;;160;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x31e0;LIM_InvalidSize;;224;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x31e1;LIM_WrongType;;225;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x31e2;LIM_WrongPid;;226;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x31e3;LIM_WrongLimitId;;227;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x31ee;LIM_MonitorNotFound;;238;LIMITS_IF;fsfw/src/fsfw/monitoring/MonitoringIF.h +0x3b01;SPH_SemaphoreTimeout;;1;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x3b02;SPH_SemaphoreNotOwned;;2;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x3b03;SPH_SemaphoreInvalid;;3;SEMAPHORE_IF;fsfw/src/fsfw/tasks/SemaphoreIF.h +0x4500;HSPI_OpeningFileFailed;;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h +0x4501;HSPI_FullDuplexTransferFailed;;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h +0x4502;HSPI_HalfDuplexTransferFailed;;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h 0x4601;HURT_UartReadFailure;;1;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h 0x4602;HURT_UartReadSizeMissmatch;;2;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h 0x4603;HURT_UartRxBufferTooSmall;;3;HAL_UART;fsfw/src/fsfw_hal/linux/serial/SerialComIF.h @@ -462,8 +462,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x4403;UXOS_CommandError;Command execution failed;3;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h 0x4404;UXOS_NoCommandLoadedOrPending;;4;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h 0x4406;UXOS_PcloseCallError;;6;LINUX_OSAL;fsfw/src/fsfw_hal/linux/CommandExecutor.h -0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h -0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x6a00;SDMA_OpOngoing;;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6a01;SDMA_AlreadyOn;;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6a02;SDMA_AlreadyMounted;;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h @@ -474,4 +472,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a0d;SDMA_UnmountError;;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6a0e;SDMA_SystemCallError;;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h 0x6a0f;SDMA_PopenCallError;;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h +0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h 0x6b00;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index fdcc4488..2015316a 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 239 translations. * @details - * Generated on: 2022-11-28 18:24:37 + * Generated on: 2022-12-01 17:07:27 */ #include "translateEvents.h" diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index f6e58aab..1291aae2 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2022-11-28 18:24:37 + * Generated on: 2022-12-01 17:07:27 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index fdcc4488..2015316a 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 239 translations. * @details - * Generated on: 2022-11-28 18:24:37 + * Generated on: 2022-12-01 17:07:27 */ #include "translateEvents.h" diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index f6e58aab..1291aae2 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 148 translations. - * Generated on: 2022-11-28 18:24:37 + * Generated on: 2022-12-01 17:07:27 */ #include "translateObjects.h" diff --git a/tmtc b/tmtc index 6e152818..d9fc58ab 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6e152818707c7a12be8900bc239a0073053c0bec +Subproject commit d9fc58abed9df476a68d5dd4c798da6012fc597e From 60418f6fdc25b5e81faf1de54ad0fa2c682e86a6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 2 Dec 2022 11:38:51 +0100 Subject: [PATCH 243/244] small compile fixes --- mission/devices/devicedefinitions/GPSDefinitions.h | 1 + unittest/controller/testThermalController.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index e842f54b..7653745f 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -3,6 +3,7 @@ #include "fsfw/datapoollocal/StaticLocalDataSet.h" #include "fsfw/devicehandlers/DeviceHandlerIF.h" +#include "eive/eventSubsystemIds.h" namespace GpsHyperion { diff --git a/unittest/controller/testThermalController.cpp b/unittest/controller/testThermalController.cpp index 3fac0f4e..1c5f63fe 100644 --- a/unittest/controller/testThermalController.cpp +++ b/unittest/controller/testThermalController.cpp @@ -13,7 +13,7 @@ TEST_CASE("Thermal Controller", "[ThermalController]") { const object_id_t THERMAL_CONTROLLER_ID = 0x123; new TemperatureSensorsDummy(); - new SusDummy(); + //new SusDummy(); // testEnvironment::initialize(); From d72052fe2a64aa5efd45185c47252b4e0e33b59b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 2 Dec 2022 11:39:30 +0100 Subject: [PATCH 244/244] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 6e152818..d9fc58ab 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 6e152818707c7a12be8900bc239a0073053c0bec +Subproject commit d9fc58abed9df476a68d5dd4c798da6012fc597e