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

Final Version of the ACS Controller #367

Merged
muellerr merged 78 commits from eggert/acs into develop 2023-02-08 13:50:11 +01:00
Conversation 0 Commits 78 Files Changed 27 +117 -91
7 changed files with 117 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit d8c0ba19fd - Show all commits

30
mission/controller/AcsController.cpp
View File

@ -269,30 +269,56 @@ void AcsController::performControlOperation() {
&mekfData, &validMekf); &mekfData, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0}; double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 0};
uint8_t enableAntiStiction = true;
switch (submode) { switch (submode) {
case SUBMODE_IDLE: case SUBMODE_IDLE:
guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now, guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
targetQuat, refSatRate); targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break; break;
case SUBMODE_PTG_TARGET: case SUBMODE_PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
targetQuat, refSatRate); targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break; break;
case SUBMODE_PTG_TARGET_GS: case SUBMODE_PTG_TARGET_GS:
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
now, targetQuat, refSatRate); now, targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break; break;
case SUBMODE_PTG_NADIR: case SUBMODE_PTG_NADIR:
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat, guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate); refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.nadirModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break; break;
case SUBMODE_PTG_INERTIAL: case SUBMODE_PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate); guidance.inertialQuatPtg(targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.inertialModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break; break;
} }
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0}, double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate); guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0; double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
@ -305,7 +331,7 @@ void AcsController::performControlOperation() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled); actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) { if ( enableAntiStiction ) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = { int32_t rwSpeed[4] = {
(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),

15
mission/controller/acs/AcsParameters.cpp
View File

@ -372,21 +372,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0xE: case 0xE:
parameterWrapper->set(targetModeControllerParameters.desatOn); parameterWrapper->set(targetModeControllerParameters.desatOn);
break; break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.omegaEarth);
break;
case 0x10:
parameterWrapper->set(targetModeControllerParameters.nadirRefDirection);
break;
case 0x11:
parameterWrapper->set(targetModeControllerParameters.tgtQuatInertial);
break;
case 0x12:
parameterWrapper->set(targetModeControllerParameters.tgtRotRateInertial);
break;
case 0x13:
parameterWrapper->set(targetModeControllerParameters.nadirTimeElapsedMax);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }

66
mission/controller/acs/AcsParameters.h
View File

@ -815,35 +815,51 @@ class AcsParameters : public HasParametersIF {
} safeModeControllerParameters; } safeModeControllerParameters;
// ToDo: mutiple structs for different pointing mode controllers? struct PointingLawParameters {
struct PointingModeControllerParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
uint8_t avoidBlindStr = true;
double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180;
double zeta = 0.3; double zeta = 0.3;
double om = 0.3; double om = 0.3;
double omMax = 1 * M_PI / 180; double omMax = 1 * M_PI / 180;
double qiMin = 0.1; double qiMin = 0.1;
double gainNullspace = 0.01; double gainNullspace = 0.01;
double desatMomentumRef[3] = {0, 0, 0}; double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000; double deSatGainFactor = 1000;
uint8_t desatOn = true; uint8_t desatOn = true;
uint8_t enableAntiStiction = true; uint8_t enableAntiStiction = true;
double omegaEarth = 0.000072921158553; } pointingLawParameters;
double nadirRefDirection[3] = {-1, 0, 0}; // Camera struct TargetModeControllerParameters : PointingLawParameters {
double tgtQuatInertial[4] = {0, 0, 0, 1}; double refDirection[3] = {-1, 0, 0}; // Antenna
double tgtRotRateInertial[3] = {0, 0, 0}; double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to
int8_t nadirTimeElapsedMax = 10; // give this as an input- currently en calculation is done
} pointingModeControllerParameters, inertialModeControllerParameters, double quatRef[4] = {0, 0, 0, 1};
nadirModeControllerParameters, targetModeControllerParameters; int8_t timeElapsedMax = 10; // rot rate calculations
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeTgt = 500; // [m]
// For one-axis control:
uint8_t avoidBlindStr = true;
double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180;
} targetModeControllerParameters;
struct NadirModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
double quatRef[4] = {0, 0, 0, 1};
int8_t timeElapsedMax = 10; // rot rate calculations
} nadirModeControllerParameters;
struct InertialModeControllerParameters : PointingLawParameters {
double tgtQuat[4] = {0, 0, 0, 1};
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
} inertialModeControllerParameters;
struct StrParameters { struct StrParameters {
double exclusionAngle = 20 * M_PI / 180; double exclusionAngle = 20 * M_PI / 180;

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}; double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.ptgTargetParameters.altitudeTgt, targetCart); acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; 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 // Calculation of reference rotation rate
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double timeElapsed = double timeElapsed =
now.tv_sec + now.tv_usec * pow(10, -6) - now.tv_sec + now.tv_usec * pow(10, -6) -
(timeSavedQuaternionNadir.tv_sec + (timeSavedQuaternion.tv_sec +
timeSavedQuaternionNadir.tv_usec * pow((double)timeSavedQuaternionNadir.tv_usec, -6)); timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
if (timeElapsed < acsParameters.pointingModeControllerParameters.nadirTimeElapsedMax) { if (timeElapsed < timeElapsedMax) {
double qDiff[4] = {0, 0, 0, 0}; 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); VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, 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(sum, -2, omegaRefNew, 3);
VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3); VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
VectorOperations<double>::subtract(refSatRate, omegaRefSavedNadir, refSatRate, 3); VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
omegaRefSavedNadir[0] = omegaRefNew[0]; omegaRefSaved[0] = omegaRefNew[0];
omegaRefSavedNadir[1] = omegaRefNew[1]; omegaRefSaved[1] = omegaRefNew[1];
omegaRefSavedNadir[2] = omegaRefNew[2]; omegaRefSaved[2] = omegaRefNew[2];
} else { } else {
refSatRate[0] = 0; refSatRate[0] = 0;
refSatRate[1] = 0; refSatRate[1] = 0;
refSatRate[2] = 0; refSatRate[2] = 0;
} }
timeSavedQuaternionNadir = now; timeSavedQuaternion = now;
savedQuaternionNadir[0] = quatInertialTarget[0]; savedQuaternion[0] = quatInertialTarget[0];
savedQuaternionNadir[1] = quatInertialTarget[1]; savedQuaternion[1] = quatInertialTarget[1];
savedQuaternionNadir[2] = quatInertialTarget[2]; savedQuaternion[2] = quatInertialTarget[2];
savedQuaternionNadir[3] = quatInertialTarget[3]; savedQuaternion[3] = quatInertialTarget[3];
} }
void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues, void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
@ -226,8 +226,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
double targetCart[3] = {0, 0, 0}; double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.ptgTargetParameters.altitudeTgt, targetCart); acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; 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}; double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); 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 // Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0}; 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}; double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.ptgTargetParameters.latitudeTgt, acsParameters.ptgTargetParameters.longitudeTgt, acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.ptgTargetParameters.altitudeTgt, groundStationCart); acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
// Position of the satellite in the earth/fixed frame via GPS // Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0}; double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180; 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}; double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); 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 // Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0}; double quatBJ[4] = {0, 0, 0, 0};
@ -495,9 +497,9 @@ void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::
// rotation quaternion from two vectors // rotation quaternion from two vectors
double refDir[3] = {0, 0, 0}; double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0]; refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1]; refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[2]; refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0}; double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3); VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3); VectorOperations<double>::normalize(refDir, refDir, 3);
@ -576,7 +578,8 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
double quatInertialTarget[4] = {0, 0, 0, 0}; double quatInertialTarget[4] = {0, 0, 0, 0};
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget); 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 // Transform in system relative to satellite frame
double quatBJ[4] = {0, 0, 0, 0}; 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]) { void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
for (int i = 0; i < 4; i++) { 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++) { 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 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}; double satRate[3] = {0, 0, 0};
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double)); std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));

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

