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

v7.5.0 #828

Merged
muellerr merged 96 commits from dev-7.5.0 into main 2023-12-06 17:44:23 +01:00
Conversation 0 Commits 96 Files Changed 47 +67 -30
7 changed files with 67 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit c155f399b1 - Show all commits

6
mission/acs/defs.h
View File

@ -23,9 +23,9 @@ enum AcsMode : Mode_t {
enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
enum ControlModeStrategy : uint8_t { enum ControlModeStrategy : uint8_t {
SAFECTRL_OFF = 0, CTRL_OFF = 0,
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, CTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, CTRL_NO_SENSORS_FOR_CONTROL = 2,
// OBSW version <= v6.1.0 // OBSW version <= v6.1.0
LEGACY_SAFECTRL_ACTIVE_MEKF = 10, LEGACY_SAFECTRL_ACTIVE_MEKF = 10,
LEGACY_SAFECTRL_WITHOUT_MEKF = 11, LEGACY_SAFECTRL_WITHOUT_MEKF = 11,

10
mission/controller/AcsController.cpp
View File

@ -287,10 +287,10 @@ void AcsController::performSafe() {
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0); safeCtrlFailure(1, 0);
break; break;
case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1); safeCtrlFailure(0, 1);
break; break;
default: default:
@ -349,10 +349,10 @@ void AcsController::performDetumble() {
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainBdot); magMomMtq, acsParameters.detumbleParameter.gainBdot);
break; break;
case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0); safeCtrlFailure(1, 0);
break; break;
case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1); safeCtrlFailure(0, 1);
break; break;
default: default:
@ -668,7 +668,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng; ctrlValData.errAng.value = errAng;
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
ctrlValData.safeStrat.value = acs::ControlModeStrategy::SAFECTRL_OFF; ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
ctrlValData.setValidity(true, true); ctrlValData.setValidity(true, true);
} }
} }

41
mission/controller/acs/FusedRotationEstimation.cpp
View File

@ -4,6 +4,31 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
acsParameters = acsParameters_; acsParameters = acsParameters_;
} }
void FusedRotationEstimation::estimateFusedRotationRate(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
acsctrl::FusedRotRateData *fusedRotRateData) {
if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) {
double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
if (VectorOperations<double>::norm(quatOld, 4) != 0) {
estimateFusedRotationRateStr(quatNew, fusedRotRateData);
} else {
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateData);
}
std::memcpy(quatOld, quatNew, sizeof(quatOld));
} else {
std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld));
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateData);
}
}
void FusedRotationEstimation::estimateFusedRotationRateStr(
double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData) {}
void FusedRotationEstimation::estimateFusedRotationRateSafe( void FusedRotationEstimation::estimateFusedRotationRateSafe(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
@ -13,9 +38,9 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
not mgmDataProcessed->mgmVecTotDerivative.isValid())) { not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true); fusedRotRateData->setValidity(false, true);
} }
// store for calculation of angular acceleration // store for calculation of angular acceleration
@ -92,9 +117,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse(
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true); fusedRotRateData->setValidity(false, true);
} }
return; return;
@ -106,9 +131,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse(
3); 3);
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false); fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false); fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true); fusedRotRateData->rotRateTotal.setValid(true);

14
mission/controller/acs/FusedRotationEstimation.h
View File

@ -2,24 +2,36 @@
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h> #include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h> #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class FusedRotationEstimation { class FusedRotationEstimation {
public: public:
FusedRotationEstimation(AcsParameters *acsParameters_); FusedRotationEstimation(AcsParameters *acsParameters_);
void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
ACS::SensorValues *sensorValues,
acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData); acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateStr(double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData);
protected: protected:
private: private:
static constexpr double ZERO_VEC[3] = {0, 0, 0}; static constexpr double ZERO_VEC3[3] = {0, 0, 0};
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
AcsParameters *acsParameters; AcsParameters *acsParameters;
double quatOld[3] = {0, 0, 0, 0};
double rotRateOldB[3] = {0, 0, 0}; double rotRateOldB[3] = {0, 0, 0};
void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,

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

@ -12,13 +12,13 @@ acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
const bool magFieldRateValid, const bool magFieldRateValid,
const bool useFullDetumbleLaw) { const bool useFullDetumbleLaw) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (satRotRateValid and useFullDetumbleLaw) { } else if (satRotRateValid and useFullDetumbleLaw) {
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL; return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
} else if (magFieldRateValid) { } else if (magFieldRateValid) {
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} }

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

@ -15,16 +15,16 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled) { const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) { } else if (mekfEnabled and mekfValid) {
return acs::ControlModeStrategy::SAFECTRL_MEKF; return acs::ControlModeStrategy::PTGCTRL_ACTIVE_MEKF;
} else if (sunDirValid) { } else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) { if (gyrEnabled and satRotRateValid) {
return acs::ControlModeStrategy::SAFECTRL_GYR; return acs::ControlModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateTotalValid) { } else if (not gyrEnabled and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_SUSMGM; return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} else if (not sunDirValid) { } else if (not sunDirValid) {
if (dampingEnabled) { if (dampingEnabled) {
@ -33,15 +33,15 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} else if (not dampingEnabled and satRotRateValid) { } else if (not dampingEnabled and satRotRateValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} }

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

@ -16,7 +16,7 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co
const uint8_t gyrEnabled, const uint8_t gyrEnabled,
const uint8_t dampingEnabled) { const uint8_t dampingEnabled) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) { } else if (mekfEnabled and mekfValid) {
return acs::ControlModeStrategy::SAFECTRL_MEKF; return acs::ControlModeStrategy::SAFECTRL_MEKF;
} else if (sunDirValid) { } else if (sunDirValid) {
@ -25,7 +25,7 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co
} else if (not gyrEnabled and fusedRateTotalValid) { } else if (not gyrEnabled and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_SUSMGM; return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} else if (not sunDirValid) { } else if (not sunDirValid) {
if (dampingEnabled) { if (dampingEnabled) {
@ -34,15 +34,15 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} else if (not dampingEnabled and satRotRateValid) { } else if (not dampingEnabled and satRotRateValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} else { } else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
} }
} }