combined SensorProcessing and SusConverter
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details
This commit is contained in:
parent
84e960a9ef
commit
3079dabc20
|
|
@ -6,382 +6,441 @@
|
|||
*/
|
||||
|
||||
#include "SensorProcessing.h"
|
||||
#include "Igrf13Model.h"
|
||||
#include "util/MathOperations.h"
|
||||
#include <math.h>
|
||||
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/timevalOperations.h>
|
||||
#include <math.h>
|
||||
|
||||
#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<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;
|
||||
}
|
||||
|
||||
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<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;
|
||||
}
|
||||
if (susData.sus0.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus0orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus0, susParameters->sus0coeffAlpha,
|
||||
susParameters->sus0coeffBeta),
|
||||
sus0VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus1.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus1orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus1, susParameters->sus1coeffAlpha,
|
||||
susParameters->sus1coeffBeta),
|
||||
sus1VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus2.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus2orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus2, susParameters->sus2coeffAlpha,
|
||||
susParameters->sus2coeffBeta),
|
||||
sus2VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus3.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus3orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus3, susParameters->sus3coeffAlpha,
|
||||
susParameters->sus3coeffBeta),
|
||||
sus3VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus4.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus4orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus4, susParameters->sus4coeffAlpha,
|
||||
susParameters->sus4coeffBeta),
|
||||
sus4VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus5.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus5orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus5, susParameters->sus5coeffAlpha,
|
||||
susParameters->sus5coeffBeta),
|
||||
sus5VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus6.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus6orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus6, susParameters->sus6coeffAlpha,
|
||||
susParameters->sus6coeffBeta),
|
||||
sus6VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus7.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus7orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus7, susParameters->sus7coeffAlpha,
|
||||
susParameters->sus7coeffBeta),
|
||||
sus7VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus8.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus8orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus8, susParameters->sus8coeffAlpha,
|
||||
susParameters->sus8coeffBeta),
|
||||
sus8VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus9.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus9orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus9, susParameters->sus9coeffAlpha,
|
||||
susParameters->sus9coeffBeta),
|
||||
sus9VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus10.isValid()) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus10orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(susData.sus10, susParameters->sus10coeffAlpha,
|
||||
susParameters->sus10coeffBeta),
|
||||
sus10VecBody, 3, 3, 1);
|
||||
}
|
||||
if (susData.sus11.isValid()) {
|
||||
MatrixOperations<float>::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<double>::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<double>::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<double>::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<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){
|
||||
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<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;
|
||||
}
|
||||
|
||||
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;
|
||||
/* -------- 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;
|
||||
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -5,14 +5,16 @@
|
|||
#ifndef SENSORPROCESSING_H_
|
||||
#define SENSORPROCESSING_H_
|
||||
|
||||
#include "AcsParameters.h"
|
||||
#include "SensorValues.h"
|
||||
#include "OutputValues.h"
|
||||
#include "config/classIds.h"
|
||||
#include <stdint.h> //uint8_t
|
||||
#include <time.h> /*purpose, timeval ?*/
|
||||
#include <fsfw/returnvalues/returnvalue.h>
|
||||
#include <stdint.h> //uint8_t
|
||||
#include <time.h> /*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_*/
|
||||
|
||||
|
|
|
|||
|
|
@ -6,191 +6,68 @@
|
|||
*/
|
||||
|
||||
#include "SusConverter.h"
|
||||
#include <math.h> //for atan2
|
||||
#include <iostream>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
|
||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||
#include <fsfw/datapoollocal/LocalPoolVector.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <math.h> //for atan2
|
||||
|
||||
#include <iostream>
|
||||
|
||||
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<uint16_t, 6> 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<uint16_t, 6> 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<uint16_t, 6> 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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -8,69 +8,54 @@
|
|||
#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
|
||||
#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
|
||||
|
||||
#include "AcsParameters.h"
|
||||
#include <fsfw/datapoollocal/LocalPoolVector.h>
|
||||
#include <stdint.h>
|
||||
|
||||
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<uint16_t, 6> susChannel);
|
||||
|
||||
void calcAngle(lp_vec_t<uint16_t, 6> 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<uint16_t, 6> 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_ */
|
||||
|
|
|
|||
Loading…
Reference in New Issue