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

#include "FusedRotationEstimation.h"

FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) {
  acsParameters = acsParameters_;
}

void FusedRotationEstimation::estimateFusedRotationRateSafe(
    bool sunValid, double sunB[3], double sunRateB[3], bool magValid, double magB[3],
    double magRateB[3], bool rotRateValid, double rotRateB[3], double fusedRateB[3],
    double fusedRateParallelB[3], double fusedRateOrthogonalB[3]) {
  if ((not magValid) or (not sunValid and not VectorOperations<double>::norm(fusedRateOldB, 3)) or
      ((not VectorOperations<double>::norm(sunRateB, 3) and
        not VectorOperations<double>::norm(magRateB, 3)))) {
    return;
  }
  if (not sunValid) {
    estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
                                     fusedRateOrthogonalB);
    return;
  }

  // calculate rotation around the sun
  double magSunCross[3] = {0, 0, 0};

  VectorOperations<double>::cross(magB, sunB, magSunCross);
  double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3);
  double magNorm = VectorOperations<double>::norm(magB, 3);
  if (magSunCrossNorm >
      (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
    double omegaParallel =
        VectorOperations<double>::dot(magRateB, magSunCross) * pow(magSunCrossNorm, -2);
    double omegaParallelVec[3] = {0, 0, 0};
    VectorOperations<double>::mulScalar(sunB, omegaParallel, omegaParallelVec, 3);
    std::memcpy(fusedRateParallelB, omegaParallelVec, 3 * sizeof(double));  // to dataset
  } else {
    estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
                                     fusedRateOrthogonalB);  // ToDo
    return;
  }

  // calculate rotation orthogonal to the sun
  VectorOperations<double>::cross(sunRateB, sunB, fusedRateOrthogonalB);
  VectorOperations<double>::mulScalar(fusedRateOrthogonalB,
                                      pow(VectorOperations<double>::norm(sunB, 2), 2),
                                      fusedRateOrthogonalB, 3);

  // calculate total rotation rate
  VectorOperations<double>::add(fusedRateParallelB, fusedRateOrthogonalB, fusedRateB);

  std::memcpy(fusedRateOldB, fusedRateB, 3 * sizeof(double));
  if (rotRateValid) {
    std::memcpy(rotRateOldB, rotRateB, 3 * sizeof(double));
  }
}

void FusedRotationEstimation::estimateFusedRotationRateEclipse(bool rotRateValid,
                                                               double rotRateB[3],
                                                               double fusedRateB[3],
                                                               double fusedRateParallelB[3],
                                                               double fusedRateOrthogonalB[3]) {
  if (not rotRateValid or not VectorOperations<double>::norm(fusedRateOldB, 3)) {
    return;
  }
  double angAccelB[3] = {0, 0, 0};
  VectorOperations<double>::subtract(rotRateB, rotRateOldB, angAccelB, 3);
  double omegaTotVec[3] = {0, 0, 0};
  VectorOperations<double>::add(fusedRateOldB, angAccelB, fusedRateB, 3);
}