eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

revert single axis pointing to original code
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-21 10:22:02 +01:00
parent 3ad6c8a56c
commit 5349fb45e3
2 changed files with 44 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
mission/controller/acs/Guidance.cpp
View File

@ -16,13 +16,13 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters
Guidance::~Guidance() {} Guidance::~Guidance() {}
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double refDirB[3], void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3],
double quatIB[4], double targetQuat[4], double sunDirI[3], double refDirB[3], double quatBI[4],
double targetSatRotRate[3]) { double targetQuat[4], double targetSatRotRate[3]) {
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude // Calculation of target quaternion to groundstation or given latitude, longitude and altitude
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF) // transform longitude, latitude and altitude to ECEF
double targetE[3] = {0, 0, 0}; double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
@ -30,7 +30,11 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re
acsParameters.targetModeControllerParameters.longitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE); acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
// transformation between ECEF and ECI frame // target direction in the ECEF frame
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 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 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}}; double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -40,36 +44,44 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot); MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
// target direction in the ECI frame // transformation between ECEF and Body frame
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0}; double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1); double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
// reference direction in ECI frame QuaternionOperations::toDcm(quatBI, dcmBI);
double refDirI[3] = {0, 0, 0}; MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
QuaternionOperations::multiplyVector(quatIB, refDirB, refDirI);
// rotation quaternion from two vectors // target Direction in the body frame
double crossDirI[3] = {0, 0, 0}; double targetDirB[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(targetDirI, refDirI); MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
VectorOperations<double>::cross(targetDirI, refDirI, crossDirI);
targetQuat[0] = crossDirI[0]; // rotation quaternion from two vectors
targetQuat[1] = crossDirI[1]; double refDir[3] = {0, 0, 0};
targetQuat[2] = crossDirI[2]; refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
targetQuat[3] = sqrt(pow(VectorOperations<double>::norm(targetDirI, 3), 2) * refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
pow(VectorOperations<double>::norm(refDirI, 3), 2) + refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
dotDirections); 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); VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of reference rotation rate // calculation of reference rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0}; 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 // velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3);
MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1); MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3); VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
@ -79,21 +91,14 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re
double satRateDir[3] = {0, 0, 0}; double satRateDir[3] = {0, 0, 0};
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir); VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
VectorOperations<double>::normalize(satRateDir, satRateDir, 3); VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3); VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of reference rotation rate in case of star tracker blinding // Calculation of reference rotation rate in case of star tracker blinding
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) { if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
double sunDirB[3] = {0, 0, 0}; double sunDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
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, double exclAngle = acsParameters.strParameters.exclusionAngle,
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart, blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
@ -103,18 +108,14 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re
if (!(strBlindAvoidFlag)) { if (!(strBlindAvoidFlag)) {
double critSightAngle = blindStart * exclAngle; double critSightAngle = blindStart * exclAngle;
if (sightAngleSun < critSightAngle) { if (sightAngleSun < critSightAngle) {
strBlindAvoidFlag = true; strBlindAvoidFlag = true;
} }
} }
else { else {
if (sightAngleSun < blindEnd * exclAngle) { if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate; double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0}; double blindRefRate[3] = {0, 0, 0};
if (sunDirB[1] < 0) { if (sunDirB[1] < 0) {
blindRefRate[0] = normBlindRefRate; blindRefRate[0] = normBlindRefRate;
blindRefRate[1] = 0; blindRefRate[1] = 0;
@ -124,9 +125,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double re
blindRefRate[1] = 0; blindRefRate[1] = 0;
blindRefRate[2] = 0; blindRefRate[2] = 0;
} }
VectorOperations<double>::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3);
VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
} else { } else {
strBlindAvoidFlag = false; strBlindAvoidFlag = false;
} }

4
mission/controller/acs/Guidance.h
View File

@ -16,7 +16,9 @@ class Guidance {
// Function to get the target quaternion and refence rotation rate from gps position and // Function to get the target quaternion and refence rotation rate from gps position and
// position of the ground station // position of the ground station
void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]); void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
double refDirB[3], double quatBI[4], double targetQuat[4],
double targetSatRotRate[3]);
void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4], void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
double targetSatRotRate[3]); double targetSatRotRate[3]);
void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],