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:
@ -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) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
|
||||
Reference in New Issue
Block a user