diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 749ec373..3c6a3564 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -353,62 +353,79 @@ void SensorProcessing::processSus( *sunVectorInertialValid = true; } -void SensorProcessing::processRmu(const double rmu0Value[], bool rmu0valid, - const double rmu1Value[], bool rmu1valid, - const double rmu2Value[], bool rmu2valid, - timeval timeOfrmuMeasurement, - const AcsParameters::RmuHandlingParameters *rmuParameters, - double *satRatEst, bool *satRateEstValid) { - if (!rmu0valid && !rmu1valid && !rmu2valid) { +void SensorProcessing::processGyr( + const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, bool gyr0axYvalid, + const double gyr0axZvalue, bool gyr0axZvalid, const double gyr1axXvalue, bool gyr1axXvalid, + const double gyr1axYvalue, bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid, + const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid, + const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid, + const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, + timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters, + double *satRatEst, bool *satRateEstValid) { + if (!gyr0axXvalid && !gyr0axYvalid && !gyr0axZvalid && !gyr1axXvalid && !gyr1axYvalid && + !gyr1axZvalid && !gyr2axXvalid && !gyr2axYvalid && !gyr2axZvalid && !gyr3axXvalid && + !gyr3axYvalid && !gyr3axZvalid) { *satRateEstValid = false; return; } // Transforming Values to the Body Frame (actually it is the geometry frame atm) - double rmu0ValueBody[3] = {0, 0, 0}, rmu1ValueBody[3] = {0, 0, 0}, rmu2ValueBody[3] = {0, 0, 0}; + double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0}, + gyr3ValueBody[3] = {0, 0, 0}; - bool validUnit[3] = {false, false, false}; + bool validUnit[4] = {false, false, false, false}; uint8_t validCount = 0; - if (rmu0valid) { - MatrixOperations::multiply(rmuParameters->rmu0orientationMatrix[0], rmu0Value, - rmu0ValueBody, 3, 3, 1); + if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) { + const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, + gyr0ValueBody, 3, 3, 1); validCount += 1; validUnit[0] = true; } - if (rmu1valid) { - MatrixOperations::multiply(rmuParameters->rmu1orientationMatrix[0], rmu1Value, - rmu1ValueBody, 3, 3, 1); + if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) { + const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, + gyr1ValueBody, 3, 3, 1); validCount += 1; validUnit[1] = true; } - if (rmu2valid) { - MatrixOperations::multiply(rmuParameters->rmu2orientationMatrix[0], rmu2Value, - rmu2ValueBody, 3, 3, 1); + if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) { + const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, + gyr2ValueBody, 3, 3, 1); validCount += 1; validUnit[2] = true; } + if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) { + const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; + MatrixOperations::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, + gyr3ValueBody, 3, 3, 1); + validCount += 1; + validUnit[3] = true; + } /* -------- SatRateEst: Middle Value ------- */ - double rmuValues[3][3] = {{rmu0ValueBody[0], rmu1ValueBody[0], rmu2ValueBody[0]}, - {rmu0ValueBody[1], rmu1ValueBody[1], rmu2ValueBody[1]}, - {rmu0ValueBody[2], rmu1ValueBody[2], rmu2ValueBody[2]}}; - double rmuValidValues[3][validCount]; + double gyrValues[3][4] = { + {gyr0ValueBody[0], gyr1ValueBody[0], gyr2ValueBody[0], gyr3ValueBody[0]}, + {gyr0ValueBody[1], gyr1ValueBody[1], gyr2ValueBody[1], gyr3ValueBody[1]}, + {gyr0ValueBody[2], gyr1ValueBody[2], gyr2ValueBody[2], gyr3ValueBody[2]}}; + double gyrValidValues[3][validCount]; uint8_t j = 0; for (uint8_t i = 0; i < validCount; i++) { if (validUnit[i]) { - rmuValidValues[0][j] = rmuValues[0][i]; - rmuValidValues[1][j] = rmuValues[1][i]; - rmuValidValues[2][j] = rmuValues[2][i]; + gyrValidValues[0][j] = gyrValues[0][i]; + gyrValidValues[1][j] = gyrValues[1][i]; + gyrValidValues[2][j] = gyrValues[2][i]; j += 1; } } // Selection Sort - double rmuValidValuesSort[3][validCount]; - MathOperations::selectionSort(*rmuValidValues, *rmuValidValuesSort, 3, validCount); + double gyrValidValuesSort[3][validCount]; + MathOperations::selectionSort(*gyrValidValues, *gyrValidValuesSort, 3, validCount); uint8_t n = ceil(validCount / 2); - satRatEst[0] = rmuValidValuesSort[0][n]; - satRatEst[1] = rmuValidValuesSort[1][n]; - satRatEst[2] = rmuValidValuesSort[2][n]; + satRatEst[0] = gyrValidValuesSort[0][n]; + satRatEst[1] = gyrValidValuesSort[1][n]; + satRatEst[2] = gyrValidValuesSort[2][n]; *satRateEstValid = true; } @@ -467,8 +484,19 @@ void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now, &outputValues->sunVectorDerivativeValid); // VALID outputs ? - // processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, - // sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now, - // &acsParameters->rmuHandlingParameters, outputValues->satRateEst, - // &outputValues->satRateEstValid); + processGyr( + sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(), + sensorValues->gyr0AdisSet.angVelocY.value, sensorValues->gyr0AdisSet.angVelocY.isValid(), + sensorValues->gyr0AdisSet.angVelocZ.value, sensorValues->gyr0AdisSet.angVelocZ.isValid(), + sensorValues->gyr1L3gSet.angVelocX.value, sensorValues->gyr1L3gSet.angVelocX.isValid(), + sensorValues->gyr1L3gSet.angVelocY.value, sensorValues->gyr1L3gSet.angVelocY.isValid(), + sensorValues->gyr1L3gSet.angVelocZ.value, sensorValues->gyr1L3gSet.angVelocZ.isValid(), + sensorValues->gyr2AdisSet.angVelocX.value, sensorValues->gyr2AdisSet.angVelocX.isValid(), + sensorValues->gyr2AdisSet.angVelocY.value, sensorValues->gyr2AdisSet.angVelocY.isValid(), + sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(), + sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(), + sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(), + sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, + &acsParameters->gyrHandlingParameters, outputValues->satRateEst, + &outputValues->satRateEstValid); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index df2b2a37..609c6e6a 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -52,10 +52,16 @@ class SensorProcessing { bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, double *sunVectorDerivative, bool *sunVectorDerivativeValid); - void processRmu(const double rmu0Value[], bool rmu0valid, // processRmu - const double rmu1Value[], bool rmu1valid, const double rmu2Value[], - bool rmu2valid, timeval timeOfrmuMeasurement, - const AcsParameters::RmuHandlingParameters *rmuParameters, double *satRatEst, + void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue, + bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid, + const double gyr1axXvalue, bool gyr1axXvalid, const double gyr1axYvalue, + bool gyr1axYvalid, const double gyr1axZvalue, bool gyr1axZvalid, + const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, + bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid, + const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue, + bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid, + timeval timeOfGyrMeasurement, + const AcsParameters::GyrHandlingParameters *gyrParameters, double *satRatEst, bool *satRateEstValid); void processStr(); diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 9ca2b90b..3a47937a 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -42,20 +42,25 @@ ReturnValue_t SensorValues::updateSus() { return result; } +ReturnValue_t SensorValues::updateGyr() { + ReturnValue_t result; + PoolReadGuard pgGyr0(&gyr0AdisSet), pgGyr1(&gyr1L3gSet), pgGyr2(&gyr2AdisSet), + pgGyr3(&gyr3L3gSet); + + result = (pgGyr0.getReadResult() || pgGyr1.getReadResult() || pgGyr2.getReadResult() || + pgGyr3.getReadResult()); + return result; +} + ReturnValue_t SensorValues::update() { - updateSus(); - updateMgm(); - - // lp_var_t quaternion(objects::STAR_TRACKER, PoolIds::CALI_QW, nullptr, - // pool_rwm_t::VAR_READ); - // ReturnValue_t result = quaternion.read(); - // - // if (result != RETURN_OK) { - // return RETURN_FAILED; - // } - // quatJB[3] = static_cast(quaternion.value); - // quatJBValid = quaternion.isValid(); - + ReturnValue_t mgmUpdate = updateMgm(); + ReturnValue_t susUpdate = updateSus(); + ReturnValue_t gyrUpdate = updateGyr(); + if ((mgmUpdate && susUpdate && gyrUpdate) == returnvalue::FAILED) { + return returnvalue::FAILED; + }; return returnvalue::OK; } + } // namespace ACS +// namespace ACS diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index daf4913c..5182215d 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,16 +1,18 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ - #include -#include "mission/devices/devicedefinitions/SusDefinitions.h" + #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" +#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h" #include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h" +#include "mission/devices/devicedefinitions/SusDefinitions.h" namespace ACS { -class SensorValues{ +class SensorValues { public: SensorValues(); virtual ~SensorValues(); @@ -18,6 +20,7 @@ class SensorValues{ ReturnValue_t update(); ReturnValue_t updateMgm(); ReturnValue_t updateSus(); + ReturnValue_t updateGyr(); MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); @@ -44,13 +47,10 @@ class SensorValues{ SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), }; - double rmu0[3]; - double rmu1[3]; - double rmu2[3]; - - bool rmu0Valid; - bool rmu1Valid; - bool rmu2Valid; + AdisGyroPrimaryDataset gyr0AdisSet = AdisGyroPrimaryDataset(objects::GYRO_0_ADIS_HANDLER); + GyroPrimaryDataset gyr1L3gSet = GyroPrimaryDataset(objects::GYRO_1_L3G_HANDLER); + AdisGyroPrimaryDataset gyr2AdisSet = AdisGyroPrimaryDataset(objects::GYRO_2_ADIS_HANDLER); + GyroPrimaryDataset gyr3L3gSet = GyroPrimaryDataset(objects::GYRO_3_L3G_HANDLER); double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? bool quatJBValid;