some things also broken some things fixed again
This commit is contained in:
@ -12,27 +12,12 @@
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
|
||||
Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
|
||||
|
||||
Guidance::~Guidance() {}
|
||||
|
||||
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
|
||||
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
|
||||
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
|
||||
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
|
||||
3 * sizeof(double));
|
||||
} else {
|
||||
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4],
|
||||
double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude
|
||||
//-------------------------------------------------------------------------------------
|
||||
@ -49,7 +34,7 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
// Transformation between ECEF and ECI frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -60,13 +45,12 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
|
||||
// Transformation between ECEF and Body frame
|
||||
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
||||
// double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// double quatBJ[4] = {0, 0, 0, 0};
|
||||
|
||||
QuaternionOperations::toDcm(quatBJ, dcmBJ);
|
||||
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
|
||||
// QuaternionOperations::toDcm(quatBJ, dcmBJ);
|
||||
// MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
|
||||
|
||||
// Target Direction in the body frame
|
||||
double targetDirB[3] = {0, 0, 0};
|
||||
@ -163,119 +147,70 @@ void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
|
||||
}
|
||||
}
|
||||
|
||||
void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *refSatRate) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
|
||||
(timeSavedQuaternion.tv_sec +
|
||||
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
|
||||
if (timeElapsed < timeElapsedMax) {
|
||||
double qDiff[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
|
||||
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
|
||||
|
||||
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
|
||||
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
|
||||
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
|
||||
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
|
||||
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
|
||||
VectorOperations<double>::add(sum1, sum2, sum, 3);
|
||||
VectorOperations<double>::subtract(sum, sum3, sum, 3);
|
||||
double omegaRefNew[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
|
||||
|
||||
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
|
||||
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
|
||||
omegaRefSaved[0] = omegaRefNew[0];
|
||||
omegaRefSaved[1] = omegaRefNew[1];
|
||||
omegaRefSaved[2] = omegaRefNew[2];
|
||||
} else {
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
}
|
||||
|
||||
timeSavedQuaternion = now;
|
||||
savedQuaternion[0] = quatInertialTarget[0];
|
||||
savedQuaternion[1] = quatInertialTarget[1];
|
||||
savedQuaternion[2] = quatInertialTarget[2];
|
||||
savedQuaternion[3] = quatInertialTarget[3];
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgThreeAxes(acsctrl::MekfData *mekfData, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
|
||||
double quatIX[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for target pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
|
||||
// fixed/centered frame)
|
||||
double targetCart[3] = {0, 0, 0};
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double targetE[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
|
||||
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// Target Direction and position vector in the inertial frame
|
||||
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
|
||||
// target direction and position vector in the inertial frame
|
||||
double targetDirI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, targetDirE, targetDirI, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
|
||||
// negative x-Axis aligned with target (Camera/E-band transmitter position)
|
||||
// negative x-axis aligned with target
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
|
||||
VectorOperations<double>::normalize(targetDirI, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
|
||||
// Transform velocity into inertial frame
|
||||
double velocityE[3];
|
||||
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
|
||||
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
|
||||
// transform velocity into inertial frame
|
||||
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
|
||||
|
||||
// orbital normal vector
|
||||
double orbitalNormalJ[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
|
||||
VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
|
||||
// orbital normal vector of target and velocity vector
|
||||
double orbitalNormalI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
|
||||
VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
|
||||
|
||||
// y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
|
||||
// y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
|
||||
// resolution
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
|
||||
VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// z-Axis completes RHS
|
||||
// z-axis completes RHS
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
|
||||
|
||||
// Complete transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
double quatInertialTarget[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
|
||||
// join transformation matrix
|
||||
double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, quatIX);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
|
||||
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
|
||||
|
||||
// Transform in system relative to satellite frame
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
||||
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
|
||||
refRotationRate(timeElapsedMax, now, quatIX, refSatRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) {
|
||||
@ -293,7 +228,7 @@ void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatR
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
// Transformation between ECEF and ECI frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -367,7 +302,7 @@ void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSat
|
||||
return;
|
||||
}
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
// Transformation between ECEF and ECI frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -422,7 +357,7 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
// Transformation between ECEF and ECI frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -479,7 +414,7 @@ void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], tim
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
// Transformation between ECEF and ECI frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -560,6 +495,48 @@ void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSat
|
||||
// under 150 arcsec ??
|
||||
}
|
||||
|
||||
void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *refSatRate) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
|
||||
(timeSavedQuaternion.tv_sec +
|
||||
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
|
||||
if (timeElapsed < timeElapsedMax) {
|
||||
double qDiff[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
|
||||
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
|
||||
|
||||
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
|
||||
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
|
||||
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
|
||||
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
|
||||
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
|
||||
VectorOperations<double>::add(sum1, sum2, sum, 3);
|
||||
VectorOperations<double>::subtract(sum, sum3, sum, 3);
|
||||
double omegaRefNew[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
|
||||
|
||||
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
|
||||
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
|
||||
omegaRefSaved[0] = omegaRefNew[0];
|
||||
omegaRefSaved[1] = omegaRefNew[1];
|
||||
omegaRefSaved[2] = omegaRefNew[2];
|
||||
} else {
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
}
|
||||
|
||||
timeSavedQuaternion = now;
|
||||
savedQuaternion[0] = quatInertialTarget[0];
|
||||
savedQuaternion[1] = quatInertialTarget[1];
|
||||
savedQuaternion[2] = quatInertialTarget[2];
|
||||
savedQuaternion[3] = quatInertialTarget[3];
|
||||
}
|
||||
|
||||
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||
double *rwPseudoInv) {
|
||||
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
|
||||
@ -590,3 +567,16 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
|
||||
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
|
||||
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
|
||||
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
|
||||
3 * sizeof(double));
|
||||
} else {
|
||||
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user