diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f5b72252..813851dc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -762,6 +762,8 @@ class AcsParameters /*: public HasParametersIF*/ { pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; + enum PreferAdis { NO = 0, YES = 1 }; + uint8_t preferAdis = PreferAdis::YES; } gyrHandlingParameters; struct RwHandlingParameters { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index bb445485..483b94c2 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -480,80 +480,57 @@ void SensorProcessing::processGyr( // Transforming Values to the Body Frame (actually it is the geometry frame atm) double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0}, gyr3ValueBody[3] = {0, 0, 0}; + float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0}; if (gyr0valid) { const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, gyr0ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; + } } if (gyr1valid) { const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, gyr1ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; + } } if (gyr2valid) { const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, gyr2ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; + } } if (gyr3valid) { const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, gyr3ValueBody, 3, 3, 1); + for (uint8_t i = 0; i < 3; i++) { + sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; + sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; + } } /* -------- SatRateEst: Middle Value ------- */ // take ADIS measurements, if both avail // if just one ADIS measurement avail, perform sensor fusion double gyrVecTot[3] = {0.0, 0.0, 0.0}; - if (gyr0valid && gyr2valid) { + if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) { double gyr02ValuesSum[3]; VectorOperations::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3); VectorOperations::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3); - } else if ((gyr0valid || gyr2valid) && !(gyr1valid || gyr3valid)) { - if (gyr0valid) { - std::memcpy(gyrVecTot, gyr0ValueBody, 3 * sizeof(double)); - } else if (gyr2valid) { - std::memcpy(gyrVecTot, gyr2ValueBody, 3 * sizeof(double)); + } else { + for (uint8_t i = 0; i < 3; i++) { + gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } - } else if (gyr1valid && gyr3valid) { - double gyr13ValuesSum[3]; - double gyr13ValuesMean[3]; - VectorOperations::add(gyr1ValueBody, gyr3ValueBody, gyr13ValuesSum, 3); - VectorOperations::mulScalar(gyr13ValuesSum, .5, gyr13ValuesMean, 3); - if (gyr0valid) { - gyrVecTot[0] = - ((gyr0ValueBody[0] / gyrParameters->gyr02variance[0]) + - (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / - ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); - gyrVecTot[1] = - ((gyr0ValueBody[1] / gyrParameters->gyr02variance[1]) + - (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / - ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); - gyrVecTot[2] = - ((gyr0ValueBody[2] / gyrParameters->gyr02variance[2]) + - (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / - ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); - } else if (gyr2valid) { - gyrVecTot[0] = - ((gyr2ValueBody[0] / gyrParameters->gyr02variance[0]) + - (gyr13ValuesMean[0] / gyrParameters->gyr13variance[0])) / - ((1 / gyrParameters->gyr02variance[0]) + (1 / gyrParameters->gyr13variance[0])); - gyrVecTot[1] = - ((gyr2ValueBody[1] / gyrParameters->gyr02variance[1]) + - (gyr13ValuesMean[1] / gyrParameters->gyr13variance[1])) / - ((1 / gyrParameters->gyr02variance[1]) + (1 / gyrParameters->gyr13variance[1])); - gyrVecTot[2] = - ((gyr2ValueBody[2] / gyrParameters->gyr02variance[2]) + - (gyr13ValuesMean[2] / gyrParameters->gyr13variance[2])) / - ((1 / gyrParameters->gyr02variance[2]) + (1 / gyrParameters->gyr13variance[2])); - } else { - std::memcpy(gyrVecTot, gyr13ValuesMean, 3 * sizeof(double)); - } - } else if (gyr1valid) { - std::memcpy(gyrVecTot, gyr1ValueBody, 3 * sizeof(double)); - } else if (gyr3valid) { - std::memcpy(gyrVecTot, gyr3ValueBody, 3 * sizeof(double)); } { PoolReadGuard pg(gyrDataProcessed);