added SensorProcessing
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
This commit is contained in:
parent
295a2e3fdc
commit
28dfcbf0ef
387
mission/controller/acs/SensorProcessing.cpp
Normal file
387
mission/controller/acs/SensorProcessing.cpp
Normal file
@ -0,0 +1,387 @@
|
|||||||
|
/*
|
||||||
|
* SensorProcessing.cpp
|
||||||
|
*
|
||||||
|
* Created on: 7 Mar 2022
|
||||||
|
* Author: Robin Marquardt
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "SensorProcessing.h"
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
|
||||||
|
#include <fsfw/src/fsfw/globalfunctions/timevalOperations.h>
|
||||||
|
#include <util/MathOperations.h>
|
||||||
|
#include <Igrf13Model.h>
|
||||||
|
#include <math.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() {
|
||||||
|
}
|
||||||
|
|
||||||
|
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<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value, mgm0ValueBody, 3, 3, 1);
|
||||||
|
validCount += 1;
|
||||||
|
validUnit[0] = true;
|
||||||
|
}
|
||||||
|
if (mgm1valid) {
|
||||||
|
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value, mgm1ValueBody, 3, 3, 1);
|
||||||
|
validCount += 1;
|
||||||
|
validUnit[1] = true;
|
||||||
|
}
|
||||||
|
if (mgm2valid) {
|
||||||
|
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value, mgm2ValueBody, 3, 3, 1);
|
||||||
|
validCount += 1;
|
||||||
|
validUnit[2] = true;
|
||||||
|
}
|
||||||
|
if (mgm3valid) {
|
||||||
|
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value, mgm3ValueBody, 3, 3, 1);
|
||||||
|
validCount += 1;
|
||||||
|
validUnit[3] = true;
|
||||||
|
}
|
||||||
|
if (mgm4valid) {
|
||||||
|
MatrixOperations<float>::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<double>::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<float>::multiply(susParameters->sus0orientationMatrix[0], sus0Value, sus0ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus1valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus1orientationMatrix[0], sus1Value, sus1ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus2valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus2orientationMatrix[0], sus2Value, sus2ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus3valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus3orientationMatrix[0], sus3Value, sus3ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus4valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus4orientationMatrix[0], sus4Value, sus4ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus5valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus5orientationMatrix[0], sus5Value, sus5ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus6valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus6orientationMatrix[0], sus6Value, sus6ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus7valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus7orientationMatrix[0], sus7Value, sus7ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus8valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus8orientationMatrix[0], sus8Value, sus8ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus9valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus9orientationMatrix[0], sus9Value, sus9ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus10valid) {
|
||||||
|
MatrixOperations<float>::multiply(susParameters->sus10orientationMatrix[0], sus10Value, sus10ValueBody, 3, 3, 1);
|
||||||
|
}
|
||||||
|
if (sus11valid) {
|
||||||
|
MatrixOperations<float>::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<double>::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<double>::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<double>::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value, rmu0ValueBody, 3, 3, 1);
|
||||||
|
validCount += 1;
|
||||||
|
validUnit[0] = true;
|
||||||
|
}
|
||||||
|
if (rmu1valid) {
|
||||||
|
MatrixOperations<double>::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value, rmu1ValueBody, 3, 3, 1);
|
||||||
|
validCount += 1;
|
||||||
|
validUnit[1] = true;
|
||||||
|
}
|
||||||
|
if (rmu2valid) {
|
||||||
|
MatrixOperations<double>::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<double>::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);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
89
mission/controller/acs/SensorProcessing.h
Normal file
89
mission/controller/acs/SensorProcessing.h
Normal file
@ -0,0 +1,89 @@
|
|||||||
|
/*******************************
|
||||||
|
* EIVE Flight Software Framework (FSFW)
|
||||||
|
* (c) 2022 IRS, Uni Stuttgart
|
||||||
|
*******************************/
|
||||||
|
#ifndef SENSORPROCESSING_H_
|
||||||
|
#define SENSORPROCESSING_H_
|
||||||
|
|
||||||
|
#include <stdint.h> //uint8_t
|
||||||
|
#include <time.h> /*purpose, timeval ?*/
|
||||||
|
#include "acs/config/classIds.h"
|
||||||
|
#include <fsfw/src/fsfw/returnvalues/HasReturnvaluesIF.h>
|
||||||
|
#include "AcsParameters.h"
|
||||||
|
#include <acs/SensorValues.h>
|
||||||
|
#include <acs/OutputValues.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 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_
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user