583 lines
27 KiB
C++
583 lines
27 KiB
C++
#include "Guidance.h"
|
|
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <math.h>
|
|
|
|
#include <filesystem>
|
|
|
|
#include "string.h"
|
|
#include "util/CholeskyDecomposition.h"
|
|
#include "util/MathOperations.h"
|
|
|
|
Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
|
|
|
|
Guidance::~Guidance() {}
|
|
|
|
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
|
|
//-------------------------------------------------------------------------------------
|
|
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
|
|
// fixed/centered frame)
|
|
double targetCart[3] = {0, 0, 0};
|
|
|
|
MathOperations<double>::cartesianFromLatLongAlt(
|
|
acsParameters.targetModeControllerParameters.latitudeTgt,
|
|
acsParameters.targetModeControllerParameters.longitudeTgt,
|
|
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
|
|
|
|
// Target direction in the ECEF frame
|
|
double targetDirE[3] = {0, 0, 0};
|
|
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
|
|
|
|
// 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}};
|
|
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
|
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
|
|
|
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
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};
|
|
|
|
// 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};
|
|
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
|
|
|
|
// rotation quaternion from two vectors
|
|
double refDir[3] = {0, 0, 0};
|
|
refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
|
|
refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
|
|
refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
|
|
double noramlizedTargetDirB[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
|
|
VectorOperations<double>::normalize(refDir, refDir, 3);
|
|
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
|
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
|
double crossDir[3] = {0, 0, 0};
|
|
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
|
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
|
|
targetQuat[0] = crossDir[0];
|
|
targetQuat[1] = crossDir[1];
|
|
targetQuat[2] = crossDir[2];
|
|
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
|
|
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
// Calculation of reference rotation rate
|
|
//-------------------------------------------------------------------------------------
|
|
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
|
|
// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
|
|
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
|
|
double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MatrixOperations<double>::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3);
|
|
MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
|
|
VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
|
|
|
|
double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
|
|
double normRefSatRate = normVelSatB / normTargetDirB;
|
|
|
|
double satRateDir[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
|
|
VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
|
|
VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3);
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
// Calculation of reference rotation rate in case of star tracker blinding
|
|
//-------------------------------------------------------------------------------------
|
|
if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
|
|
double sunDirB[3] = {0, 0, 0};
|
|
|
|
if (susDataProcessed->sunIjkModel.isValid()) {
|
|
double sunDirJ[3] = {0, 0, 0};
|
|
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
|
|
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
|
|
} else {
|
|
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
|
|
}
|
|
|
|
double exclAngle = acsParameters.strParameters.exclusionAngle,
|
|
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
|
|
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
|
|
double sightAngleSun =
|
|
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
|
|
|
|
if (!(strBlindAvoidFlag)) {
|
|
double critSightAngle = blindStart * exclAngle;
|
|
|
|
if (sightAngleSun < critSightAngle) {
|
|
strBlindAvoidFlag = true;
|
|
}
|
|
|
|
}
|
|
|
|
else {
|
|
if (sightAngleSun < blindEnd * exclAngle) {
|
|
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
|
|
double blindRefRate[3] = {0, 0, 0};
|
|
|
|
if (sunDirB[1] < 0) {
|
|
blindRefRate[0] = normBlindRefRate;
|
|
blindRefRate[1] = 0;
|
|
blindRefRate[2] = 0;
|
|
} else {
|
|
blindRefRate[0] = -normBlindRefRate;
|
|
blindRefRate[1] = 0;
|
|
blindRefRate[2] = 0;
|
|
}
|
|
|
|
VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
|
|
|
|
} else {
|
|
strBlindAvoidFlag = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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 (ECEF)
|
|
double targetE[3] = {0, 0, 0};
|
|
MathOperations<double>::cartesianFromLatLongAlt(
|
|
acsParameters.targetModeControllerParameters.latitudeTgt,
|
|
acsParameters.targetModeControllerParameters.longitudeTgt,
|
|
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
|
|
double targetDirE[3] = {0, 0, 0};
|
|
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
|
|
|
// 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 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 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
|
|
// this aligns with the camera, E- and S-band antennas
|
|
double xAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(targetDirI, xAxis, 3);
|
|
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 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 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 is parallel to long side of picture
|
|
// resolution
|
|
double yAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
|
|
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
|
|
|
// z-axis completes RHS
|
|
double zAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
|
|
|
|
// 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, quatIX, refSatRate);
|
|
}
|
|
|
|
void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) {
|
|
//-------------------------------------------------------------------------------------
|
|
// Calculation of target quaternion for ground station pointing
|
|
//-------------------------------------------------------------------------------------
|
|
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
|
|
// fixed/centered frame)
|
|
double groundStationCart[3] = {0, 0, 0};
|
|
|
|
MathOperations<double>::cartesianFromLatLongAlt(
|
|
acsParameters.targetModeControllerParameters.latitudeTgt,
|
|
acsParameters.targetModeControllerParameters.longitudeTgt,
|
|
acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
|
|
double targetDirE[3] = {0, 0, 0};
|
|
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
|
|
|
|
// 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}};
|
|
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
|
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
|
|
|
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
|
|
|
// 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);
|
|
|
|
// negative x-Axis aligned with target (Camera/E-band transmitter position)
|
|
double xAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
|
|
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
|
|
|
// get Sun Vector Model in ECI
|
|
double sunJ[3];
|
|
std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
|
|
VectorOperations<double>::normalize(sunJ, sunJ, 3);
|
|
|
|
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
|
|
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
|
|
double xDotS = VectorOperations<double>::dot(xAxis, sunJ);
|
|
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
|
|
double sunParallel[3], zAxis[3];
|
|
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
|
|
VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
|
|
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
|
|
|
// calculate y-axis
|
|
double yAxis[3];
|
|
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
|
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
|
|
|
// 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);
|
|
|
|
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);
|
|
}
|
|
|
|
void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]) {
|
|
//-------------------------------------------------------------------------------------
|
|
// Calculation of target quaternion to sun
|
|
//-------------------------------------------------------------------------------------
|
|
double quatBJ[4] = {0, 0, 0, 0};
|
|
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
|
QuaternionOperations::toDcm(quatBJ, dcmBJ);
|
|
|
|
double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
|
|
if (susDataProcessed->sunIjkModel.isValid()) {
|
|
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
|
|
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
|
|
} else if (susDataProcessed->susVecTot.isValid()) {
|
|
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
|
|
} else {
|
|
return;
|
|
}
|
|
|
|
// 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}};
|
|
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
|
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
|
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
|
|
|
// positive z-Axis of EIVE in direction of sun
|
|
double zAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(sunDirB, zAxis, 3);
|
|
|
|
// Assign helper vector (north pole inertial)
|
|
double helperVec[3] = {0, 0, 1};
|
|
|
|
// Construct y-axis from helper vector and z-axis
|
|
double yAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
|
|
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
|
|
|
// Construct x-axis from y- and z-axes
|
|
double xAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
|
|
VectorOperations<double>::normalize(xAxis, xAxis, 3);
|
|
|
|
// Transformation matrix to Sun, no further transforamtions, reference is already
|
|
// the EIVE body frame
|
|
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
|
{xAxis[1], yAxis[1], zAxis[1]},
|
|
{xAxis[2], yAxis[2], zAxis[2]}};
|
|
double quatSun[4] = {0, 0, 0, 0};
|
|
QuaternionOperations::fromDcm(dcmTgt, quatSun);
|
|
|
|
targetQuat[0] = quatSun[0];
|
|
targetQuat[1] = quatSun[1];
|
|
targetQuat[2] = quatSun[2];
|
|
targetQuat[3] = quatSun[3];
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calculation of reference rotation rate
|
|
//----------------------------------------------------------------------------
|
|
refSatRate[0] = 0;
|
|
refSatRate[1] = 0;
|
|
refSatRate[2] = 0;
|
|
}
|
|
|
|
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
|
|
double refSatRate[3]) { // old version of Nadir Pointing
|
|
//-------------------------------------------------------------------------------------
|
|
// Calculation of target quaternion for Nadir pointing
|
|
//-------------------------------------------------------------------------------------
|
|
double targetDirE[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
|
|
|
// 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}};
|
|
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
|
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
|
|
|
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
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));
|
|
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};
|
|
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
|
|
|
|
// rotation quaternion from two vectors
|
|
double refDir[3] = {0, 0, 0};
|
|
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
|
|
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
|
|
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
|
|
double noramlizedTargetDirB[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
|
|
VectorOperations<double>::normalize(refDir, refDir, 3);
|
|
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
|
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
|
double crossDir[3] = {0, 0, 0};
|
|
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
|
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
|
|
targetQuat[0] = crossDir[0];
|
|
targetQuat[1] = crossDir[1];
|
|
targetQuat[2] = crossDir[2];
|
|
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
|
|
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
// Calculation of reference rotation rate
|
|
//-------------------------------------------------------------------------------------
|
|
refSatRate[0] = 0;
|
|
refSatRate[1] = 0;
|
|
refSatRate[2] = 0;
|
|
}
|
|
|
|
void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], timeval now,
|
|
double targetQuat[4], double refSatRate[3]) {
|
|
//-------------------------------------------------------------------------------------
|
|
// Calculation of target quaternion for Nadir pointing
|
|
//-------------------------------------------------------------------------------------
|
|
double targetDirE[3] = {0, 0, 0};
|
|
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
|
|
|
// 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}};
|
|
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
|
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
|
|
|
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
|
|
|
// Target Direction in the body frame
|
|
double targetDirJ[3] = {0, 0, 0};
|
|
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
|
|
|
|
// negative x-Axis aligned with target (Camera position)
|
|
double xAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
|
|
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
|
|
|
// z-Axis parallel to long side of picture resolution
|
|
double zAxis[3] = {0, 0, 0};
|
|
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);
|
|
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
|
|
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
|
|
|
// y-Axis completes RHS
|
|
double yAxis[3] = {0, 0, 0};
|
|
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
|
|
|
// 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);
|
|
|
|
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.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);
|
|
}
|
|
|
|
void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) {
|
|
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
|
|
4 * sizeof(double));
|
|
std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
|
|
3 * sizeof(double));
|
|
}
|
|
|
|
void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3],
|
|
double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
|
|
double satRate[3] = {0, 0, 0};
|
|
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
|
|
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
|
|
// valid checks ?
|
|
double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
|
|
{-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]},
|
|
{quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
|
|
{quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
|
|
|
|
MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
|
|
|
|
if (quatErrorComplete[3] < 0) {
|
|
quatErrorComplete[3] *= -1;
|
|
}
|
|
|
|
quatError[0] = quatErrorComplete[0];
|
|
quatError[1] = quatErrorComplete[1];
|
|
quatError[2] = quatErrorComplete[2];
|
|
|
|
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
|
|
// 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());
|
|
bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
|
|
bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
|
|
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
|
|
|
|
if (rw1valid && rw2valid && rw3valid && rw4valid) {
|
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
|
|
return returnvalue::OK;
|
|
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
|
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
|
|
return returnvalue::OK;
|
|
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
|
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
|
|
return returnvalue::OK;
|
|
} else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
|
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
|
|
return returnvalue::OK;
|
|
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
|
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
|
|
return returnvalue::OK;
|
|
} else {
|
|
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
|
|
// Does not make sense, but is implemented that way in MATLAB ?!
|
|
// Thought: It does not really play a role, because in case there are more then one
|
|
// reaction wheel invalid the pointing control is destined to fail.
|
|
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));
|
|
}
|