This commit is contained in:
parent
338c8299e2
commit
053c1a7fe8
347
mission/controller/acs/Guidance.cpp
Normal file
347
mission/controller/acs/Guidance.cpp
Normal file
@ -0,0 +1,347 @@
|
||||
/*
|
||||
* Guidance.cpp
|
||||
*
|
||||
* Created on: 6 Jun 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
|
||||
#include "Guidance.h"
|
||||
#include "string.h"
|
||||
#include <math.h>
|
||||
#include <acs/util/MathOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <acs/util/CholeskyDecomposition.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::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
|
||||
timeval now, double targetQuat[4], double refSatRate[3]){
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to groundstation
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Transform longitude, latitude and altitude of groundstation 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};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gps0latitude, sensorValues->gps0longitude,
|
||||
sensorValues->gps0altitude, 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}};
|
||||
MathOperations<double>::dcmEJ(now, *dcmEJ);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
||||
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
|
||||
double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth;
|
||||
|
||||
// TEST SECTION !
|
||||
double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, dcmTEST, dcmTEST, 3, 3, 3);
|
||||
|
||||
MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
|
||||
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] = sensorValues->gps0Velocity[0];
|
||||
velSatE[1] = sensorValues->gps0Velocity[1];
|
||||
velSatE[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 sunDirJ[3] = {0, 0, 0};
|
||||
double 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];
|
||||
|
||||
}
|
||||
|
||||
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::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->validRw0 && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) {
|
||||
|
||||
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->validRw0) && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) {
|
||||
|
||||
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->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) {
|
||||
|
||||
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->validRw0) && (sensorValues->validRw1) && !(sensorValues->validRw2) && sensorValues->validRw3) {
|
||||
|
||||
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->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && (sensorValues->validRw3)) {
|
||||
|
||||
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 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];
|
||||
|
||||
}
|
||||
}
|
||||
42
mission/controller/acs/Guidance.h
Normal file
42
mission/controller/acs/Guidance.h
Normal file
@ -0,0 +1,42 @@
|
||||
/*
|
||||
* Guidance.h
|
||||
*
|
||||
* Created on: 6 Jun 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
#ifndef GUIDANCE_H_
|
||||
#define GUIDANCE_H_
|
||||
|
||||
|
||||
#include <AcsParameters.h>
|
||||
#include <SensorValues.h>
|
||||
#include <OutputValues.h>
|
||||
#include <time.h>
|
||||
|
||||
|
||||
class Guidance {
|
||||
public:
|
||||
Guidance(AcsParameters *acsParameters_);
|
||||
virtual ~Guidance();
|
||||
|
||||
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position and position of the ground station
|
||||
void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired
|
||||
void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] );
|
||||
|
||||
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid reation wheel
|
||||
// maybe can be done in "commanding.h"
|
||||
void getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv);
|
||||
|
||||
|
||||
private:
|
||||
AcsParameters acsParameters;
|
||||
bool strBlindAvoidFlag = false;
|
||||
};
|
||||
|
||||
#endif /* ACS_GUIDANCE_H_ */
|
||||
Loading…
Reference in New Issue
Block a user