meggert
9f5a198c5d
All checks were successful
EIVE/eive-obsw/pipeline/pr-dev-7.5.0 This commit looks good
655 lines
34 KiB
C++
655 lines
34 KiB
C++
#include "SensorProcessing.h"
|
|
|
|
SensorProcessing::SensorProcessing() {}
|
|
|
|
SensorProcessing::~SensorProcessing() {}
|
|
|
|
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 timeAbsolute, double timeDelta,
|
|
const AcsParameters::MgmHandlingParameters *mgmParameters,
|
|
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
|
acsctrl::MgmDataProcessed *mgmDataProcessed) {
|
|
// ---------------- IGRF- 13 Implementation here
|
|
// ------------------------------------------------
|
|
double magIgrfModel[3] = {0.0, 0.0, 0.0};
|
|
bool gpsValid = false;
|
|
if (gpsDataProcessed->source.value != acs::gps::Source::NONE) {
|
|
// There seems to be a bug here, which causes the model vector to drift until infinity, if the
|
|
// model class is not initialized new every time. Works for now, but should be investigated.
|
|
Igrf13Model igrf13;
|
|
igrf13.schmidtNormalization();
|
|
igrf13.updateCoeffGH(timeAbsolute);
|
|
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
|
|
gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel);
|
|
gpsValid = true;
|
|
}
|
|
if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and
|
|
(not mgm4valid or not mgmParameters->useMgm4)) {
|
|
{
|
|
PoolReadGuard pg(mgmDataProcessed);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmDataProcessed->mgm0vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(mgmDataProcessed->mgm1vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(mgmDataProcessed->mgm2vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(mgmDataProcessed->mgm3vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(mgmDataProcessed->mgm4vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(double));
|
|
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double));
|
|
mgmDataProcessed->setValidity(false, true);
|
|
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
|
|
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
|
|
}
|
|
}
|
|
std::memcpy(savedMgmVecTot, ZERO_VEC_D, sizeof(savedMgmVecTot));
|
|
return;
|
|
}
|
|
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
|
|
mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
|
|
mgm4ValueNoBias[3] = {0, 0, 0};
|
|
float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
|
|
mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
|
|
float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
|
|
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
|
|
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
|
|
|
|
if (mgm0valid) {
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
|
|
mgm0ValueBody, 3, 3, 1);
|
|
VectorOperations<float>::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset,
|
|
mgm0ValueNoBias, 3);
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
|
|
mgm0ValueCalib, 3, 3, 1);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i];
|
|
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
|
|
}
|
|
}
|
|
if (mgm1valid) {
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
|
|
mgm1ValueBody, 3, 3, 1);
|
|
VectorOperations<float>::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset,
|
|
mgm1ValueNoBias, 3);
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
|
|
mgm1ValueCalib, 3, 3, 1);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i];
|
|
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
|
|
}
|
|
}
|
|
if (mgm2valid) {
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
|
|
mgm2ValueBody, 3, 3, 1);
|
|
VectorOperations<float>::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset,
|
|
mgm2ValueNoBias, 3);
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
|
|
mgm2ValueCalib, 3, 3, 1);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i];
|
|
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
|
|
}
|
|
}
|
|
if (mgm3valid) {
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
|
|
mgm3ValueBody, 3, 3, 1);
|
|
VectorOperations<float>::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset,
|
|
mgm3ValueNoBias, 3);
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
|
|
mgm3ValueCalib, 3, 3, 1);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i];
|
|
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
|
|
}
|
|
}
|
|
if (mgm4valid and mgmParameters->useMgm4) {
|
|
float mgm4ValueUT[3];
|
|
VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3); // nT to uT
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
|
|
mgm4ValueBody, 3, 3, 1);
|
|
VectorOperations<float>::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset,
|
|
mgm4ValueNoBias, 3);
|
|
MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
|
|
mgm4ValueCalib, 3, 3, 1);
|
|
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i];
|
|
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
|
|
}
|
|
}
|
|
double mgmVecTot[3] = {0.0, 0.0, 0.0};
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
|
|
}
|
|
if (VectorOperations<double>::norm(mgmVecTot, 3) != 0 and mgmDataProcessed->mgmVecTot.isValid()) {
|
|
lowPassFilter(mgmVecTot, mgmDataProcessed->mgmVecTot.value,
|
|
mgmParameters->mgmVectorFilterWeight);
|
|
}
|
|
|
|
//-----------------------Mgm Rate Computation ---------------------------------------------------
|
|
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
|
|
bool mgmVecTotDerivativeValid = false;
|
|
if (timeDelta > 0 and VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
|
|
VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
|
|
VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDelta, mgmVecTotDerivative,
|
|
3);
|
|
mgmVecTotDerivativeValid = true;
|
|
}
|
|
std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
|
|
|
|
if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
|
|
mgmDataProcessed->mgmVecTotDerivative.isValid()) {
|
|
lowPassFilter(mgmVecTotDerivative, mgmDataProcessed->mgmVecTotDerivative.value,
|
|
mgmParameters->mgmDerivativeFilterWeight);
|
|
}
|
|
|
|
{
|
|
PoolReadGuard pg(mgmDataProcessed);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float));
|
|
mgmDataProcessed->mgm0vec.setValid(mgm0valid);
|
|
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float));
|
|
mgmDataProcessed->mgm1vec.setValid(mgm1valid);
|
|
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float));
|
|
mgmDataProcessed->mgm2vec.setValid(mgm2valid);
|
|
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float));
|
|
mgmDataProcessed->mgm3vec.setValid(mgm3valid);
|
|
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float));
|
|
mgmDataProcessed->mgm4vec.setValid(mgm4valid);
|
|
std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
|
|
mgmDataProcessed->mgmVecTot.setValid(true);
|
|
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative,
|
|
3 * sizeof(double));
|
|
mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid);
|
|
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
|
|
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
|
|
mgmDataProcessed->setValidity(true, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SensorProcessing::processSus(
|
|
const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid,
|
|
const uint16_t *sus2Value, bool sus2valid, const uint16_t *sus3Value, bool sus3valid,
|
|
const uint16_t *sus4Value, bool sus4valid, const uint16_t *sus5Value, bool sus5valid,
|
|
const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid,
|
|
const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
|
|
const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
|
|
timeval timeAbsolute, double timeDelta,
|
|
const AcsParameters::SusHandlingParameters *susParameters,
|
|
const AcsParameters::SunModelParameters *sunModelParameters,
|
|
acsctrl::SusDataProcessed *susDataProcessed) {
|
|
/* -------- Sun Model Direction (IJK frame) ------- */
|
|
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
|
|
|
|
// Julean Centuries
|
|
double sunIjkModel[3] = {0.0, 0.0, 0.0};
|
|
double JC2000 = JD2000 / 36525.;
|
|
|
|
double meanLongitude =
|
|
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * M_PI / 180.;
|
|
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * M_PI / 180.;
|
|
|
|
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
|
|
sunModelParameters->p2 * sin(2 * meanAnomaly);
|
|
|
|
double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
|
|
|
|
sunIjkModel[0] = cos(eclipticLongitude);
|
|
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
|
|
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
|
|
|
|
uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
|
if (sus0valid) {
|
|
susBrightness[0] = susConverter.checkSunSensorData(sus0Value);
|
|
}
|
|
if (sus1valid) {
|
|
susBrightness[1] = susConverter.checkSunSensorData(sus1Value);
|
|
}
|
|
if (sus2valid) {
|
|
susBrightness[2] = susConverter.checkSunSensorData(sus2Value);
|
|
}
|
|
if (sus3valid) {
|
|
susBrightness[3] = susConverter.checkSunSensorData(sus3Value);
|
|
}
|
|
if (sus4valid) {
|
|
susBrightness[4] = susConverter.checkSunSensorData(sus4Value);
|
|
}
|
|
if (sus5valid) {
|
|
susBrightness[5] = susConverter.checkSunSensorData(sus5Value);
|
|
}
|
|
if (sus6valid) {
|
|
susBrightness[6] = susConverter.checkSunSensorData(sus6Value);
|
|
}
|
|
if (sus7valid) {
|
|
susBrightness[7] = susConverter.checkSunSensorData(sus7Value);
|
|
}
|
|
if (sus8valid) {
|
|
susBrightness[8] = susConverter.checkSunSensorData(sus8Value);
|
|
}
|
|
if (sus9valid) {
|
|
susBrightness[9] = susConverter.checkSunSensorData(sus9Value);
|
|
}
|
|
if (sus10valid) {
|
|
susBrightness[10] = susConverter.checkSunSensorData(sus10Value);
|
|
}
|
|
if (sus11valid) {
|
|
susBrightness[11] = susConverter.checkSunSensorData(sus11Value);
|
|
}
|
|
|
|
bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
|
|
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
|
|
bool allInvalid =
|
|
susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold);
|
|
|
|
if (allInvalid) {
|
|
{
|
|
PoolReadGuard pg(susDataProcessed);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataProcessed->sus0vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus1vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus2vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus3vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus4vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus5vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus6vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus7vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus8vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus9vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus10vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->sus11vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
|
std::memcpy(susDataProcessed->susVecTot.value, ZERO_VEC_D, 3 * sizeof(double));
|
|
std::memcpy(susDataProcessed->susVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double));
|
|
susDataProcessed->setValidity(false, true);
|
|
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
|
|
susDataProcessed->sunIjkModel.setValid(true);
|
|
}
|
|
}
|
|
std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot));
|
|
return;
|
|
}
|
|
|
|
float susVecSensor[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
|
|
{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
float susVecBody[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
|
|
{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
|
|
if (susValid[0]) {
|
|
susConverter.calculateSunVector(susVecSensor[0], sus0Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0],
|
|
susVecBody[0], 3, 3, 1);
|
|
}
|
|
if (susValid[1]) {
|
|
susConverter.calculateSunVector(susVecSensor[1], sus1Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1],
|
|
susVecBody[1], 3, 3, 1);
|
|
}
|
|
if (susValid[2]) {
|
|
susConverter.calculateSunVector(susVecSensor[2], sus2Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2],
|
|
susVecBody[2], 3, 3, 1);
|
|
}
|
|
if (susValid[3]) {
|
|
susConverter.calculateSunVector(susVecSensor[3], sus3Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3],
|
|
susVecBody[3], 3, 3, 1);
|
|
}
|
|
if (susValid[4]) {
|
|
susConverter.calculateSunVector(susVecSensor[4], sus4Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4],
|
|
susVecBody[4], 3, 3, 1);
|
|
}
|
|
if (susValid[5]) {
|
|
susConverter.calculateSunVector(susVecSensor[5], sus5Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5],
|
|
susVecBody[5], 3, 3, 1);
|
|
}
|
|
if (susValid[6]) {
|
|
susConverter.calculateSunVector(susVecSensor[6], sus6Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6],
|
|
susVecBody[6], 3, 3, 1);
|
|
}
|
|
if (susValid[7]) {
|
|
susConverter.calculateSunVector(susVecSensor[7], sus7Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7],
|
|
susVecBody[7], 3, 3, 1);
|
|
}
|
|
if (susValid[8]) {
|
|
susConverter.calculateSunVector(susVecSensor[8], sus8Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8],
|
|
susVecBody[8], 3, 3, 1);
|
|
}
|
|
if (susValid[9]) {
|
|
susConverter.calculateSunVector(susVecSensor[9], sus9Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9],
|
|
susVecBody[9], 3, 3, 1);
|
|
}
|
|
if (susValid[10]) {
|
|
susConverter.calculateSunVector(susVecSensor[10], sus10Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10],
|
|
susVecBody[10], 3, 3, 1);
|
|
}
|
|
if (susValid[11]) {
|
|
susConverter.calculateSunVector(susVecSensor[11], sus11Value);
|
|
MatrixOperations<float>::multiply(susParameters->sus11orientationMatrix[0], susVecSensor[11],
|
|
susVecBody[11], 3, 3, 1);
|
|
}
|
|
|
|
double susMeanValue[3] = {0, 0, 0};
|
|
for (uint8_t i = 0; i < 12; i++) {
|
|
susMeanValue[0] += susVecBody[i][0];
|
|
susMeanValue[1] += susVecBody[i][1];
|
|
susMeanValue[2] += susVecBody[i][2];
|
|
}
|
|
double susVecTot[3] = {0.0, 0.0, 0.0};
|
|
VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
|
|
|
|
if (VectorOperations<double>::norm(susVecTot, 3) != 0 and susDataProcessed->susVecTot.isValid()) {
|
|
lowPassFilter(susVecTot, susDataProcessed->susVecTot.value,
|
|
susParameters->susVectorFilterWeight);
|
|
}
|
|
|
|
/* -------- Sun Derivatiative --------------------- */
|
|
|
|
double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
|
|
bool susVecTotDerivativeValid = false;
|
|
if (timeDelta > 0 and VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
|
|
VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
|
|
VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDelta, susVecTotDerivative,
|
|
3);
|
|
susVecTotDerivativeValid = true;
|
|
}
|
|
std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
|
|
if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
|
|
susDataProcessed->susVecTotDerivative.isValid()) {
|
|
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
|
|
susParameters->susRateFilterWeight);
|
|
}
|
|
{
|
|
PoolReadGuard pg(susDataProcessed);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataProcessed->sus0vec.value, susVecBody[0], 3 * sizeof(float));
|
|
susDataProcessed->sus0vec.setValid(sus0valid);
|
|
std::memcpy(susDataProcessed->sus1vec.value, susVecBody[1], 3 * sizeof(float));
|
|
susDataProcessed->sus1vec.setValid(sus1valid);
|
|
std::memcpy(susDataProcessed->sus2vec.value, susVecBody[2], 3 * sizeof(float));
|
|
susDataProcessed->sus2vec.setValid(sus2valid);
|
|
std::memcpy(susDataProcessed->sus3vec.value, susVecBody[3], 3 * sizeof(float));
|
|
susDataProcessed->sus3vec.setValid(sus3valid);
|
|
std::memcpy(susDataProcessed->sus4vec.value, susVecBody[4], 3 * sizeof(float));
|
|
susDataProcessed->sus4vec.setValid(sus4valid);
|
|
std::memcpy(susDataProcessed->sus5vec.value, susVecBody[5], 3 * sizeof(float));
|
|
susDataProcessed->sus5vec.setValid(sus5valid);
|
|
std::memcpy(susDataProcessed->sus6vec.value, susVecBody[6], 3 * sizeof(float));
|
|
susDataProcessed->sus6vec.setValid(sus6valid);
|
|
std::memcpy(susDataProcessed->sus7vec.value, susVecBody[7], 3 * sizeof(float));
|
|
susDataProcessed->sus7vec.setValid(sus7valid);
|
|
std::memcpy(susDataProcessed->sus8vec.value, susVecBody[8], 3 * sizeof(float));
|
|
susDataProcessed->sus8vec.setValid(sus8valid);
|
|
std::memcpy(susDataProcessed->sus9vec.value, susVecBody[9], 3 * sizeof(float));
|
|
susDataProcessed->sus9vec.setValid(sus9valid);
|
|
std::memcpy(susDataProcessed->sus10vec.value, susVecBody[10], 3 * sizeof(float));
|
|
susDataProcessed->sus10vec.setValid(sus10valid);
|
|
std::memcpy(susDataProcessed->sus11vec.value, susVecBody[11], 3 * sizeof(float));
|
|
susDataProcessed->sus11vec.setValid(sus11valid);
|
|
std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
|
|
susDataProcessed->susVecTot.setValid(true);
|
|
std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative,
|
|
3 * sizeof(double));
|
|
susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid);
|
|
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
|
|
susDataProcessed->sunIjkModel.setValid(true);
|
|
susDataProcessed->setValidity(true, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SensorProcessing::processGyr(
|
|
const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid,
|
|
const double gyr0axZvalue, bool gyr0axZvalid, const double gyr1axXvalue, bool gyr1axXvalid,
|
|
const double gyr1axYvalue, bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid,
|
|
const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid,
|
|
const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
|
|
const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
|
|
const AcsParameters::GyrHandlingParameters *gyrParameters,
|
|
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, ZERO_VEC_D, 3 * sizeof(double));
|
|
std::memcpy(gyrDataProcessed->gyr1vec.value, ZERO_VEC_D, 3 * sizeof(double));
|
|
std::memcpy(gyrDataProcessed->gyr2vec.value, ZERO_VEC_D, 3 * sizeof(double));
|
|
std::memcpy(gyrDataProcessed->gyr3vec.value, ZERO_VEC_D, 3 * sizeof(double));
|
|
std::memcpy(gyrDataProcessed->gyrVecTot.value, ZERO_VEC_D, 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};
|
|
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
|
|
|
|
if (gyr0valid) {
|
|
double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
|
|
VectorOperations<double>::subtract(gyr0Value, gyrParameters->gyr0bias, gyr0Value, 3);
|
|
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
|
|
gyr0ValueBody, 3, 3, 1);
|
|
VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
|
|
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
|
|
}
|
|
}
|
|
if (gyr1valid) {
|
|
double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
|
|
VectorOperations<double>::subtract(gyr1Value, gyrParameters->gyr1bias, gyr1Value, 3);
|
|
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
|
|
gyr1ValueBody, 3, 3, 1);
|
|
VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
|
|
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
|
|
}
|
|
}
|
|
if (gyr2valid) {
|
|
double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
|
|
VectorOperations<double>::subtract(gyr2Value, gyrParameters->gyr2bias, gyr2Value, 3);
|
|
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
|
|
gyr2ValueBody, 3, 3, 1);
|
|
VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
|
|
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
|
|
}
|
|
}
|
|
if (gyr3valid) {
|
|
double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
|
|
VectorOperations<double>::subtract(gyr3Value, gyrParameters->gyr3bias, gyr3Value, 3);
|
|
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
|
|
gyr3ValueBody, 3, 3, 1);
|
|
VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
|
|
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
|
|
}
|
|
}
|
|
|
|
/* -------- SatRateEst: Middle Value ------- */
|
|
// take ADIS measurements, if both avail
|
|
// if just one ADIS measurement avail, perform sensor fusion
|
|
double gyrVecTot[3] = {0.0, 0.0, 0.0};
|
|
if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == true) {
|
|
double gyr02ValuesSum[3];
|
|
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
|
|
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
|
|
} else {
|
|
for (uint8_t i = 0; i < 3; i++) {
|
|
gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
|
|
}
|
|
}
|
|
|
|
if (VectorOperations<double>::norm(gyrVecTot, 3) != 0 and gyrDataProcessed->gyrVecTot.isValid()) {
|
|
lowPassFilter(gyrVecTot, gyrDataProcessed->gyrVecTot.value, gyrParameters->gyrFilterWeight);
|
|
}
|
|
|
|
{
|
|
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));
|
|
gyrDataProcessed->gyr1vec.setValid(gyr1valid);
|
|
std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double));
|
|
gyrDataProcessed->gyr2vec.setValid(gyr2valid);
|
|
std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double));
|
|
gyrDataProcessed->gyr3vec.setValid(gyr3valid);
|
|
std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double));
|
|
gyrDataProcessed->gyrVecTot.setValid(true);
|
|
gyrDataProcessed->setValidity(true, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
|
|
const double gpsAltitude, const double timeDelta,
|
|
const bool validGps,
|
|
const AcsParameters::GpsParameters *gpsParameters,
|
|
acsctrl::GpsDataProcessed *gpsDataProcessed) {
|
|
// init variables
|
|
double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
|
|
gpsVelocityE[3] = {0, 0, 0};
|
|
uint8_t gpsSource = acs::gps::Source::NONE;
|
|
// We do not trust the GPS and therefore it shall die here if SPG4 is running
|
|
if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
|
|
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
|
|
gdLongitude, altitude);
|
|
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
|
|
gcLatitude = atan(factor * tan(gdLatitude));
|
|
{
|
|
PoolReadGuard pg(gpsDataProcessed);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
gpsDataProcessed->gdLongitude.value = gdLongitude;
|
|
gpsDataProcessed->gcLatitude.value = gcLatitude;
|
|
gpsDataProcessed->altitude.value = altitude;
|
|
gpsDataProcessed->setValidity(true, true);
|
|
}
|
|
}
|
|
return;
|
|
} else if (validGps) {
|
|
// Transforming from Degree to Radians and calculation geocentric latitude from geodetic
|
|
gdLongitude = gpsLongitude * M_PI / 180.;
|
|
double latitudeRad = gpsLatitude * M_PI / 180.;
|
|
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
|
|
gcLatitude = atan(factor * tan(latitudeRad));
|
|
|
|
// Altitude FDIR
|
|
if (gpsAltitude > gpsParameters->maximumFdirAltitude ||
|
|
gpsAltitude < gpsParameters->minimumFdirAltitude) {
|
|
altitude = gpsParameters->fdirAltitude;
|
|
} else {
|
|
altitude = gpsAltitude;
|
|
}
|
|
|
|
// Calculation of the satellite velocity in earth fixed frame
|
|
double deltaDistance[3] = {0, 0, 0};
|
|
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
|
|
if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
|
|
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
|
|
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);
|
|
}
|
|
savedPosSatE[0] = posSatE[0];
|
|
savedPosSatE[1] = posSatE[1];
|
|
savedPosSatE[2] = posSatE[2];
|
|
|
|
validSavedPosSatE = true;
|
|
|
|
gpsSource = acs::gps::Source::GPS;
|
|
}
|
|
{
|
|
PoolReadGuard pg(gpsDataProcessed);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
gpsDataProcessed->gdLongitude.value = gdLongitude;
|
|
gpsDataProcessed->gcLatitude.value = gcLatitude;
|
|
gpsDataProcessed->altitude.value = altitude;
|
|
std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
|
|
std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
|
|
gpsDataProcessed->setValidity(validGps, true);
|
|
gpsDataProcessed->source.value = gpsSource;
|
|
gpsDataProcessed->source.setValid(true);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
|
|
ACS::SensorValues *sensorValues,
|
|
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.altitude.value, sensorValues->gpsSet.unixSeconds.value,
|
|
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
|
|
sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.unixSeconds.isValid()),
|
|
&acsParameters->gpsParameters, gpsDataProcessed);
|
|
|
|
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(),
|
|
timeAbsolute, timeDelta, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
|
|
mgmDataProcessed);
|
|
|
|
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
|
|
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
|
|
sensorValues->susSets[2].channels.value, sensorValues->susSets[2].channels.isValid(),
|
|
sensorValues->susSets[3].channels.value, sensorValues->susSets[3].channels.isValid(),
|
|
sensorValues->susSets[4].channels.value, sensorValues->susSets[4].channels.isValid(),
|
|
sensorValues->susSets[5].channels.value, sensorValues->susSets[5].channels.isValid(),
|
|
sensorValues->susSets[6].channels.value, sensorValues->susSets[6].channels.isValid(),
|
|
sensorValues->susSets[7].channels.value, sensorValues->susSets[7].channels.isValid(),
|
|
sensorValues->susSets[8].channels.value, sensorValues->susSets[8].channels.isValid(),
|
|
sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(),
|
|
sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
|
|
sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
|
|
timeAbsolute, timeDelta, &acsParameters->susHandlingParameters,
|
|
&acsParameters->sunModelParameters, susDataProcessed);
|
|
|
|
processGyr(
|
|
sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
|
|
sensorValues->gyr0AdisSet.angVelocY.value, sensorValues->gyr0AdisSet.angVelocY.isValid(),
|
|
sensorValues->gyr0AdisSet.angVelocZ.value, sensorValues->gyr0AdisSet.angVelocZ.isValid(),
|
|
sensorValues->gyr1L3gSet.angVelocX.value, sensorValues->gyr1L3gSet.angVelocX.isValid(),
|
|
sensorValues->gyr1L3gSet.angVelocY.value, sensorValues->gyr1L3gSet.angVelocY.isValid(),
|
|
sensorValues->gyr1L3gSet.angVelocZ.value, sensorValues->gyr1L3gSet.angVelocZ.isValid(),
|
|
sensorValues->gyr2AdisSet.angVelocX.value, sensorValues->gyr2AdisSet.angVelocX.isValid(),
|
|
sensorValues->gyr2AdisSet.angVelocY.value, sensorValues->gyr2AdisSet.angVelocY.isValid(),
|
|
sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(),
|
|
sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
|
|
sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
|
|
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(),
|
|
&acsParameters->gyrHandlingParameters, gyrDataProcessed);
|
|
}
|
|
|
|
void SensorProcessing::lowPassFilter(double *newValue, double *oldValue, const double weight) {
|
|
double leftSide[3] = {0, 0, 0}, rightSide[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(newValue, (1 - weight), leftSide, 3);
|
|
VectorOperations<double>::mulScalar(oldValue, weight, rightSide, 3);
|
|
VectorOperations<double>::add(leftSide, rightSide, newValue, 3);
|
|
}
|