changed calculation of quaternion for target and sun pointing
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
This commit is contained in:
Robin Marquardt
parent
609d429161
commit
8d1cbd9f8b
@ -168,10 +168,10 @@ void AcsController::performPointingCtrl() {
|
||||
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
||||
break;
|
||||
case SUBMODE_PTG_SUN:
|
||||
guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
|
||||
guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
||||
break;
|
||||
case SUBMODE_PTG_NADIR:
|
||||
guidance.quatNadirPtgFLPVersion(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
||||
guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
||||
break;
|
||||
case SUBMODE_PTG_INERTIAL:
|
||||
guidance.inertialQuatPtg(targetQuat, refSatRate);
|
||||
|
||||
@ -850,8 +850,8 @@ class AcsParameters /*: public HasParametersIF*/ {
|
||||
double omegaEarth = 0.000072921158553;
|
||||
|
||||
double nadirRefDirection[3] = {-1, 0, 0}; //Camera
|
||||
double refQuatInertial[4] = {0, 0, 0, 1};
|
||||
double refRotRateInertial[3] = {0, 0, 0};
|
||||
double tgtQuatInertial[4] = {0, 0, 0, 1};
|
||||
double tgtRotRateInertial[3] = {0, 0, 0};
|
||||
int8_t nadirTimeElapsedMax = 10;
|
||||
} pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
|
||||
|
||||
|
||||
@ -29,7 +29,7 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
|
||||
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
|
||||
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
|
||||
@ -181,7 +181,124 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
|
||||
}
|
||||
}
|
||||
|
||||
void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
|
||||
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
|
||||
@ -194,30 +311,34 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou
|
||||
quatBJ[3] = outputValues->quatMekfBJ[3];
|
||||
QuaternionOperations::toDcm(quatBJ, dcmBJ);
|
||||
|
||||
double sunDirJ[3] = {0, 0, 0}, sunDir[3] = {0, 0, 0};
|
||||
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, sunDir, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
|
||||
}
|
||||
|
||||
else {
|
||||
sunDir[0] = outputValues->sunDirEst[0];
|
||||
sunDir[1] = outputValues->sunDirEst[1];
|
||||
sunDir[2] = outputValues->sunDirEst[2];
|
||||
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(sunDir, sunRef, sunCross);
|
||||
double normSunDir = VectorOperations<double>::norm(sunDir, 3);
|
||||
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(sunDir, sunRef);
|
||||
double dotSun = VectorOperations<double>::dot(sunDirB, sunRef);
|
||||
|
||||
targetQuat[0] = sunCross[0];
|
||||
targetQuat[1] = sunCross[1];
|
||||
@ -225,17 +346,73 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou
|
||||
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::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
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
|
||||
//-------------------------------------------------------------------------------------
|
||||
@ -301,7 +478,7 @@ void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *
|
||||
|
||||
}
|
||||
|
||||
void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
void Guidance::quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
@ -351,7 +528,8 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp
|
||||
|
||||
//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]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt,targetQuat);
|
||||
double quatInertialTarget[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::fromDcm(dcmTgt,quatInertialTarget);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
@ -360,15 +538,15 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp
|
||||
timeSavedQuaternionNadir.tv_usec * pow(timeSavedQuaternionNadir.tv_usec,-6));
|
||||
if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) {
|
||||
double qDiff[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetQuat, savedQuaternionNadir, qDiff, 4);
|
||||
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4);
|
||||
VectorOperations<double>::mulScalar(qDiff, 1/timeElapsed, qDiff, 4);
|
||||
|
||||
double tgtQuatVec[3] = {targetQuat[0], targetQuat[1], targetQuat[2]},
|
||||
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(targetQuat, qDiff, sum1);
|
||||
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
|
||||
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
|
||||
VectorOperations<double>::mulScalar(qDiffVec, targetQuat[3], sum3, 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};
|
||||
@ -387,18 +565,26 @@ void Guidance::quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::Outp
|
||||
}
|
||||
|
||||
timeSavedQuaternionNadir = now;
|
||||
savedQuaternionNadir[0] = targetQuat[0];
|
||||
savedQuaternionNadir[1] = targetQuat[1];
|
||||
savedQuaternionNadir[2] = targetQuat[2];
|
||||
savedQuaternionNadir[3] = targetQuat[3];
|
||||
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.refQuatInertial[i];
|
||||
targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i];
|
||||
}
|
||||
for (int i = 0; i < 3; i++) {
|
||||
refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRateInertial[i];
|
||||
refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -23,18 +23,20 @@ public:
|
||||
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 targetQuatPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate for sun pointing after ground station
|
||||
void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
|
||||
void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing
|
||||
void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
void quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
void quatNadirPtgFLPVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from parameters for inertial pointing
|
||||
|
||||
Loading…
Reference in New Issue
Block a user