added 3 axes stabilized target mode for GS contact. renamed tgt coordinates acordingly
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:
@ -39,12 +39,11 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
|
||||
// fixed/centered frame)
|
||||
double groundStationCart[3] = {0, 0, 0};
|
||||
double targetCart[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
|
||||
acsParameters.groundStationParameters.longitudeGs,
|
||||
acsParameters.groundStationParameters.altitudeGs,
|
||||
groundStationCart);
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
|
||||
acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
|
||||
|
||||
// Position of the satellite in the earth/fixed frame via GPS
|
||||
double posSatE[3] = {0, 0, 0};
|
||||
@ -55,7 +54,7 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
|
||||
|
||||
// Target direction in the ECEF frame
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
|
||||
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -173,6 +172,48 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
|
||||
}
|
||||
}
|
||||
|
||||
void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
double timeElapsed =
|
||||
now.tv_sec + now.tv_usec * pow(10, -6) -
|
||||
(timeSavedQuaternionNadir.tv_sec +
|
||||
timeSavedQuaternionNadir.tv_usec * pow((double)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];
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
acsctrl::MekfData *mekfData, timeval now,
|
||||
@ -182,12 +223,11 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
|
||||
// fixed/centered frame)
|
||||
double groundStationCart[3] = {0, 0, 0};
|
||||
double targetCart[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
|
||||
acsParameters.groundStationParameters.longitudeGs,
|
||||
acsParameters.groundStationParameters.altitudeGs,
|
||||
groundStationCart);
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
|
||||
acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
|
||||
// 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;
|
||||
@ -195,7 +235,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
|
||||
sensorValues->gpsSet.altitude.value, posSatE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
|
||||
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -246,45 +286,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
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((double)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];
|
||||
refRotationRate(now, quatInertialTarget, refSatRate);
|
||||
|
||||
// Transform in system relative to satellite frame
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
@ -295,7 +297,75 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {}
|
||||
double targetQuat[4], double refSatRate[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};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
|
||||
acsParameters.ptgTargetParameters.altitudeTgt, 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);
|
||||
|
||||
// get Sun Vector Model in ECI as helper vector (element of x-z plane)
|
||||
double sunJ[3];
|
||||
std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
|
||||
VectorOperations<double>::normalize(sunJ, sunJ, 3);
|
||||
|
||||
// calculate y-axis
|
||||
double yAxis[3];
|
||||
VectorOperations<double>::cross(sunJ, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// calculate z-axis
|
||||
double zAxis[3];
|
||||
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
|
||||
// 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);
|
||||
|
||||
refRotationRate(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);
|
||||
}
|
||||
|
||||
void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
@ -317,32 +387,6 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
|
||||
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
|
||||
}
|
||||
|
||||
/*
|
||||
// ---------------------------------------------------------------------------
|
||||
// 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}};
|
||||
@ -526,45 +570,7 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
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((double)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];
|
||||
refRotationRate(now, quatInertialTarget, refSatRate);
|
||||
|
||||
// Transform in system relative to satellite frame
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
|
||||
Reference in New Issue
Block a user