eive-obsw/mission/controller/acs/Guidance.cpp
Robin Marquardt 8d1cbd9f8b
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
changed calculation of quaternion for target and sun pointing
2022-11-24 13:40:55 +01:00

726 lines
35 KiB
C++

/*
* Guidance.cpp
*
* Created on: 6 Jun 2022
* Author: Robin Marquardt
*/
#include "Guidance.h"
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include "string.h"
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
Guidance::~Guidance() {}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
for (int i = 0; i < 3; i++) {
sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i];
}
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
}
void Guidance::targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
timeval now, 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 groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
acsParameters.groundStationParameters.longitudeGs,
acsParameters.groundStationParameters.altitudeGs,
groundStationCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, 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);
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};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[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};
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 velSatE[3] = {0, 0, 0};
velSatE[0] = outputValues->gpsVelocity[0]; // sensorValues->gps0Velocity[0];
velSatE[1] = outputValues->gpsVelocity[1]; // sensorValues->gps0Velocity[1];
velSatE[2] = outputValues->gpsVelocity[2]; // sensorValues->gps0Velocity[2];
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 (outputValues->sunDirModelValid) {
double sunDirJ[3] = {0, 0, 0};
sunDirJ[0] = outputValues->sunDirModel[0];
sunDirJ[1] = outputValues->sunDirModel[1];
sunDirJ[2] = outputValues->sunDirModel[2];
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
}
else {
sunDirB[0] = outputValues->sunDirEst[0];
sunDirB[1] = outputValues->sunDirEst[1];
sunDirB[2] = outputValues->sunDirEst[2];
}
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::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
acsParameters.groundStationParameters.longitudeGs,
acsParameters.groundStationParameters.altitudeGs,
groundStationCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationCart, 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);
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);
// Transform velocity into inertial frame
double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]};
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);
// orbital normal vector
double orbitalNormalJ[3] = {0, 0, 0};
VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
// y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// 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);
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec +
timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6));
if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, 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, omegaRefSavedNadir, refSatRate, 3);
omegaRefSavedNadir[0] = omegaRefNew[0];
omegaRefSavedNadir[1] = omegaRefNew[1];
omegaRefSavedNadir[2] = omegaRefNew[2];
}
else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
timeSavedQuaternionNadir = now;
savedQuaternionNadir[0] = quatInertialTarget[0];
savedQuaternionNadir[1] = quatInertialTarget[1];
savedQuaternionNadir[2] = quatInertialTarget[2];
savedQuaternionNadir[3] = quatInertialTarget[3];
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, 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}};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
QuaternionOperations::toDcm(quatBJ, dcmBJ);
double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
if (outputValues->sunDirModelValid) {
sunDirJ[0] = outputValues->sunDirModel[0];
sunDirJ[1] = outputValues->sunDirModel[1];
sunDirJ[2] = outputValues->sunDirModel[2];
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
}
else {
sunDirB[0] = outputValues->sunDirEst[0];
sunDirB[1] = outputValues->sunDirEst[1];
sunDirB[2] = outputValues->sunDirEst[2];
}
/*
// ---------------------------------------------------------------------------
// Old version of two vector quaternion (only one axis to align)
// ---------------------------------------------------------------------------
double sunRef[3] = {0, 0, 0};
sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0];
sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1];
sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2];
double sunCross[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDirB, sunRef, sunCross);
double normSunDir = VectorOperations<double>::norm(sunDirB, 3);
double normSunRef = VectorOperations<double>::norm(sunRef, 3);
double dotSun = VectorOperations<double>::dot(sunDirB, sunRef);
targetQuat[0] = sunCross[0];
targetQuat[1] = sunCross[1];
targetQuat[2] = sunCross[2];
targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
*/
//----------------------------------------------------------------------------
// New version
//----------------------------------------------------------------------------
// 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);
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);
// Position of the satellite in the earth/fixed frame via GPS and body
// velocity
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]};
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);
double velocityB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBJ, velocityJ, velocityB, 3, 3, 1);
// Normale to velocity and sunDir
double crossVelSun[3] = {0, 0, 0};
VectorOperations<double>::cross(velocityB, sunDirB, crossVelSun);
// y- Axis as cross of normal velSun and zAxis
double yAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(crossVelSun, sunDirB, yAxis);
VectorOperations<double>::normalize(yAxis, yAxis, 3);
// complete RHS for x-Axis
double xAxis[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
// 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::quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]) { // old version of Nadir Pointing
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, 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);
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};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[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};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[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::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, 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);
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};
double velocityE[3] = {outputValues->gpsVelocity[0], outputValues->gpsVelocity[1], outputValues->gpsVelocity[2]};
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);
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double timeElapsed = now.tv_sec + now.tv_usec * pow(10,-6) - (timeSavedQuaternionNadir.tv_sec +
timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6));
if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, 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, omegaRefSavedNadir, refSatRate, 3);
omegaRefSavedNadir[0] = omegaRefNew[0];
omegaRefSavedNadir[1] = omegaRefNew[1];
omegaRefSavedNadir[2] = omegaRefNew[2];
}
else {
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
timeSavedQuaternionNadir = now;
savedQuaternionNadir[0] = quatInertialTarget[0];
savedQuaternionNadir[1] = quatInertialTarget[1];
savedQuaternionNadir[2] = quatInertialTarget[2];
savedQuaternionNadir[3] = quatInertialTarget[3];
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
for (int i = 0; i < 4; i++) {
targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i];
}
for (int i = 0; i < 3; i++) {
refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i];
}
}
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
double refSatRate[3], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0};
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
double satRate[3] = {0, 0, 0};
satRate[0] = outputValues->satRateMekf[0];
satRate[1] = outputValues->satRateMekf[1];
satRate[2] = outputValues->satRateMekf[2];
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]}};
double quatErrorComplete[4] = {0, 0, 0, 0};
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::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() &&
sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
}
else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
!(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
(sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) {
rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2];
}
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.
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
}
}