when did i push this last
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
This commit is contained in:
parent
700d7ced64
commit
04e1cb52ac
@ -249,26 +249,21 @@ void AcsController::performPointingCtrl() {
|
||||
satRotRate[3] = {0, 0, 0}, errorSatRotRate[3] = {0, 0, 0};
|
||||
switch (submode) {
|
||||
case acs::PTG_IDLE:
|
||||
guidance.targetQuatPtgSun(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
|
||||
targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
|
||||
4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
|
||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
case acs::PTG_TARGET:
|
||||
@ -296,17 +291,15 @@ void AcsController::performPointingCtrl() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
case acs::PTG_TARGET_GS:
|
||||
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
|
||||
targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
|
||||
4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
|
||||
guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
|
||||
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
|
||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
@ -319,16 +312,16 @@ void AcsController::performPointingCtrl() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
case acs::PTG_NADIR:
|
||||
guidance.targetQuatPtgNadirThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
|
||||
targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
@ -341,13 +334,13 @@ void AcsController::performPointingCtrl() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
case acs::PTG_INERTIAL:
|
||||
guidance.targetQuatPtgInertial(targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
|
||||
4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
|
||||
@ -363,6 +356,7 @@ void AcsController::performPointingCtrl() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
@ -841,10 +841,12 @@ class AcsParameters : public HasParametersIF {
|
||||
uint8_t enableAntiStiction = true;
|
||||
} pointingLawParameters;
|
||||
|
||||
struct IdleModeControllerParameters : PointingLawParameters {
|
||||
} idleModeControllerParameters;
|
||||
|
||||
struct TargetModeControllerParameters : PointingLawParameters {
|
||||
double refDirection[3] = {-1, 0, 0}; // Antenna
|
||||
double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to
|
||||
// give this as an input- currently en calculation is done
|
||||
double refRotRate[3] = {0, 0, 0};
|
||||
double quatRef[4] = {0, 0, 0, 1};
|
||||
int8_t timeElapsedMax = 10; // rot rate calculations
|
||||
|
||||
@ -860,6 +862,16 @@ class AcsParameters : public HasParametersIF {
|
||||
double blindRotRate = 1 * M_PI / 180;
|
||||
} targetModeControllerParameters;
|
||||
|
||||
struct GsTargetModeControllerParameters : PointingLawParameters {
|
||||
double refDirection[3] = {-1, 0, 0}; // Antenna
|
||||
int8_t timeElapsedMax = 10; // rot rate calculations
|
||||
|
||||
// Default is Stuttgart GS
|
||||
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
} gsTargetModeControllerParameters;
|
||||
|
||||
struct NadirModeControllerParameters : PointingLawParameters {
|
||||
double refDirection[3] = {-1, 0, 0}; // Antenna
|
||||
double quatRef[4] = {0, 0, 0, 1};
|
||||
|
||||
@ -17,12 +17,12 @@ Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters
|
||||
Guidance::~Guidance() {}
|
||||
|
||||
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double targetQuat[4],
|
||||
double refSatRate[3]) {
|
||||
double targetSatRotRate[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)
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (earth
|
||||
// fixed/centered frame)
|
||||
double targetCart[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
@ -30,19 +30,19 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
|
||||
acsParameters.targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
|
||||
|
||||
// Target direction in the ECEF frame
|
||||
// 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);
|
||||
// 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);
|
||||
|
||||
// Transformation between ECEF and Body frame
|
||||
// double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -52,20 +52,16 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ta
|
||||
// QuaternionOperations::toDcm(quatBJ, dcmBJ);
|
||||
// MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
|
||||
|
||||
// Target Direction in the body frame
|
||||
// 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 refDirB[3] = {0, 0, 0};
|
||||
std::memcpy(refDirB, acsParameters.targetModeControllerParameters.refDirection,
|
||||
3 * sizeof(double));
|
||||
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
||||
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
||||
double normRefDirB = VectorOperations<double>::norm(refDir, 3);
|
||||
double crossDir[3] = {0, 0, 0};
|
||||
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
||||
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
|
||||
@ -152,7 +148,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for target pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double targetE[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.targetModeControllerParameters.latitudeTgt,
|
||||
@ -161,7 +157,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
||||
|
||||
// transformation between ECEF and ECI frame
|
||||
// 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}};
|
||||
@ -171,7 +167,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
|
||||
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
|
||||
// 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);
|
||||
@ -213,133 +209,96 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
|
||||
targetRotationRate(timeElapsedMax, now, quatIX, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
|
||||
double targetSatRotRate[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};
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double groundStationE[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
|
||||
acsParameters.gsTargetModeControllerParameters.latitudeTgt,
|
||||
acsParameters.gsTargetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
|
||||
VectorOperations<double>::subtract(groundStationE, 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);
|
||||
// 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);
|
||||
|
||||
// get Sun Vector Model in ECI
|
||||
double sunJ[3];
|
||||
std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
|
||||
VectorOperations<double>::normalize(sunJ, sunJ, 3);
|
||||
// get sun vector model in ECI
|
||||
VectorOperations<double>::normalize(sunDirI, sunDirI, 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);
|
||||
double xDotS = VectorOperations<double>::dot(xAxis, sunDirI);
|
||||
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>::subtract(sunDirI, sunParallel, zAxis, 3);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
|
||||
// calculate y-axis
|
||||
// y-axis completes RHS
|
||||
double yAxis[3];
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// Complete transformation matrix
|
||||
// join 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);
|
||||
int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, now, quatInertialTarget, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgSun(timeval now, double targetQuat[4], double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgSun(double sunDirI[3], 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
|
||||
// positive z-Axis of EIVE in direction of sun
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirB, zAxis, 3);
|
||||
VectorOperations<double>::normalize(sunDirI, zAxis, 3);
|
||||
|
||||
// Assign helper vector (north pole inertial)
|
||||
// assign helper vector (north pole inertial)
|
||||
double helperVec[3] = {0, 0, 1};
|
||||
|
||||
// Construct y-axis from helper vector and z-axis
|
||||
// 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
|
||||
// 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
|
||||
// join 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 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];
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
@ -501,6 +460,31 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
||||
// under 150 arcsec ??
|
||||
}
|
||||
|
||||
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double errorQuat[4],
|
||||
double errorSatRotRate[3], double errorAngle) {
|
||||
// First calculate error quaternion between current and target orientation
|
||||
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
|
||||
// Keep scalar part of quaternion positive
|
||||
if (errorQuat[3] < 0) {
|
||||
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
|
||||
}
|
||||
// Calculate error angle
|
||||
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
|
||||
|
||||
// Only give back error satellite rotational rate if orientation has already been aquired
|
||||
if (errorAngle < 2. / 180. * M_PI) {
|
||||
// Then substract the combined required satellite rotational rates from the actual rate
|
||||
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
|
||||
} else {
|
||||
// If orientation has not been aquired yet set satellite rotational rate to zero
|
||||
errorSatRotRate = 0;
|
||||
}
|
||||
|
||||
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
|
||||
// under 150 arcsec ??
|
||||
}
|
||||
|
||||
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *refSatRate) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
|
||||
@ -19,11 +19,12 @@ class Guidance {
|
||||
void targetQuatPtgSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
|
||||
double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(timeval now, double targetQuat[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
|
||||
double targetSatRotRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate for sun pointing after ground
|
||||
// station
|
||||
void targetQuatPtgSun(timeval now, double targetQuat[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
|
||||
// pointing
|
||||
@ -41,6 +42,9 @@ class Guidance {
|
||||
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
|
||||
double errorQuat[4], double errorSatRotRate[3], double errorAngle);
|
||||
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
|
||||
double errorAngle);
|
||||
|
||||
void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *targetSatRotRate);
|
||||
|
||||
@ -1,10 +1,3 @@
|
||||
/*
|
||||
* SensorProcessing.cpp
|
||||
*
|
||||
* Created on: 7 Mar 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
#include "SensorProcessing.h"
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user