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

frmt
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-22 08:59:29 +01:00
parent f6a0954315
commit 56642a11f7
1 changed files with 23 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -72,50 +72,59 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
} }
// Will be the version in worst case scenario in event of no working MEKF // Will be the version in worst case scenario in event of no working MEKF
ReturnValue_t SafeCtrl::safeNoMekf(const double *magneticFieldVector, ReturnValue_t SafeCtrl::safeNoMekf(const double *magFieldB,
const double *magneticFieldVectorDerivative, const double *magneticFieldVectorDerivative,
const double *sunVector, const double *sunvectorDerivative, const double *sunVector, const double *sunvectorDerivative,
double omegaRef, double *torqueCommand, double *spinAxis) { double omegaRef, double *magMomB, double *spinAxis) {
if (0) { if (0) {
return returnvalue::FAILED; return returnvalue::FAILED;
} }
double magneticFieldVectorT[3] = {0, 0, 0}; double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magneticFieldVector, 1e-6, magneticFieldVectorT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
double commandParallel[3] = {0, 0, 0}, commandAlign[3] = {0, 0, 0}, commandOrtho[3] = {0, 0, 0}; double cmdParallel[3] = {0, 0, 0}, cmdAlign[3] = {0, 0, 0}, cmdOrtho[3] = {0, 0, 0},
torqueCmd[3] = {0, 0, 0};
bool valid; bool valid;
double omega = estimateRotationAroundSun(magneticFieldVector, magneticFieldVectorDerivative, double omega =
sunVector, &valid); estimateRotationAroundSun(magFieldB, magneticFieldVectorDerivative, sunVector, &valid);
if (valid) { if (valid) {
VectorOperations<double>::mulScalar( VectorOperations<double>::mulScalar(
sunVector, sunVector,
acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega), acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega),
commandParallel, 3); cmdParallel, 3);
omega = omega * sign<double>(omega); omega = omega * sign<double>(omega);
} }
VectorOperations<double>::cross(spinAxis, sunVector, commandAlign); VectorOperations<double>::cross(spinAxis, sunVector, cmdAlign);
VectorOperations<double>::mulScalar( VectorOperations<double>::mulScalar(
commandAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, commandAlign, 3); cmdAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, cmdAlign, 3);
VectorOperations<double>::cross(sunvectorDerivative, sunVector, commandOrtho); VectorOperations<double>::cross(sunvectorDerivative, sunVector, cmdOrtho);
VectorOperations<double>::mulScalar( VectorOperations<double>::mulScalar(
commandOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, commandOrtho, 3); cmdOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, cmdOrtho, 3);
// only spin up when the angle to the sun is less than a certain angle. // only spin up when the angle to the sun is less than a certain angle.
// note that we check the cosin, thus "<" // note that we check the cosin, thus "<"
if (VectorOperations<double>::dot(spinAxis, sunVector) < if (VectorOperations<double>::dot(spinAxis, sunVector) <
acsParameters->safeModeControllerParameters.cosineStartSpin) { acsParameters->safeModeControllerParameters.cosineStartSpin) {
VectorOperations<double>::mulScalar(commandParallel, 0, commandParallel, 3); VectorOperations<double>::mulScalar(cmdParallel, 0, cmdParallel, 3);
} }
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
torqueCommand[i] = commandAlign[i] + commandOrtho[i] + commandParallel[i]; torqueCmd[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
} }
// MagMom B (orthogonal torque)
double torqueMgt[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldBT, torqueCmd, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), magMomB, 3);
*outputAngle = angleAlignErr;
return returnvalue::OK; return returnvalue::OK;
} }