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

.
Some checks failed
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-14 13:22:35 +01:00
parent 60a348a08f
commit c155f399b1
7 changed files with 67 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
mission/acs/defs.h
View File

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

10
mission/controller/AcsController.cpp
View File

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

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

@ -4,6 +4,31 @@ FusedRotationEstimation::FusedRotationEstimation(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(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
@ -13,9 +38,9 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
// store for calculation of angular acceleration
@ -92,9 +117,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse(
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
return;
@ -106,9 +131,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse(
3);
{
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);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);

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

@ -2,24 +2,36 @@
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class FusedRotationEstimation {
public:
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,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData);
void estimateFusedRotationRateStr(double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData);
protected:
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;
double quatOld[3] = {0, 0, 0, 0};
double rotRateOldB[3] = {0, 0, 0};
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 useFullDetumbleLaw) {
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) {
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
} else if (magFieldRateValid) {
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
} 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 uint8_t gyrEnabled, const uint8_t dampingEnabled) {
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) {
return acs::ControlModeStrategy::SAFECTRL_MEKF;
return acs::ControlModeStrategy::PTGCTRL_ACTIVE_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;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not sunDirValid) {
if (dampingEnabled) {
@ -33,15 +33,15 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_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;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} 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 dampingEnabled) {
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) {
return acs::ControlModeStrategy::SAFECTRL_MEKF;
} else if (sunDirValid) {
@ -25,7 +25,7 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co
} else if (not gyrEnabled and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else if (not sunDirValid) {
if (dampingEnabled) {
@ -34,15 +34,15 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co
} else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
} else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_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;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
} else {
return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
}
}