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

nearly there
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-03-24 11:35:46 +01:00
parent e6813efb88
commit 9d8dfdfd4f
3 changed files with 70 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
mission/controller/AcsController.cpp
View File

@ -174,7 +174,8 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq);
} else {
result = safeCtrl.safeNoMekf(
now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),

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

@ -13,6 +13,21 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
const bool satRotRateValid, const bool sunDirValid) {
if (not magFieldValid) {
return SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfValid) {
return SAFECTRL_USE_MEKF;
} else if (satRotRateValid and sunDirValid) {
return SAFECTRL_USE_NONMEKF;
} else if (satRotRateValid and not sunDirValid) {
return SAFECTRL_USE_DAMPING;
} else {
return SAFECTRL_NO_SENSORS_FOR_CONTROL;
}
}
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
@ -71,81 +86,56 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
return returnvalue::OK;
}
// Will be the version in worst case scenario in event of no working MEKF
ReturnValue_t SafeCtrl::safeNoMekf(const double *magFieldB,
const double *magneticFieldVectorDerivative,
const double *sunVector, const double *sunvectorDerivative,
double omegaRef, double *magMomB, double *spinAxis) {
if (0) {
return returnvalue::FAILED;
}
void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
double &errorAngle) {
// convert magFieldB from uT to T
double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
double cmdParallel[3] = {0, 0, 0}, cmdAlign[3] = {0, 0, 0}, cmdOrtho[3] = {0, 0, 0},
torqueCmd[3] = {0, 0, 0};
// calculate angle alpha between sunDirRef and sunDir
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
errorAngle = acos(dotSun);
bool valid;
double omega =
estimateRotationAroundSun(magFieldB, magneticFieldVectorDerivative, sunVector, &valid);
// split rotational rate into parallel and orthogonal parts
double satRotRateParallelB[3] = {0, 0, 0}, satRotRateOrthogonalB[3] = {0, 0, 0};
double parallelLength = VectorOperations<double>::dot(satRotRateB, sunDirB) *
pow(VectorOperations<double>::norm(sunDirB, 3), -2);
VectorOperations<double>::mulScalar(sunDirB, parallelLength, satRotRateParallelB, 3);
VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
if (valid) {
// calculate torque for parallel rotational rate
double cmdParallel[3] = {0, 0, 0};
if (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin) {
VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
VectorOperations<double>::mulScalar(
sunVector,
acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega),
cmdParallel, 3);
omega = omega * sign<double>(omega);
cmdParallel, acsParameters->safeModeControllerParameters.k_parallel_mekf, cmdParallel, 3);
}
VectorOperations<double>::cross(spinAxis, sunVector, cmdAlign);
// calculate torque for orthogonal rotational rate
double cmdOrtho[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
-acsParameters->safeModeControllerParameters.k_ortho_mekf,
cmdOrtho, 3);
VectorOperations<double>::mulScalar(
cmdAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, cmdAlign, 3);
VectorOperations<double>::cross(sunvectorDerivative, sunVector, cmdOrtho);
VectorOperations<double>::mulScalar(
cmdOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, cmdOrtho, 3);
// only spin up when the angle to the sun is less than a certain angle.
// note that we check the cosin, thus "<"
if (VectorOperations<double>::dot(spinAxis, sunVector) <
acsParameters->safeModeControllerParameters.cosineStartSpin) {
VectorOperations<double>::mulScalar(cmdParallel, 0, cmdParallel, 3);
}
// calculate torque for alignment
double cmdAlign[3] = {0, 0, 0}, crossAlign[3] = {0, 0, 0},
alignFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiplyScalar(*acsParameters->inertiaEIVE.inertiaMatrix,
acsParameters->safeModeControllerParameters.k_align_mekf,
*alignFactor, 3, 3);
VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
MatrixOperations<double>::multiply(*alignFactor, *crossAlign, *cmdAlign, 3, 3, 1);
// sum of all torques
double cmdTorque[3] = {0, 0, 0};
for (uint8_t i = 0; i < 3; i++) {
torqueCmd[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
}
// MagMom B (orthogonal torque)
double torqueMgt[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldBT, torqueCmd, torqueMgt);
VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), magMomB, 3);
*outputAngle = angleAlignErr;
return returnvalue::OK;
}
double SafeCtrl::estimateRotationAroundSun(const double *magneticFieldVector,
const double *magneticFieldVectorDerivative,
const double *sunVector, bool *updated) {
*updated = true;
double vector[3];
VectorOperations<double>::cross(magneticFieldVector, sunVector, vector);
float magLength = VectorOperations<double>::norm(magneticFieldVector, 3);
float length = VectorOperations<double>::norm(vector, 3);
// only check if angle between B and sun is large enough
if (length > (acsParameters->safeModeControllerParameters.sineCalculateOmegaSun * magLength)) {
float omega = VectorOperations<double>::dot(magneticFieldVectorDerivative, vector);
omega = omega / (length * length);
lastCalculatedOmega = omega;
return omega;
} else {
*updated = false;
return lastCalculatedOmega;
}
VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
}

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

@ -1,21 +1,29 @@
#ifndef SAFECTRL_H_
#define SAFECTRL_H_
#include <eive/resultClassIds.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "eive/resultClassIds.h"
class SafeCtrl {
public:
SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl();
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
static constexpr uint8_t IF_SAFE_ID = CLASS_ID::ACS_SAFE;
static constexpr ReturnValue_t SAFECTRL_NO_MAG_FIELD_FOR_CONTROL =
returnvalue::makeCode(IF_SAFE_ID, 2);
static constexpr ReturnValue_t SAFECTRL_USE_MEKF = returnvalue::makeCode(IF_SAFE_ID, 3);
static constexpr ReturnValue_t SAFECTRL_USE_NONMEKF = returnvalue::makeCode(IF_SAFE_ID, 4);
static constexpr ReturnValue_t SAFECTRL_USE_DAMPING = returnvalue::makeCode(IF_SAFE_ID, 5);
static constexpr ReturnValue_t SAFECTRL_NO_SENSORS_FOR_CONTROL =
returnvalue::makeCode(IF_SAFE_ID, 6);
ReturnValue_t safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
const bool satRotRateValid, const bool sunDirValid);
ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
@ -23,24 +31,13 @@ class SafeCtrl {
double *satRatRef, // From Guidance (!)
double *outputAngle, double *outputMagMomB);
ReturnValue_t safeNoMekf(const double *magneticFieldVector,
const double *magneticFieldVectorDerivative, const double *sunVector,
const double *sunvectorDerivative, double omegaRef,
double *torqueCommand, double *spinAxis);
double estimateRotationAroundSun(const double *magneticFieldVector,
const double *magneticFieldVectorDerivative,
const double *sunvector, bool *updated);
void safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
double &errorAngle);
protected:
private:
AcsParameters *acsParameters;
double gainMatrixInertia[3][3];
double magFieldBState[3];
timeval magFieldBStateTime;
float lastCalculatedOmega = 0.0;
};
#endif /* ACS_CONTROL_SAFECTRL_H_ */