eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

changed pointingParameters struct
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt
2023-01-12 15:19:21 +01:00
parent 25867e76b1
commit d8c0ba19fd
7 changed files with 117 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
mission/controller/acs/Guidance.cpp
View File

@ -42,8 +42,8 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
@ -172,17 +172,17 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
}
}
void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate) {
void Guidance::refRotationRate(int8_t timeElapsedMax, 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) {
(timeSavedQuaternion.tv_sec +
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
if (timeElapsed < timeElapsedMax) {
double qDiff[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternionNadir, qDiff, 4);
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
@ -197,21 +197,21 @@ void Guidance::refRotationRate(timeval now, double quatInertialTarget[4], double
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];
VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
omegaRefSaved[0] = omegaRefNew[0];
omegaRefSaved[1] = omegaRefNew[1];
omegaRefSaved[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];
timeSavedQuaternion = now;
savedQuaternion[0] = quatInertialTarget[0];
savedQuaternion[1] = quatInertialTarget[1];
savedQuaternion[2] = quatInertialTarget[2];
savedQuaternion[3] = quatInertialTarget[3];
}
void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
@ -226,8 +226,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
acsParameters.ptgTargetParameters.altitudeTgt, targetCart);
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.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;
@ -286,7 +286,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
refRotationRate(now, quatInertialTarget, refSatRate);
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};
@ -306,8 +307,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt,
acsParameters.ptgTargetParameters.altitudeTgt, groundStationCart);
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.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;
@ -363,7 +364,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
refRotationRate(now, quatInertialTarget, refSatRate);
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};
@ -495,9 +497,9 @@ void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[2];
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);
@ -576,7 +578,8 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
refRotationRate(now, quatInertialTarget, refSatRate);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
// Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0};
@ -586,20 +589,15 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
for (int i = 0; i < 4; i++) {
targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuatInertial[i];
targetQuat[i] = acsParameters.inertialModeControllerParameters.tgtQuat[i];
}
for (int i = 0; i < 3; i++) {
refSatRate[i] = acsParameters.inertialModeControllerParameters.tgtRotRateInertial[i];
refSatRate[i] = acsParameters.inertialModeControllerParameters.refRotRate[i];
}
}
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0};
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
double satRate[3] = {0, 0, 0};
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));