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

removed inertia matrix for safe mode

Browse Source
This commit is contained in:
Marius Eggert 2023-04-14 11:37:23 +02:00
parent 7f3f99c6aa
commit 53cccc3c13
5 changed files with 23 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
mission/controller/AcsController.cpp
View File

@ -165,8 +165,8 @@ void AcsController::performSafe() {
} }
// get desired satellite rate, sun direction to align to and inertia // get desired satellite rate, sun direction to align to and inertia
double sunTargetDir[3] = {0, 0, 0}, inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, inertiaEive); guidance.getTargetParamsSafe(sunTargetDir);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
@ -178,20 +178,19 @@ void AcsController::performSafe() {
case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
inertiaEive, magMomMtq, errAng); magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF): case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF):
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, inertiaEive, magMomMtq, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING):
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
sunTargetDir, inertiaEive, magMomMtq, errAng); sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;

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

@ -539,19 +539,15 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
} }
} }
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double inertiaEive[3][3]) { void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
std::error_code e; std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
3 * sizeof(double)); 3 * sizeof(double));
std::memcpy(inertiaEive, acsParameters->inertiaEIVE.inertiaMatrixDeployed,
3 * 3 * sizeof(double));
} else { } else {
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop, std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double)); 3 * sizeof(double));
std::memcpy(inertiaEive, acsParameters->inertiaEIVE.inertiaMatrixUndeployed,
3 * 3 * sizeof(double));
} }
} }

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

@ -12,7 +12,7 @@ class Guidance {
Guidance(AcsParameters *acsParameters_); Guidance(AcsParameters *acsParameters_);
virtual ~Guidance(); virtual ~Guidance();
void getTargetParamsSafe(double sunTargetSafe[3], double inertiaEive[3][3]); void getTargetParamsSafe(double sunTargetSafe[3]);
ReturnValue_t solarArrayDeploymentComplete(); ReturnValue_t solarArrayDeploymentComplete();
// Function to get the target quaternion and refence rotation rate from gps position and // Function to get the target quaternion and refence rotation rate from gps position and

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

@ -29,7 +29,7 @@ uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t
void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB,
const double inertiaMatrix[3][3], double *magMomB, double &errorAngle) { double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -42,9 +42,9 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque( calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf, inertiaMatrix); acsParameters->safeModeControllerParameters.k_orthoMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignMekf); acsParameters->safeModeControllerParameters.k_alignMekf);
@ -57,8 +57,8 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
} }
void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirB, const double *sunDirRefB, const double *sunDirB, const double *sunDirRefB, double *magMomB,
const double inertiaMatrix[3][3], double *magMomB, double &errorAngle) { double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
double magFieldBT[3] = {0, 0, 0}; double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -70,8 +70,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf);
inertiaMatrix);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignNonMekf); acsParameters->safeModeControllerParameters.k_alignNonMekf);
@ -84,8 +83,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
} }
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, const double inertiaMatrix[3][3], const double *sunDirRefB, double *magMomB, double &errorAngle) {
double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -95,8 +93,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
splitRotationalRate(satRotRateB, sunDirRefB); splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf);
inertiaMatrix);
// sum of all torques // sum of all torques
double cmdTorque[3] = {0, 0, 0}; double cmdTorque[3] = {0, 0, 0};
@ -116,15 +113,12 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, const double gainParallel, double &errorAngle, const double gainParallel,
const double gainOrtho, const double gainOrtho) {
const double inertiaMatrix[3][3]) {
// calculate torque for parallel rotational rate // calculate torque for parallel rotational rate
VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);
// calculate torque for orthogonal rotational rate // calculate torque for orthogonal rotational rate
double orthoFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; VectorOperations<double>::mulScalar(satRotRateOrthogonalB, -gainOrtho, cmdOrtho, 3);
MatrixOperations<double>::multiplyScalar(*inertiaMatrix, -gainOrtho, *orthoFactor, 3, 3);
MatrixOperations<double>::multiply(*orthoFactor, satRotRateOrthogonalB, cmdOrtho, 3, 3, 1);
} }
void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,

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

@ -17,21 +17,20 @@ class SafeCtrl {
const uint8_t mekfEnabled, const uint8_t dampingEnabled); const uint8_t mekfEnabled, const uint8_t dampingEnabled);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, const double inertiaMatrix[3][3], const double *quatBI, const double *sunDirRefB, double *magMomB,
double *magMomB, double &errorAngle);
void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, const double inertiaMatrix[3][3], double *magMomB,
double &errorAngle); double &errorAngle);
void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle);
void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB,
const double inertiaMatrix[3][3], double *magMomB, double &errorAngle); double *magMomB, double &errorAngle);
void splitRotationalRate(const double *satRotRateB, const double *sunDirB); void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
double &errorAngle, const double gainParallel, double &errorAngle, const double gainParallel,
const double gainOrtho, const double inertiaMatrix[3][3]); const double gainOrtho);
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
const double gainAlign); const double gainAlign);