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

fixed susTotVec and mgmTotVec types to dataSet types

Browse Source
This commit is contained in:
Marius Eggert 2022-11-17 15:55:30 +01:00
parent f3ac6d4a7e
commit 281eb3237c
1 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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