First Version of ACS Controller #329
@ -95,7 +95,8 @@ void AcsController::performSafe() {
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
||||
|
||||
@ -148,7 +149,8 @@ void AcsController::performDetumble() {
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
||||
|
||||
@ -182,7 +184,8 @@ void AcsController::performPointingCtrl() {
|
||||
timeval now;
|
||||
Clock::getClock_timeval(&now);
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
||||
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include "SensorProcessing.h"
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
@ -13,27 +14,35 @@
|
||||
|
||||
using namespace Math;
|
||||
|
||||
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} {
|
||||
validMagField = false;
|
||||
validGcLatitude = false;
|
||||
}
|
||||
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_)
|
||||
: savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}
|
||||
|
||||
SensorProcessing::~SensorProcessing() {}
|
||||
|
||||
bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
|
||||
void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
|
||||
bool mgm1valid, const float *mgm2Value, bool mgm2valid,
|
||||
const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
|
||||
bool mgm4valid, timeval timeOfMgmMeasurement,
|
||||
const AcsParameters::MgmHandlingParameters *mgmParameters,
|
||||
const double gpsLatitude, const double gpsLongitude,
|
||||
const double gpsAltitude, bool gpsValid, double *magFieldEst,
|
||||
bool *outputValid, double *magFieldModel,
|
||||
bool *magFieldModelValid, double *magneticFieldVectorDerivative,
|
||||
bool *magneticFieldVectorDerivativeValid) {
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
const double gpsAltitude, bool gpsValid,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed) {
|
||||
if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
|
||||
*outputValid = false;
|
||||
validMagField = false;
|
||||
return false;
|
||||
{
|
||||
PoolReadGuard pg(mgmDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double));
|
||||
mgmDataProcessed->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
|
||||
mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
|
||||
@ -104,34 +113,26 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
|
||||
}
|
||||
}
|
||||
float mgmVecTot[3] = {0.0, 0.0, 0.0};
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
magFieldEst[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
|
||||
mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
|
||||
}
|
||||
validMagField = true;
|
||||
|
||||
//-----------------------Mag Rate Computation ---------------------------------------------------
|
||||
//-----------------------Mgm Rate Computation ---------------------------------------------------
|
||||
float mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
|
||||
bool mgmVecTotDerivativeValid = false;
|
||||
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
|
||||
if (timeOfSavedMagFieldEst.tv_sec != 0) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
magneticFieldVectorDerivative[i] = (magFieldEst[i] - savedMagFieldEst[i]) / timeDiff;
|
||||
savedMagFieldEst[i] = magFieldEst[i];
|
||||
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
|
||||
savedMgmVecTot[i] = mgmVecTot[i];
|
||||
}
|
||||
|
||||
*magneticFieldVectorDerivativeValid = true;
|
||||
if (timeOfSavedMagFieldEst.tv_sec == 0) {
|
||||
magneticFieldVectorDerivative[0] = 0;
|
||||
magneticFieldVectorDerivative[1] = 0;
|
||||
magneticFieldVectorDerivative[2] = 0;
|
||||
*magneticFieldVectorDerivativeValid = false;
|
||||
}
|
||||
|
||||
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
|
||||
|
||||
*outputValid = true;
|
||||
|
||||
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
|
||||
if (!gpsValid) {
|
||||
*magFieldModelValid = false;
|
||||
} else {
|
||||
double magIgrfModel[3] = {0.0, 0.0, 0.0};
|
||||
if (gpsValid) {
|
||||
// Should be existing class object which will be called and modified here.
|
||||
Igrf13Model igrf13;
|
||||
// So the line above should not be done here. Update: Can be done here as long updated coffs
|
||||
@ -139,12 +140,32 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
igrf13.updateCoeffGH(timeOfMgmMeasurement);
|
||||
// maybe put a condition here, to only update after a full day, this
|
||||
// class function has around 700 steps to perform
|
||||
igrf13.magFieldComp(gpsLongitude, gpsLatitude, gpsAltitude, timeOfMgmMeasurement,
|
||||
magFieldModel);
|
||||
*magFieldModelValid = false;
|
||||
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
|
||||
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(mgmDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm0vec.setValid(mgm0valid);
|
||||
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm1vec.setValid(mgm1valid);
|
||||
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm2vec.setValid(mgm2valid);
|
||||
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm3vec.setValid(mgm3valid);
|
||||
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgm4vec.setValid(mgm4valid);
|
||||
std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(float));
|
||||
mgmDataProcessed->mgmVecTot.setValid(true);
|
||||
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative,
|
||||
3 * sizeof(float));
|
||||
mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid);
|
||||
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
|
||||
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
|
||||
mgmDataProcessed->setValidity(true, false);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SensorProcessing::processSus(
|
||||
@ -155,9 +176,8 @@ void SensorProcessing::processSus(
|
||||
const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
|
||||
const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
|
||||
timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
|
||||
const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
|
||||
bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
|
||||
double *sunVectorDerivative, bool *sunVectorDerivativeValid) {
|
||||
const AcsParameters::SunModelParameters *sunModelParameters,
|
||||
acsctrl::SusDataProcessed *susDataProcessed) {
|
||||
if (sus0valid) {
|
||||
sus0valid = susConverter.checkSunSensorData(sus0Value);
|
||||
}
|
||||
@ -197,9 +217,29 @@ void SensorProcessing::processSus(
|
||||
|
||||
if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
|
||||
!sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
|
||||
*sunDirEstValid = false;
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double));
|
||||
susDataProcessed->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
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},
|
||||
@ -214,6 +254,16 @@ void SensorProcessing::processSus(
|
||||
susParameters->sus0coeffBeta),
|
||||
sus0VecBody, 3, 3, 1);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus0vec.setValid(sus0valid);
|
||||
if (!sus0valid) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sus1valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus1orientationMatrix[0],
|
||||
@ -221,6 +271,16 @@ void SensorProcessing::processSus(
|
||||
susParameters->sus1coeffBeta),
|
||||
sus1VecBody, 3, 3, 1);
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus1vec.setValid(sus1valid);
|
||||
if (!sus1valid) {
|
||||
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sus2valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus2orientationMatrix[0],
|
||||
@ -305,7 +365,7 @@ void SensorProcessing::processSus(
|
||||
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
|
||||
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
|
||||
|
||||
double susMeanValue[3] = {0, 0, 0};
|
||||
float susMeanValue[3] = {0, 0, 0};
|
||||
for (uint8_t i = 0; i < 12; i++) {
|
||||
if (validIds[i]) {
|
||||
susMeanValue[0] += susVecBody[0][i];
|
||||
@ -313,26 +373,20 @@ void SensorProcessing::processSus(
|
||||
susMeanValue[2] += susVecBody[2][i];
|
||||
}
|
||||
}
|
||||
VectorOperations<double>::normalize(susMeanValue, sunDirEst, 3);
|
||||
*sunDirEstValid = true;
|
||||
}
|
||||
float susVecTot[3] = {0.0, 0.0, 0.0};
|
||||
VectorOperations<float>::normalize(susMeanValue, susVecTot, 3);
|
||||
|
||||
/* -------- Sun Derivatiative --------------------- */
|
||||
|
||||
float susVecTotDerivative[3] = {0.0, 0.0, 0.0};
|
||||
bool susVecTotDerivativeValid = false;
|
||||
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
|
||||
if (timeOfSavedSusDirEst.tv_sec != 0) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff;
|
||||
savedSunVector[i] = sunDirEst[i];
|
||||
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
|
||||
savedSusVecTot[i] = susVecTot[i];
|
||||
}
|
||||
|
||||
*sunVectorDerivativeValid = true;
|
||||
if (timeOfSavedSusDirEst.tv_sec == 0) {
|
||||
sunVectorDerivative[0] = 0;
|
||||
sunVectorDerivative[1] = 0;
|
||||
sunVectorDerivative[2] = 0;
|
||||
*sunVectorDerivativeValid = false;
|
||||
}
|
||||
|
||||
timeOfSavedSusDirEst = timeOfSusMeasurement;
|
||||
|
||||
/* -------- Sun Model Direction (IJK frame) ------- */
|
||||
@ -340,6 +394,7 @@ void SensorProcessing::processSus(
|
||||
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
|
||||
|
||||
// Julean Centuries
|
||||
double sunIjkModel[3] = {0.0, 0.0, 0.0};
|
||||
double JC2000 = JD2000 / 36525;
|
||||
|
||||
double meanLongitude =
|
||||
@ -351,11 +406,46 @@ void SensorProcessing::processSus(
|
||||
|
||||
double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
|
||||
|
||||
sunVectorInertial[0] = cos(eclipticLongitude);
|
||||
sunVectorInertial[1] = sin(eclipticLongitude) * cos(epsilon);
|
||||
sunVectorInertial[2] = sin(eclipticLongitude) * sin(epsilon);
|
||||
|
||||
*sunVectorInertialValid = true;
|
||||
sunIjkModel[0] = cos(eclipticLongitude);
|
||||
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
|
||||
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus0vec.setValid(sus0valid);
|
||||
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus1vec.setValid(sus1valid);
|
||||
std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus2vec.setValid(sus2valid);
|
||||
std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus3vec.setValid(sus3valid);
|
||||
std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus4vec.setValid(sus4valid);
|
||||
std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus5vec.setValid(sus5valid);
|
||||
std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus6vec.setValid(sus6valid);
|
||||
std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus7vec.setValid(sus7valid);
|
||||
std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus8vec.setValid(sus8valid);
|
||||
std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus9vec.setValid(sus9valid);
|
||||
std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus10vec.setValid(sus10valid);
|
||||
std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
|
||||
susDataProcessed->sus11vec.setValid(sus11valid);
|
||||
std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(float));
|
||||
susDataProcessed->susVecTot.setValid(true);
|
||||
std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative,
|
||||
3 * sizeof(float));
|
||||
susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid);
|
||||
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
|
||||
susDataProcessed->sunIjkModel.setValid(true);
|
||||
susDataProcessed->setValidity(true, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SensorProcessing::processGyr(
|
||||
@ -366,133 +456,166 @@ void SensorProcessing::processGyr(
|
||||
const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
|
||||
const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
|
||||
timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
|
||||
double *satRatEst, bool *satRateEstValid) {
|
||||
if (!gyr0axXvalid && !gyr0axYvalid && !gyr0axZvalid && !gyr1axXvalid && !gyr1axYvalid &&
|
||||
!gyr1axZvalid && !gyr2axXvalid && !gyr2axYvalid && !gyr2axZvalid && !gyr3axXvalid &&
|
||||
!gyr3axYvalid && !gyr3axZvalid) {
|
||||
*satRateEstValid = false;
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed) {
|
||||
bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
|
||||
bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
|
||||
bool gyr2valid = (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid);
|
||||
bool gyr3valid = (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid);
|
||||
if (!gyr0valid && !gyr1valid && !gyr2valid && !gyr3valid) {
|
||||
{
|
||||
PoolReadGuard pg(gyrDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double));
|
||||
gyrDataProcessed->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
// Transforming Values to the Body Frame (actually it is the geometry frame atm)
|
||||
double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0},
|
||||
gyr3ValueBody[3] = {0, 0, 0};
|
||||
|
||||
bool validUnit[4] = {false, false, false, false};
|
||||
if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) {
|
||||
if (gyr0valid) {
|
||||
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
|
||||
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
|
||||
gyr0ValueBody, 3, 3, 1);
|
||||
validUnit[0] = true;
|
||||
}
|
||||
if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) {
|
||||
if (gyr1valid) {
|
||||
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
|
||||
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
|
||||
gyr1ValueBody, 3, 3, 1);
|
||||
validUnit[1] = true;
|
||||
}
|
||||
if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) {
|
||||
if (gyr2valid) {
|
||||
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
|
||||
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
|
||||
gyr2ValueBody, 3, 3, 1);
|
||||
validUnit[2] = true;
|
||||
}
|
||||
if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) {
|
||||
if (gyr3valid) {
|
||||
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
|
||||
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
|
||||
gyr3ValueBody, 3, 3, 1);
|
||||
validUnit[3] = true;
|
||||
}
|
||||
|
||||
/* -------- SatRateEst: Middle Value ------- */
|
||||
// take ADIS measurements, if both avail
|
||||
// if just one ADIS measurement avail, perform sensor fusion
|
||||
if (validUnit[0] && validUnit[2]) {
|
||||
double gyrVecTot[3] = {0.0, 0.0, 0.0};
|
||||
if (gyr0valid && gyr2valid) {
|
||||
double gyr02ValuesSum[3];
|
||||
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
|
||||
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, satRatEst, 3);
|
||||
} else if ((validUnit[0] || validUnit[2]) && !(validUnit[1] || validUnit[3])) {
|
||||
if (validUnit[0]) {
|
||||
satRatEst = gyr0ValueBody;
|
||||
} else if (validUnit[2]) {
|
||||
satRatEst = gyr2ValueBody;
|
||||
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
|
||||
} else if ((gyr0valid || gyr2valid) && !(gyr1valid || gyr3valid)) {
|
||||
if (gyr0valid) {
|
||||
std::memcpy(gyrVecTot, gyr0ValueBody, 3 * sizeof(double));
|
||||
} else if (gyr2valid) {
|
||||
std::memcpy(gyrVecTot, gyr2ValueBody, 3 * sizeof(double));
|
||||
}
|
||||
} else if ((validUnit[1]) && (validUnit[3])) {
|
||||
} else if (gyr1valid && gyr3valid) {
|
||||
double gyr13ValuesSum[3];
|
||||
double gyr13ValuesMean[3];
|
||||
VectorOperations<double>::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3);
|
||||
VectorOperations<double>::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3);
|
||||
if (validUnit[0]) {
|
||||
satRatEst[0] =
|
||||
if (gyr0valid) {
|
||||
gyrVecTot[0] =
|
||||
((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) +
|
||||
(gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
|
||||
((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
|
||||
satRatEst[1] =
|
||||
gyrVecTot[1] =
|
||||
((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) +
|
||||
(gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
|
||||
((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
|
||||
satRatEst[2] =
|
||||
gyrVecTot[2] =
|
||||
((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) +
|
||||
(gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
|
||||
((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
|
||||
} else if (validUnit[2]) {
|
||||
satRatEst[0] =
|
||||
} else if (gyr2valid) {
|
||||
gyrVecTot[0] =
|
||||
((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) +
|
||||
(gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) /
|
||||
((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0]));
|
||||
satRatEst[1] =
|
||||
gyrVecTot[1] =
|
||||
((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) +
|
||||
(gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) /
|
||||
((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1]));
|
||||
satRatEst[2] =
|
||||
gyrVecTot[2] =
|
||||
((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) +
|
||||
(gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) /
|
||||
((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2]));
|
||||
} else
|
||||
satRatEst = gyr13ValuesMean;
|
||||
} else if (validUnit[1]) {
|
||||
satRatEst = gyr1ValueBody;
|
||||
} else if (validUnit[3]) {
|
||||
satRatEst = gyr3ValueBody;
|
||||
} else {
|
||||
std::memcpy(gyrVecTot, gyr13ValuesMean, 3 * sizeof(double));
|
||||
}
|
||||
} else if (gyr1valid) {
|
||||
std::memcpy(gyrVecTot, gyr1ValueBody, 3 * sizeof(double));
|
||||
} else if (gyr3valid) {
|
||||
std::memcpy(gyrVecTot, gyr3ValueBody, 3 * sizeof(double));
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(gyrDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double));
|
||||
gyrDataProcessed->gyr0vec.setValid(gyr0valid);
|
||||
std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double));
|
||||
gyrDataProcessed->setValidity(true, false);
|
||||
}
|
||||
}
|
||||
*satRateEstValid = true;
|
||||
}
|
||||
|
||||
void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
|
||||
const bool validGps, double *gcLatitude, double *gdLongitude) {
|
||||
const bool validGps,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed) {
|
||||
// name to convert not process
|
||||
double gdLongitude, gcLatitude;
|
||||
if (validGps) {
|
||||
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
|
||||
*gdLongitude = gps0longitude * PI / 180;
|
||||
gdLongitude = gps0longitude * PI / 180;
|
||||
double latitudeRad = gps0latitude * PI / 180;
|
||||
double eccentricityWgs84 = 0.0818195;
|
||||
double factor = 1 - pow(eccentricityWgs84, 2);
|
||||
*gcLatitude = atan(factor * tan(latitudeRad));
|
||||
validGcLatitude = true;
|
||||
gcLatitude = atan(factor * tan(latitudeRad));
|
||||
// validGcLatitude = true;
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(gpsDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
gpsDataProcessed->gdLongitude.value = gdLongitude;
|
||||
gpsDataProcessed->gcLatitude.value = gcLatitude;
|
||||
gpsDataProcessed->setValidity(validGps, validGps);
|
||||
if (!validGps) {
|
||||
gpsDataProcessed->gdLongitude.value = 0.0;
|
||||
gpsDataProcessed->gcLatitude.value = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
|
||||
ACS::OutputValues *outputValues,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
const AcsParameters *acsParameters) {
|
||||
sensorValues->update();
|
||||
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
|
||||
sensorValues->gpsSet.isValid(), &outputValues->gcLatitude, &outputValues->gdLongitude);
|
||||
sensorValues->gpsSet.isValid(), gpsDataProcessed);
|
||||
|
||||
outputValues->mgmUpdated = processMgm(
|
||||
sensorValues->mgm0Lis3Set.fieldStrengths.value,
|
||||
processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
|
||||
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm1Rm3100Set.fieldStrengths.value,
|
||||
sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm2Lis3Set.fieldStrengths.value,
|
||||
sensorValues->mgm2Lis3Set.fieldStrengths.isValid(),
|
||||
sensorValues->mgm3Rm3100Set.fieldStrengths.value,
|
||||
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value,
|
||||
sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters,
|
||||
outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value,
|
||||
sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid,
|
||||
outputValues->magFieldModel, &outputValues->magFieldModelValid,
|
||||
outputValues->magneticFieldVectorDerivative,
|
||||
&outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ?
|
||||
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
|
||||
sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
|
||||
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
|
||||
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.isValid(), mgmDataProcessed);
|
||||
|
||||
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
|
||||
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
|
||||
@ -507,10 +630,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
|
||||
sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
|
||||
sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
|
||||
now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
|
||||
outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel,
|
||||
&outputValues->sunDirModelValid, outputValues->sunVectorDerivative,
|
||||
&outputValues->sunVectorDerivativeValid);
|
||||
// VALID outputs ?
|
||||
susDataProcessed);
|
||||
|
||||
processGyr(
|
||||
sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
|
||||
@ -525,6 +645,5 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
|
||||
sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
|
||||
sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
|
||||
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
|
||||
&acsParameters->gyrHandlingParameters, outputValues->satRateEst,
|
||||
&outputValues->satRateEstValid);
|
||||
&acsParameters->gyrHandlingParameters, gyrDataProcessed);
|
||||
}
|
||||
|
||||
@ -23,19 +23,21 @@ class SensorProcessing {
|
||||
SensorProcessing(AcsParameters *acsParameters_);
|
||||
virtual ~SensorProcessing();
|
||||
|
||||
void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
|
||||
void process(timeval now, ACS::SensorValues *sensorValues,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
const AcsParameters *acsParameters); // Will call protected functions
|
||||
private:
|
||||
protected:
|
||||
// short description needed for every function
|
||||
bool processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
|
||||
void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
|
||||
const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
|
||||
const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
|
||||
const AcsParameters::MgmHandlingParameters *mgmParameters,
|
||||
const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
|
||||
bool gpsValid, double *magFieldEst, bool *outputValid, double *magFieldModel,
|
||||
bool *magFieldModelValid, double *magneticFieldVectorDerivative,
|
||||
bool *magneticFieldVectorDerivativeValid); // Output
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude,
|
||||
bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed);
|
||||
|
||||
void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value,
|
||||
bool sus1valid, const uint16_t *sus2Value, bool sus2valid,
|
||||
@ -47,9 +49,8 @@ class SensorProcessing {
|
||||
bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
|
||||
timeval timeOfSusMeasurement,
|
||||
const AcsParameters::SusHandlingParameters *susParameters,
|
||||
const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
|
||||
bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
|
||||
double *sunVectorDerivative, bool *sunVectorDerivativeValid);
|
||||
const AcsParameters::SunModelParameters *sunModelParameters,
|
||||
acsctrl::SusDataProcessed *susDataProcessed);
|
||||
|
||||
void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue,
|
||||
bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid,
|
||||
@ -60,21 +61,23 @@ class SensorProcessing {
|
||||
const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
|
||||
bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
|
||||
timeval timeOfGyrMeasurement,
|
||||
const AcsParameters::GyrHandlingParameters *gyrParameters, double *satRatEst,
|
||||
bool *satRateEstValid);
|
||||
const AcsParameters::GyrHandlingParameters *gyrParameters,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed);
|
||||
|
||||
void processStr();
|
||||
|
||||
void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
|
||||
double *gcLatitude, double *gdLongitude);
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed);
|
||||
|
||||
double savedMagFieldEst[3];
|
||||
double savedMgmVecTot[3];
|
||||
timeval timeOfSavedMagFieldEst;
|
||||
double savedSunVector[3];
|
||||
double savedSusVecTot[3];
|
||||
timeval timeOfSavedSusDirEst;
|
||||
bool validMagField;
|
||||
bool validGcLatitude;
|
||||
|
||||
const float zeroVector[3] = {0.0, 0.0, 0.0};
|
||||
|
||||
SusConverter susConverter;
|
||||
AcsParameters acsParameters;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user