eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 3 Releases 117 Wiki Activity

applyed CppStyle Format
All checks were successful
EIVE/eive-obsw/pipeline/pr-eggert/acs This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt 2023-01-23 16:34:52 +01:00
parent 44b384cd17
commit adef468c0b
6 changed files with 714 additions and 683 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1270
mission/controller/AcsController.cpp
View File

File diff suppressed because it is too large Load Diff

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

@ -816,49 +816,48 @@ class AcsParameters : public HasParametersIF {
} safeModeControllerParameters;
struct PointingLawParameters {
double zeta = 0.3;
double om = 0.3;
double omMax = 1 * M_PI / 180;
double qiMin = 0.1;
double gainNullspace = 0.01;
double zeta = 0.3;
double om = 0.3;
double omMax = 1 * M_PI / 180;
double qiMin = 0.1;
double gainNullspace = 0.01;
double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000;
uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000;
uint8_t desatOn = true;
uint8_t enableAntiStiction = true;
} pointingLawParameters;
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
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]
// 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;
// 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
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};
double tgtQuat[4] = {0, 0, 0, 1};
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
} inertialModeControllerParameters;
struct StrParameters {

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

@ -42,7 +42,8 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
// Position of the satellite in the earth/fixed frame via GPS
@ -172,14 +173,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
}
}
void Guidance::refRotationRate(int8_t timeElapsedMax, 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) -
(timeSavedQuaternion.tv_sec +
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
(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, savedQuaternion, qDiff, 4);
@ -226,7 +227,8 @@ void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
double targetCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
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};
@ -307,7 +309,8 @@ void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfDat
double groundStationCart[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt, acsParameters.targetModeControllerParameters.longitudeTgt,
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};
@ -588,13 +591,15 @@ void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
}
void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, 4 * sizeof(double));
std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate, 3 * sizeof(double));
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
4 * sizeof(double));
std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
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], acsctrl::MekfData *mekfData, 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);

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

@ -58,10 +58,12 @@ class Guidance {
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and
// desired
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4], double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[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(int8_t timeElapsedMax, 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"

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

@ -26,8 +26,9 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws) {
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *qError, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
@ -106,7 +107,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameter
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
}
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst, bool magFieldEstValid, double *satRate,
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
@ -127,8 +129,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLaw
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(
momentumTotal, pointingLawParameters->desatMomentumRef, deltaMomentum, 3);
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
deltaMomentum, 3);
// resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
@ -137,7 +139,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters * pointingLaw
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
}
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1,
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0};

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

@ -41,14 +41,16 @@ class PtgCtrl {
/* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void ptgLaw(AcsParameters::PointingLawParameters * pointingLawParameters, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws);
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(AcsParameters::PointingLawParameters * pointingLawParameters, double *magFieldEst,
bool magFieldEstValid, double *satRate, int32_t *speedRw0, int32_t *speedRw1,
int32_t *speedRw2, int32_t *speedRw3, double *mgtDpDes);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes);
void ptgNullspace(AcsParameters::PointingLawParameters * pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low