eive-obsw/mission/controller/acs/SensorProcessing.cpp

#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 timeOfMgmMeasurement,
                                  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::GpsSource::NONE) {
    Igrf13Model igrf13;
    igrf13.schmidtNormalization();
    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(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
                        gpsDataProcessed->altitude.value, timeOfMgmMeasurement, 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;
  double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
  if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
      VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
    VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
    VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3);
    mgmVecTotDerivativeValid = true;
  }
  timeOfSavedMagFieldEst = timeOfMgmMeasurement;
  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 timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
    const AcsParameters::SunModelParameters *sunModelParameters,
    acsctrl::SusDataProcessed *susDataProcessed) {
  /* -------- Sun Model Direction (IJK frame) ------- */
  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);

  // 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;
  double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
  if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
      VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
    VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
    VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, 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);
  }
  timeOfSavedSusDirEst = timeOfSusMeasurement;
  {
    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,
    timeval timeOfGyrMeasurement, 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 gpsUnixSeconds,
                                  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::GpsSource::NONE;
  // We do not trust the GPS and therefore it shall die here if SPG4 is running
  if (gpsDataProcessed->source.value == acs::GpsSource::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
        (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
        (gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
      VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
      double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
      VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
    }
    savedPosSatE[0] = posSatE[0];
    savedPosSatE[1] = posSatE[1];
    savedPosSatE[2] = posSatE[2];

    timeOfSavedPosSatE = gpsUnixSeconds;
    validSavedPosSatE = true;

    gpsSource = acs::GpsSource::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 now, 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(),
             now, &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(),
             now, &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(), now,
      &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);
}