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);
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) {
case SUBMODE_IDLE:
guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case SUBMODE_PTG_TARGET:
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now,
targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case SUBMODE_PTG_TARGET_GS:
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed,
now, targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.targetModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
break;
case SUBMODE_PTG_NADIR:
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.nadirModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
break;
case SUBMODE_PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate);
for ( uint8_t i = 0; i < 4; i++ ) {
quatRef[i] = acsParameters.inertialModeControllerParameters.quatRef[i];
}
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break;
}
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
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}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
@ -305,7 +331,7 @@ void AcsController::performControlOperation() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
if ( enableAntiStiction ) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = {
(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:
parameterWrapper->set(targetModeControllerParameters.desatOn);
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:
return INVALID_IDENTIFIER_ID;
}

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

@ -815,15 +815,7 @@ class AcsParameters : public HasParametersIF {
} safeModeControllerParameters;
// ToDo: mutiple structs for different pointing mode controllers?
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;
struct PointingLawParameters {
double zeta = 0.3;
double om = 0.3;
@ -836,14 +828,38 @@ class AcsParameters : public HasParametersIF {
uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
double omegaEarth = 0.000072921158553;
} pointingLawParameters;
double nadirRefDirection[3] = {-1, 0, 0}; // Camera
double tgtQuatInertial[4] = {0, 0, 0, 1};
double tgtRotRateInertial[3] = {0, 0, 0};
int8_t nadirTimeElapsedMax = 10;
} pointingModeControllerParameters, inertialModeControllerParameters,
nadirModeControllerParameters, targetModeControllerParameters;
struct TargetModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to
// give this as an input- currently en calculation is done
double quatRef[4] = {0, 0, 0, 1};
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 {
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};
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));

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
// 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]);
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
// reation wheel maybe can be done in "commanding.h"
@ -70,9 +70,9 @@ class Guidance {
private:
AcsParameters acsParameters;
bool strBlindAvoidFlag = false;
timeval timeSavedQuaternionNadir;
double savedQuaternionNadir[4] = {0, 0, 0, 0};
double omegaRefSavedNadir[3] = {0, 0, 0};
timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0};
};
#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() {}
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);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
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
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
double om = pointingLawParameters->om;
double zeta = pointingLawParameters->zeta;
double qErrorMin = pointingLawParameters->qiMin;
double omMax = pointingLawParameters->omMax;
double cInt = 2 * om * zeta;
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,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
mgtDpDes[0] = 0;
mgtDpDes[1] = 0;
mgtDpDes[2] = 0;
@ -129,12 +130,12 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(
momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3);
// resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
factor = (pointingLawParameters->deSatGainFactor) / normMag;
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>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
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}};
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
*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);
private:
AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
AcsParameters::PointingLawParameters *pointingLawParameters;
AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices;