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_ +