@ -58,10 +58,10 @@ class Guidance {
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
// desired // desired
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3], void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]); double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
void refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate); void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *refSatRate);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h" // reation wheel maybe can be done in "commanding.h"
@ -70,9 +70,9 @@ class Guidance {
private: private:
AcsParameters acsParameters; AcsParameters acsParameters;
bool strBlindAvoidFlag = false; bool strBlindAvoidFlag = false;
timeval timeSavedQuaternionNadir; timeval timeSavedQuaternion;
double savedQuaternionNadir[4] = {0, 0, 0, 0}; double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSavedNadir[3] = {0, 0, 0}; double omegaRefSaved[3] = {0, 0, 0};
}; };
#endif /* ACS_GUIDANCE_H_ */ #endif /* ACS_GUIDANCE_H_ */

19
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -21,7 +21,8 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) { void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters); // TODO: Here correct Parameters have to be loaded according to current submode
pointingLawParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE); inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters); rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices); rwMatrices = &(acsParameters_->rwMatrices);
@ -32,10 +33,10 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix // Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om; double om = pointingLawParameters->om;
double zeta = pointingModeControllerParameters->zeta; double zeta = pointingLawParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin; double qErrorMin = pointingLawParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax; double omMax = pointingLawParameters->omMax;
double cInt = 2 * om * zeta; double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2); double kInt = 2 * pow(om, 2);
@ -110,7 +111,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate, void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) { int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) { if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
mgtDpDes[0] = 0; mgtDpDes[0] = 0;
mgtDpDes[1] = 0; mgtDpDes[1] = 0;
mgtDpDes[2] = 0; mgtDpDes[2] = 0;
@ -129,12 +130,12 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
// calculating momentum error // calculating momentum error
double deltaMomentum[3] = {0, 0, 0}; double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract( VectorOperations<double>::subtract(
momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3); momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3);
// resulting magnetic dipole command // resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0}; double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField); VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0; double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag; factor = (pointingLawParameters->deSatGainFactor) / normMag;
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
} }
@ -150,7 +151,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel, VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
wheelMomentum, 4); wheelMomentum, 4);
double gainNs = pointingModeControllerParameters->gainNullspace; double gainNs = pointingLawParameters->gainNullspace;
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace, MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
*nullSpaceMatrix, 4); *nullSpaceMatrix, 4);

2
mission/controller/acs/control/PtgCtrl.h
View File

@ -59,7 +59,7 @@ class PtgCtrl {
void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand); void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
private: private:
AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters; AcsParameters::PointingLawParameters *pointingLawParameters;
AcsParameters::RwHandlingParameters *rwHandlingParameters; AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE *inertiaEIVE; AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices; AcsParameters::RwMatrices *rwMatrices;