Merge pull request 'PTG Something' (#856) from fix-your-rfs into main
EIVE/eive-obsw/pipeline/head This commit looks good
Details
EIVE/eive-obsw/pipeline/head This commit looks good
Details
Reviewed-on: #856 Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
commit
5ae97d7c09
|
|
@ -16,8 +16,14 @@ will consitute of a breaking change warranting a new major release:
|
|||
|
||||
# [unreleased]
|
||||
|
||||
## Changed
|
||||
|
||||
- Guidance now uses the coordinate functions from the FSFW.
|
||||
- Idle should now point the STR away from the earth
|
||||
|
||||
## Fixed
|
||||
|
||||
- Fixed bugs in `Guidance::comparePtg` and corrected overloading
|
||||
- Detumbling State Machine is now robust to commanded mode changes.
|
||||
|
||||
# [v7.6.0] 2024-01-30
|
||||
|
|
|
|||
|
|
@ -420,8 +420,8 @@ void AcsController::performPointingCtrl() {
|
|||
|
||||
switch (mode) {
|
||||
case acs::PTG_IDLE:
|
||||
guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.targetQuatPtgIdle(timeAbsolute, timeDelta, susDataProcessed.sunIjkModel.value,
|
||||
gpsDataProcessed.gpsPosition.value, targetQuat, targetSatRotRate);
|
||||
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
|
|
@ -440,9 +440,9 @@ void AcsController::performPointingCtrl() {
|
|||
break;
|
||||
|
||||
case acs::PTG_TARGET:
|
||||
guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.targetQuatPtgTarget(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
|
||||
acsParameters.targetModeControllerParameters.quatRef,
|
||||
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
|
||||
|
|
@ -483,9 +483,8 @@ void AcsController::performPointingCtrl() {
|
|||
break;
|
||||
|
||||
case acs::PTG_NADIR:
|
||||
guidance.targetQuatPtgNadirThreeAxes(
|
||||
timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
|
||||
guidance.targetQuatPtgNadir(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
|
||||
guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
|
||||
acsParameters.nadirModeControllerParameters.quatRef,
|
||||
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
|
||||
|
|
|
|||
|
|
@ -4,415 +4,200 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameter
|
|||
|
||||
Guidance::~Guidance() {}
|
||||
|
||||
[[deprecated]] void Guidance::targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3],
|
||||
double velSatE[3], double sunDirI[3],
|
||||
double refDirB[3], double quatBI[4],
|
||||
double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to ECEF
|
||||
double targetE[3] = {0, 0, 0};
|
||||
void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
|
||||
const double sunDirI[3], const double posSatF[4],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
// positive z-Axis of EIVE in direction of sun
|
||||
double zAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirI, zAxisXI, 3);
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters->targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
|
||||
// determine helper vector to point x-Axis and therefore the STR away from Earth
|
||||
double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
|
||||
VectorOperations<double>::normalize(posSatI, helperXI, 3);
|
||||
|
||||
// target direction in the ECEF frame
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
||||
// construct y-axis from helper vector and z-axis
|
||||
double yAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisXI, helperXI, yAxisXI);
|
||||
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
|
||||
|
||||
// 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(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
// x-axis completes RHS
|
||||
double xAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(yAxisXI, zAxisXI, xAxisXI);
|
||||
VectorOperations<double>::normalize(xAxisXI, xAxisXI, 3);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
// join transformation matrix
|
||||
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
|
||||
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
|
||||
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
|
||||
QuaternionOperations::fromDcm(dcmXI, targetQuat);
|
||||
|
||||
// transformation between ECEF and Body frame
|
||||
double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
|
||||
QuaternionOperations::toDcm(quatBI, dcmBI);
|
||||
MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *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 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(*dcmBI, *dcmIEDot, *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, targetSatRotRate, 3);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate in case of star tracker blinding
|
||||
//-------------------------------------------------------------------------------------
|
||||
if (acsParameters->targetModeControllerParameters.avoidBlindStr) {
|
||||
double sunDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
|
||||
|
||||
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, targetSatRotRate, targetSatRotRate, 3);
|
||||
} else {
|
||||
strBlindAvoidFlag = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
// revert calculated quaternion from qBX to qIX
|
||||
double quatIB[4] = {0, 0, 0, 1};
|
||||
QuaternionOperations::inverse(quatBI, quatIB);
|
||||
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
|
||||
// calculate of reference rotation rate
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta,
|
||||
double posSatE[3], double velSatE[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double velSatF[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for target pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double targetE[3] = {0, 0, 0};
|
||||
double targetF[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters->targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
||||
|
||||
// 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(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
|
||||
double targetDirF[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetF, posSatF, targetDirF, 3);
|
||||
|
||||
// target direction in the ECI frame
|
||||
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
|
||||
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
|
||||
CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute);
|
||||
VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
|
||||
|
||||
// x-axis aligned with target direction
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirI, xAxis, 3);
|
||||
double xAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirI, xAxisXI, 3);
|
||||
|
||||
// transform velocity into inertial frame
|
||||
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
|
||||
double velSatI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute);
|
||||
|
||||
// orbital normal vector of target and velocity vector
|
||||
double orbitalNormalI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
|
||||
VectorOperations<double>::cross(posSatI, velSatI, orbitalNormalI);
|
||||
VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
|
||||
|
||||
// y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
|
||||
// resolution
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
double yAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalI, xAxisXI, yAxisXI);
|
||||
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
|
||||
|
||||
// z-axis completes RHS
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
|
||||
double zAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxisXI, yAxisXI, zAxisXI);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
|
||||
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
|
||||
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta,
|
||||
double posSatE[3], double sunDirI[3], double targetQuat[4],
|
||||
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for ground station pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double groundStationE[3] = {0, 0, 0};
|
||||
|
||||
double posGroundStationF[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
|
||||
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
|
||||
acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
|
||||
|
||||
// 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(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
|
||||
|
||||
// target direction in the ECI frame
|
||||
double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
|
||||
VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
|
||||
double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
|
||||
CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute);
|
||||
VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
|
||||
|
||||
// negative x-axis aligned with target direction
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
double xAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(groundStationDirI, xAxisXI, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 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, sunDirI);
|
||||
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
|
||||
double sunParallel[3], zAxis[3];
|
||||
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
|
||||
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxis, 3);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
double xDotS = VectorOperations<double>::dot(xAxisXI, sunDirI);
|
||||
xDotS /= pow(VectorOperations<double>::norm(xAxisXI, 3), 2);
|
||||
double sunParallel[3], zAxisXI[3];
|
||||
VectorOperations<double>::mulScalar(xAxisXI, xDotS, sunParallel, 3);
|
||||
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxisXI, 3);
|
||||
VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
|
||||
|
||||
// y-axis completes RHS
|
||||
double yAxis[3];
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
double yAxisXI[3];
|
||||
VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
|
||||
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
|
||||
|
||||
// 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]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
|
||||
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
|
||||
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
|
||||
QuaternionOperations::fromDcm(dcmXI, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgSun(double timeDelta, double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to sun
|
||||
//-------------------------------------------------------------------------------------
|
||||
// positive z-Axis of EIVE in direction of sun
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirI, zAxis, 3);
|
||||
|
||||
// assign helper vector (north pole inertial)
|
||||
double helperVec[3] = {0, 0, 1};
|
||||
|
||||
// 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);
|
||||
|
||||
// x-axis completes RHS
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
|
||||
VectorOperations<double>::normalize(xAxis, xAxis, 3);
|
||||
|
||||
// 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]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//----------------------------------------------------------------------------
|
||||
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
[[deprecated]] void Guidance::targetQuatPtgNadirSingleAxis(const timeval timeAbsolute,
|
||||
double posSatE[3], double quatBI[4],
|
||||
double targetQuat[4], double refDirB[3],
|
||||
double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3],
|
||||
double velSatE[3], double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
||||
|
||||
// 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(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
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 dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
QuaternionOperations::toDcm(quatBI, dcmBI);
|
||||
MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *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->nadirModeControllerParameters.refDirection[0];
|
||||
refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1];
|
||||
refDir[2] = acsParameters->nadirModeControllerParameters.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
|
||||
//-------------------------------------------------------------------------------------
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
|
||||
// revert calculated quaternion from qBX to qIX
|
||||
double quatIB[4] = {0, 0, 0, 1};
|
||||
QuaternionOperations::inverse(quatBI, quatIB);
|
||||
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
|
||||
double posSatE[3], double velSatE[3],
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// 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(timeAbsolute, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// satellite position in inertial reference frame
|
||||
double posSatI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute);
|
||||
|
||||
// negative x-axis aligned with position vector
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
double xAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, xAxisXI, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
|
||||
|
||||
// make z-Axis parallel to major part of camera resolution
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
|
||||
VectorOperations<double>::cross(xAxis, velocityI, zAxis);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
double zAxisXI[3] = {0, 0, 0};
|
||||
double velSatI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
|
||||
VectorOperations<double>::cross(xAxisXI, velSatI, zAxisXI);
|
||||
VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
|
||||
|
||||
// y-Axis completes RHS
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
double yAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
|
||||
|
||||
// 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]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
|
||||
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
|
||||
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
|
||||
QuaternionOperations::fromDcm(dcmXI, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, timeDelta, targetQuat, refSatRate);
|
||||
targetRotationRate(timeDelta, targetQuat, refSatRate);
|
||||
}
|
||||
|
||||
void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) {
|
||||
if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
}
|
||||
if (timeDelta != 0.0) {
|
||||
QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
|
||||
} else {
|
||||
std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
|
||||
}
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
}
|
||||
|
||||
void Guidance::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) {
|
||||
// First calculate error quaternion between current and target orientation
|
||||
double invTargetQuat[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::inverse(targetQuat, invTargetQuat);
|
||||
QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat);
|
||||
// Last calculate add rotation from reference quaternion
|
||||
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
|
||||
// First calculate error quaternion between current and target orientation without reference
|
||||
// quaternion
|
||||
double errorQuatWoRef[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
|
||||
// Then add rotation from reference quaternion
|
||||
QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
|
||||
// Keep scalar part of quaternion positive
|
||||
if (errorQuat[3] < 0) {
|
||||
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
|
||||
|
|
@ -435,30 +220,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
|||
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);
|
||||
|
||||
// Calculate error satellite rotational rate
|
||||
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
|
||||
}
|
||||
|
||||
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
|
||||
double quatIX[4], double *refSatRate) {
|
||||
if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
}
|
||||
if (timeDelta != 0.0) {
|
||||
QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
|
||||
} else {
|
||||
std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
|
||||
}
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0};
|
||||
comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate,
|
||||
errorQuat, errorSatRotRate, errorAngle);
|
||||
}
|
||||
|
||||
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#ifndef GUIDANCE_H_
|
||||
#define GUIDANCE_H_
|
||||
|
||||
#include <fsfw/coordinates/CoordinateTransformations.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
|
|
@ -24,33 +25,18 @@ class Guidance {
|
|||
ReturnValue_t solarArrayDeploymentComplete();
|
||||
void resetValues();
|
||||
|
||||
// Function to get the target quaternion and reference rotation rate from gps position and
|
||||
// position of the ground station
|
||||
void targetQuatPtgSingleAxis(const timeval timeAbsolute, double posSatE[3], double velSatE[3],
|
||||
double sunDirI[3], double refDirB[3], double quatBI[4],
|
||||
double targetQuat[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgThreeAxes(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
|
||||
double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(const timeval timeAbsolute, const double timeDelta, double posSatE[3],
|
||||
void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3],
|
||||
const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double velSatF[3], double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// Function to get the target quaternion and reference rotation rate for sun pointing after ground
|
||||
// station
|
||||
void targetQuatPtgSun(const double timeDelta, double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]);
|
||||
void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
|
||||
double *targetSatRotRate);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
|
||||
// pointing
|
||||
void targetQuatPtgNadirSingleAxis(const timeval timeAbsolute, double posSatE[3], double quatBI[4],
|
||||
double targetQuat[4], double refDirB[3], double refSatRate[3]);
|
||||
void targetQuatPtgNadirThreeAxes(const timeval timeAbsolute, const double timeDelta,
|
||||
double posSatE[3], double velSatE[3], double targetQuat[4],
|
||||
double refSatRate[3]);
|
||||
|
||||
// @note: Calculates the error quaternion between the current orientation and the target
|
||||
// quaternion, considering a reference quaternion. Additionally the difference between the actual
|
||||
// and a desired satellite rotational rate is calculated, again considering a reference rotational
|
||||
// rate. Lastly gives back the error angle of the error quaternion.
|
||||
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);
|
||||
|
|
@ -58,11 +44,6 @@ class Guidance {
|
|||
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
|
||||
double &errorAngle);
|
||||
|
||||
void targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
|
||||
double quatInertialTarget[4], double *targetSatRotRate);
|
||||
|
||||
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
|
||||
// reation wheel maybe can be done in "commanding.h"
|
||||
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
|
||||
|
||||
private:
|
||||
|
|
|
|||
Loading…
Reference in New Issue