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

this gonna be fun
Some checks failed
EIVE/eive-obsw/pipeline/head Build queued...
Details
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-11-02 16:59:09 +01:00
parent 65f907448c
commit 60a348a08f
9 changed files with 90 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -495,24 +495,24 @@ void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double time
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
if (rw1valid && rw2valid && rw3valid && rw4valid) {
if (rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
return returnvalue::OK;
} else {

10
mission/controller/acs/control/Detumble.cpp
View File

@ -7,18 +7,18 @@ Detumble::Detumble() {}
Detumble::~Detumble() {}
acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (satRotRateValid and useFullDetumbleLaw) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
} else if (magFieldRateValid) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
}

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

@ -11,7 +11,7 @@ class Detumble {
Detumble();
virtual ~Detumble();
acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
acs::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw);

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

@ -10,6 +10,41 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
PtgCtrl::~PtgCtrl() {}
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
if (not magFieldValid) {
return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
return acs::ControlModeStrategy::SAFECTRL_MEKF;
} else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) {
return acs::ControlModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not sunDirValid) {
if (dampingEnabled) {
if (gyrEnabled and satRotRateValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not dampingEnabled and satRotRateValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {

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

@ -2,6 +2,7 @@
#define PTGCTRL_H_
#include <math.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <stdio.h>
@ -9,7 +10,7 @@
class PtgCtrl {
/*
* @brief: This class handles the pointing control mechanism. Calculation of an commanded
* torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
* torque for the reaction wheels, and magnetic Field strength for magnetorquer for desaturation
*
* @note: A description of the used algorithms can be found in
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
@ -21,6 +22,11 @@ class PtgCtrl {
PtgCtrl(AcsParameters *acsParameters_);
virtual ~PtgCtrl();
acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled);
/* @brief: Calculates the needed torque for the pointing control mechanism
*/
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
@ -36,7 +42,7 @@ class PtgCtrl {
const int32_t speedRw3, double *mgtDpDes);
/* @brief: Commands the stiction torque in case wheel speed is to low
* torqueCommand modified torque after antistiction
* torqueCommand modified torque after anti-stiction
*/
void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);

24
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -9,40 +9,40 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid,
const uint8_t mekfEnabled,
const uint8_t gyrEnabled,
const uint8_t dampingEnabled) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
return acs::SafeModeStrategy::SAFECTRL_MEKF;
return acs::ControlModeStrategy::SAFECTRL_MEKF;
} else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_GYR;
return acs::ControlModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not sunDirValid) {
if (dampingEnabled) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not dampingEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
}

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

@ -12,7 +12,7 @@ class SafeCtrl {
SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl();
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
acs::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled);