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

nadir should work again
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-20 16:20:54 +01:00
parent 48b0ee8662
commit 81a4112c45
3 changed files with 12 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
mission/controller/AcsController.cpp
View File

@ -311,11 +311,13 @@ void AcsController::performPointingCtrl() {
break;
case acs::PTG_NADIR:
guidance.targetQuatPtgNadirThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
targetQuat, refSatRate);
std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
deltaRate);
guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
gpsDataProcessed.gpsVelocity.value, targetQuat,
targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(

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

@ -364,8 +364,8 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4],
refSatRate[2] = 0;
}
void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]) {
void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
@ -409,12 +409,7 @@ void Guidance::quatNadirPtgThreeAxes(timeval now, double posSatE[3], double velS
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
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};
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
}
void Guidance::targetQuatPtgInertial(double targetQuat[4], double refSatRate[3]) {

3
mission/controller/acs/Guidance.h
View File

@ -29,7 +29,8 @@ class Guidance {
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
// pointing
void targetQuatPtgNadirSingleAxis(timeval now, double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgNadirThreeAxes(timeval now, double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from parameters for inertial
// pointing