this gonna be fun
This commit is contained in:
parent
65f907448c
commit
60a348a08f
@ -22,7 +22,7 @@ enum AcsMode : Mode_t {
|
||||
|
||||
enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
|
||||
|
||||
enum SafeModeStrategy : uint8_t {
|
||||
enum ControlModeStrategy : uint8_t {
|
||||
SAFECTRL_OFF = 0,
|
||||
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
|
||||
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
|
||||
@ -40,6 +40,9 @@ enum SafeModeStrategy : uint8_t {
|
||||
SAFECTRL_ECLIPSE_IDELING = 19,
|
||||
SAFECTRL_DETUMBLE_FULL = 20,
|
||||
SAFECTRL_DETUMBLE_DETERIORATED = 21,
|
||||
// Added in vNext
|
||||
PTGCTRL_ACTIVE_MEKF = 100,
|
||||
PTGCTRL_WITHOUT_MEKF = 101,
|
||||
};
|
||||
|
||||
enum GpsSource : uint8_t {
|
||||
|
@ -240,27 +240,27 @@ void AcsController::performSafe() {
|
||||
guidance.getTargetParamsSafe(sunTargetDir);
|
||||
|
||||
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
||||
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
|
||||
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
|
||||
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
|
||||
acsParameters.safeModeControllerParameters.useGyr,
|
||||
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
|
||||
switch (safeCtrlStrat) {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_MEKF):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_MEKF):
|
||||
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
|
||||
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
|
||||
magMomMtq, errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_GYR):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_GYR):
|
||||
safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
|
||||
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
|
||||
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
|
||||
fusedRotRateData.rotRateParallel.value,
|
||||
fusedRotRateData.rotRateOrthogonal.value,
|
||||
@ -268,29 +268,29 @@ void AcsController::performSafe() {
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
|
||||
safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
|
||||
gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
|
||||
errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
|
||||
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
|
||||
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
|
||||
errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING):
|
||||
errAng = NAN;
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
safeCtrlFailure(1, 0);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
@ -335,24 +335,24 @@ void AcsController::performSafe() {
|
||||
}
|
||||
|
||||
void AcsController::performDetumble() {
|
||||
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
||||
acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
||||
acsParameters.detumbleParameter.useFullDetumbleLaw);
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
switch (safeCtrlStrat) {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL):
|
||||
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
|
||||
magMomMtq, acsParameters.detumbleParameter.gainFull);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
|
||||
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
|
||||
magMomMtq, acsParameters.detumbleParameter.gainBdot);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
safeCtrlFailure(1, 0);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
case (acs::ControlModeStrategy::SAFECTRL_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::SafeModeStrategy::SAFECTRL_OFF;
|
||||
ctrlValData.safeStrat.value = acs::ControlModeStrategy::SAFECTRL_OFF;
|
||||
ctrlValData.setValidity(true, true);
|
||||
}
|
||||
}
|
||||
|
@ -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 {
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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) {
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
|
Loading…
x
Reference in New Issue
Block a user