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

for now final version of controller

Browse Source
This commit is contained in:
Marius Eggert 2023-03-24 14:51:33 +01:00
parent 44d0f1c533
commit ad72301ea0
2 changed files with 87 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -28,67 +28,66 @@ ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnV
}
}
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
bool rateMekfValid, double *sunDirRef, double *satRatRef,
double *outputAngle, double *outputMagMomB) {
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
return SAFECTRL_MEKF_INPUT_INVALID;
void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
const double *sunDirModelI, const double *quatBI, 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);
// convert sunDirModel to body rf
double sunDirB[3] = {0, 0, 0};
QuaternionOperations::multiplyVector(quatBI, sunDirModelI, sunDirB);
// calculate angle alpha between sunDirRef and sunDir
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
errorAngle = acos(dotSun);
// 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);
// 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(
cmdParallel, acsParameters->safeModeControllerParameters.k_parallelMekf, cmdParallel, 3);
}
double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
// calculate torque for orthogonal rotational rate
double cmdOrtho[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
-acsParameters->safeModeControllerParameters.k_orthoMekf,
cmdOrtho, 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_alignMekf,
*alignFactor, 3, 3);
VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);
// Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
// sum of all torques
double cmdTorque[3] = {0, 0, 0};
for (uint8_t i = 0; i < 3; i++) {
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
}
// change unit from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
double crossSun[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
// calc angle alpha between sunDirRef and sunDIr
double dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
double alpha = acos(dotSun);
// Law Torque calculations
double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
torqueAll[3] = {0, 0, 0};
double scalarFac = kAlign * alpha / normCrossSun;
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
double rateSafeMode[3] = {0, 0, 0};
VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
// Adding factor of inertia for axes
MatrixOperations<double>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
*gainMatrixInertia, 3,
3); // why only for mekf one and not for no mekf
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
// MagMom B (orthogonal torque)
// calculate magnetic moment to command
double torqueMgt[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
*outputAngle = alpha;
return returnvalue::OK;
VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
}
void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
double &errorAngle) {
void SafeCtrl::safeNonMekf(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);
@ -109,23 +108,23 @@ void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, co
if (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin) {
VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
VectorOperations<double>::mulScalar(
cmdParallel, acsParameters->safeModeControllerParameters.k_parallel_mekf, cmdParallel, 3);
cmdParallel, acsParameters->safeModeControllerParameters.k_parallelMekf, cmdParallel, 3);
}
// calculate torque for orthogonal rotational rate
double cmdOrtho[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
-acsParameters->safeModeControllerParameters.k_ortho_mekf,
-acsParameters->safeModeControllerParameters.k_orthoMekf,
cmdOrtho, 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,
acsParameters->safeModeControllerParameters.k_alignMekf,
*alignFactor, 3, 3);
VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
MatrixOperations<double>::multiply(*alignFactor, *crossAlign, *cmdAlign, 3, 3, 1);
MatrixOperations<double>::multiply(*alignFactor, crossAlign, cmdAlign, 3, 3, 1);
// sum of all torques
double cmdTorque[3] = {0, 0, 0};
@ -133,9 +132,30 @@ void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, co
cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
}
// MagMom B (orthogonal torque)
// calculate magnetic moment to command
double torqueMgt[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
}
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
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);
// calculate torque for rate damping
double cmdTorque[3] = {0, 0, 0}, diffSatRotRate[3] = {0, 0, 0};
VectorOperations<double>::subtract(satRotRateRefB, satRotRateB, diffSatRotRate, 3);
VectorOperations<double>::mulScalar(
satRotRateB, acsParameters->safeModeControllerParameters.k_rateDamping, cmdTorque, 3);
// calculate magnetic moment to command
double torqueMgt[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
errorAngle = NAN;
}

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

@ -25,15 +25,16 @@ class SafeCtrl {
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,
double *satRateMekf, bool rateMekfValid, double *sunDirRef,
double *satRatRef, // From Guidance (!)
double *outputAngle, double *outputMagMomB);
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, const double *satRotRateRefB,
double *magMomB, double &errorAngle);
void safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
double &errorAngle);
void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
double &errorAngle);
void safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *satRotRateRefB, double *magMomB, double &errorAngle);
protected:
private: