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

broke everything to fix everything
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-17 13:44:44 +01:00
parent d9708c6175
commit f0d77125e1
1 changed files with 11 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,9 +36,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
3 * sizeof(double));
}
void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
void Guidance::targetQuatPtgSingleAxis(acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude
@ -52,13 +52,6 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
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;
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
@ -108,8 +101,6 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
double velSatE[3] = {0, 0, 0};
std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
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);
@ -221,9 +212,7 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn
savedQuaternion[3] = quatInertialTarget[3];
}
void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now,
void Guidance::targetQuatPtgThreeAxes(acsctrl::MekfData *mekfData, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
@ -231,14 +220,10 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
// fixed/centered frame)
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
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};
std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
@ -300,10 +285,7 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
void Guidance::targetQuatPtgGs(timeval now, double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
//-------------------------------------------------------------------------------------
@ -315,12 +297,6 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
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;
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);
@ -379,10 +355,7 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
}
void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
void Guidance::sunQuatPtg(timeval now, double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
//-------------------------------------------------------------------------------------
@ -448,18 +421,11 @@ void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *me
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
timeval now, double targetQuat[4],
void Guidance::quatNadirPtgSingleAxis(timeval now, double targetQuat[4],
double refSatRate[3]) { // old version of Nadir Pointing
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// 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>::mulScalar(posSatE, -1, targetDirE, 3);
@ -512,19 +478,11 @@ void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
double refSatRate[3]) {
void Guidance::quatNadirPtgThreeAxes(double posSateE[3], double velSateE[3], timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// 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>::mulScalar(posSatE, -1, targetDirE, 3);
@ -548,7 +506,7 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
// z-Axis parallel to long side of picture resolution
double zAxis[3] = {0, 0, 0}, velocityE[3];
double zAxis[3] = {0, 0, 0};
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
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);
@ -584,9 +542,8 @@ void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
3 * sizeof(double));
}
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
double refSatRate[3], double quatErrorComplete[4], double quatError[3],
double deltaRate[3]) {
void Guidance::comparePtg(double targetQuat[4], double quatRef[4], double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
double satRate[3] = {0, 0, 0};
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);