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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +19 -10
1 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 281eb3237c - Show all commits

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

@ -115,13 +115,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i]; sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
} }
} }
float mgmVecTot[3] = {0.0, 0.0, 0.0}; double mgmVecTot[3] = {0.0, 0.0, 0.0};
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
} }
//-----------------------Mgm Rate Computation --------------------------------------------------- //-----------------------Mgm Rate Computation ---------------------------------------------------
float mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false; bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
if (timeOfSavedMagFieldEst.tv_sec != 0) { if (timeOfSavedMagFieldEst.tv_sec != 0) {
@ -367,7 +367,7 @@ void SensorProcessing::processSus(
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2], sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
float susMeanValue[3] = {0, 0, 0}; double susMeanValue[3] = {0, 0, 0};
for (uint8_t i = 0; i < 12; i++) { for (uint8_t i = 0; i < 12; i++) {
if (validIds[i]) { if (validIds[i]) {
susMeanValue[0] += susVecBody[0][i]; susMeanValue[0] += susVecBody[0][i];
@ -375,12 +375,12 @@ void SensorProcessing::processSus(
susMeanValue[2] += susVecBody[2][i]; susMeanValue[2] += susVecBody[2][i];
} }
} }
float susVecTot[3] = {0.0, 0.0, 0.0}; double susVecTot[3] = {0.0, 0.0, 0.0};
VectorOperations<float>::normalize(susMeanValue, susVecTot, 3); VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
/* -------- Sun Derivatiative --------------------- */ /* -------- Sun Derivatiative --------------------- */
float susVecTotDerivative[3] = {0.0, 0.0, 0.0}; double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool susVecTotDerivativeValid = false; bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
if (timeOfSavedSusDirEst.tv_sec != 0) { if (timeOfSavedSusDirEst.tv_sec != 0) {
@ -438,10 +438,10 @@ void SensorProcessing::processSus(
susDataProcessed->sus10vec.setValid(sus10valid); susDataProcessed->sus10vec.setValid(sus10valid);
std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float)); std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
susDataProcessed->sus11vec.setValid(sus11valid); susDataProcessed->sus11vec.setValid(sus11valid);
std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(float)); std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
susDataProcessed->susVecTot.setValid(true); susDataProcessed->susVecTot.setValid(true);
std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative, std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative,
3 * sizeof(float)); 3 * sizeof(double));
susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid); susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid);
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double)); std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
susDataProcessed->sunIjkModel.setValid(true); susDataProcessed->sunIjkModel.setValid(true);
@ -538,9 +538,13 @@ void SensorProcessing::processGyr(
std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double));
gyrDataProcessed->gyr0vec.setValid(gyr0valid); gyrDataProcessed->gyr0vec.setValid(gyr0valid);
std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double));
gyrDataProcessed->gyr1vec.setValid(gyr1valid);
std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double));
gyrDataProcessed->gyr2vec.setValid(gyr2valid);
std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double));
gyrDataProcessed->gyr3vec.setValid(gyr3valid);
std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double)); std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double));
gyrDataProcessed->gyrVecTot.setValid(true);
gyrDataProcessed->setValidity(true, false); gyrDataProcessed->setValidity(true, false);
} }
} }
@ -582,7 +586,9 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
const AcsParameters *acsParameters) { const AcsParameters *acsParameters) {
sensorValues->update(); sensorValues->update();
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value, processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.isValid(), gpsDataProcessed); (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid()),
gpsDataProcessed);
processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value, processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
@ -594,7 +600,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.isValid(), mgmDataProcessed); sensorValues->gpsSet.altitude.value,
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid()),
mgmDataProcessed);
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),