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

Revert ACS Safe Mode Controller to FLP Design #466

Merged
muellerr merged 64 commits from acs-flp-safe into develop 2023-04-14 20:26:20 +02:00
Conversation 1 Commits 64 Files Changed 31 +5 -4
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6934721a42 - Show all commits

9
mission/controller/acs/SensorProcessing.cpp
View File

@ -141,7 +141,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
if (timeOfSavedMagFieldEst.tv_sec != 0) {
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0) {
for (uint8_t i = 0; i < 3; i++) {
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
savedMgmVecTot[i] = mgmVecTot[i];
@ -394,7 +394,7 @@ void SensorProcessing::processSus(
double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
if (timeOfSavedSusDirEst.tv_sec != 0) {
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0) {
for (uint8_t i = 0; i < 3; i++) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
savedSusVecTot[i] = susVecTot[i];
@ -581,8 +581,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
// Calculation of the satellite velocity in earth fixed frame
double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE &&
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
if (validSavedPosSatE and
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
(gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);