From 076e67dd0bade52bfa0c7d8e2a8c5653b8fe8bad Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Oct 2022 16:02:57 +0200 Subject: [PATCH] switched sensor input from AcsController to SensorValues --- mission/controller/acs/SensorProcessing.cpp | 179 ++++++++++++-------- mission/controller/acs/SensorProcessing.h | 22 +-- mission/controller/acs/SensorValues.cpp | 57 +++++-- mission/controller/acs/SensorValues.h | 142 +++++++--------- mission/controller/acs/SusConverter.cpp | 39 ++--- mission/controller/acs/SusConverter.h | 8 +- 6 files changed, 241 insertions(+), 206 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index c5db92ad..749ec373 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -19,8 +19,6 @@ #include "util/MathOperations.h" using namespace Math; -// Thought: Maybe separate file for transforming of sensor values -// into geometry frame and body frame SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} { validMagField = false; @@ -144,29 +142,56 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const return true; } -void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement, - const AcsParameters::SusHandlingParameters *susParameters, - const AcsParameters::SunModelParameters *sunModelParameters, - double *sunDirEst, bool *sunDirEstValid, - double *sunVectorInertial, bool *sunVectorInertialValid, - double *sunVectorDerivative, bool *sunVectorDerivativeValid) { - susData->sus0.setValid(susConverter.checkSunSensorData(susData->sus0)); - susData->sus1.setValid(susConverter.checkSunSensorData(susData->sus1)); - susData->sus2.setValid(susConverter.checkSunSensorData(susData->sus2)); - susData->sus3.setValid(susConverter.checkSunSensorData(susData->sus3)); - susData->sus4.setValid(susConverter.checkSunSensorData(susData->sus4)); - susData->sus5.setValid(susConverter.checkSunSensorData(susData->sus5)); - susData->sus6.setValid(susConverter.checkSunSensorData(susData->sus6)); - susData->sus7.setValid(susConverter.checkSunSensorData(susData->sus7)); - susData->sus8.setValid(susConverter.checkSunSensorData(susData->sus8)); - susData->sus9.setValid(susConverter.checkSunSensorData(susData->sus9)); - susData->sus10.setValid(susConverter.checkSunSensorData(susData->sus10)); - susData->sus11.setValid(susConverter.checkSunSensorData(susData->sus11)); +void SensorProcessing::processSus( + const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, bool sus1valid, + const uint16_t *sus2Value, bool sus2valid, const uint16_t *sus3Value, bool sus3valid, + const uint16_t *sus4Value, bool sus4valid, const uint16_t *sus5Value, bool sus5valid, + const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid, + const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid, + const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid, + timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, + const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, + bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, + double *sunVectorDerivative, bool *sunVectorDerivativeValid) { + if (sus0valid) { + sus0valid = susConverter.checkSunSensorData(sus0Value); + } + if (sus1valid) { + sus1valid = susConverter.checkSunSensorData(sus1Value); + } + if (sus2valid) { + sus2valid = susConverter.checkSunSensorData(sus2Value); + } + if (sus3valid) { + sus3valid = susConverter.checkSunSensorData(sus3Value); + } + if (sus4valid) { + sus4valid = susConverter.checkSunSensorData(sus4Value); + } + if (sus5valid) { + sus5valid = susConverter.checkSunSensorData(sus5Value); + } + if (sus6valid) { + sus6valid = susConverter.checkSunSensorData(sus6Value); + } + if (sus7valid) { + sus7valid = susConverter.checkSunSensorData(sus7Value); + } + if (sus8valid) { + sus8valid = susConverter.checkSunSensorData(sus8Value); + } + if (sus9valid) { + sus9valid = susConverter.checkSunSensorData(sus9Value); + } + if (sus10valid) { + sus10valid = susConverter.checkSunSensorData(sus10Value); + } + if (sus11valid) { + sus11valid = susConverter.checkSunSensorData(sus11Value); + } - if (!susData->sus0.isValid() && !susData->sus1.isValid() && !susData->sus2.isValid() && - !susData->sus3.isValid() && !susData->sus4.isValid() && !susData->sus5.isValid() && - !susData->sus6.isValid() && !susData->sus7.isValid() && !susData->sus8.isValid() && - !susData->sus9.isValid() && !susData->sus10.isValid() && !susData->sus11.isValid()) { + if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid && + !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) { *sunDirEstValid = false; return; } else { @@ -177,97 +202,94 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSu sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0}, sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0}; - if (susData->sus0.isValid()) { + if (sus0valid) { MatrixOperations::multiply( susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus0, susParameters->sus0coeffAlpha, + susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha, susParameters->sus0coeffBeta), sus0VecBody, 3, 3, 1); } - if (susData->sus1.isValid()) { + if (sus1valid) { MatrixOperations::multiply( susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus1, susParameters->sus1coeffAlpha, + susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha, susParameters->sus1coeffBeta), sus1VecBody, 3, 3, 1); } - if (susData->sus2.isValid()) { + if (sus2valid) { MatrixOperations::multiply( susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus2, susParameters->sus2coeffAlpha, + susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha, susParameters->sus2coeffBeta), sus2VecBody, 3, 3, 1); } - if (susData->sus3.isValid()) { + if (sus3valid) { MatrixOperations::multiply( susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus3, susParameters->sus3coeffAlpha, + susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha, susParameters->sus3coeffBeta), sus3VecBody, 3, 3, 1); } - if (susData->sus4.isValid()) { + if (sus4valid) { MatrixOperations::multiply( susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus4, susParameters->sus4coeffAlpha, + susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha, susParameters->sus4coeffBeta), sus4VecBody, 3, 3, 1); } - if (susData->sus5.isValid()) { + if (sus5valid) { MatrixOperations::multiply( susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus5, susParameters->sus5coeffAlpha, + susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha, susParameters->sus5coeffBeta), sus5VecBody, 3, 3, 1); } - if (susData->sus6.isValid()) { + if (sus6valid) { MatrixOperations::multiply( susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus6, susParameters->sus6coeffAlpha, + susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha, susParameters->sus6coeffBeta), sus6VecBody, 3, 3, 1); } - if (susData->sus7.isValid()) { + if (sus7valid) { MatrixOperations::multiply( susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus7, susParameters->sus7coeffAlpha, + susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha, susParameters->sus7coeffBeta), sus7VecBody, 3, 3, 1); } - if (susData->sus8.isValid()) { + if (sus8valid) { MatrixOperations::multiply( susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus8, susParameters->sus8coeffAlpha, + susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha, susParameters->sus8coeffBeta), sus8VecBody, 3, 3, 1); } - if (susData->sus9.isValid()) { + if (sus9valid) { MatrixOperations::multiply( susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus9, susParameters->sus9coeffAlpha, + susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha, susParameters->sus9coeffBeta), sus9VecBody, 3, 3, 1); } - if (susData->sus10.isValid()) { + if (sus10valid) { MatrixOperations::multiply( susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus10, susParameters->sus10coeffAlpha, + susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha, susParameters->sus10coeffBeta), sus10VecBody, 3, 3, 1); } - if (susData->sus11.isValid()) { + if (sus11valid) { MatrixOperations::multiply( susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData->sus11, susParameters->sus11coeffAlpha, + susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha, susParameters->sus11coeffBeta), sus11VecBody, 3, 3, 1); } /* ------ Mean Value: susDirEst ------ */ - bool validIds[12] = { - susData->sus0.isValid(), susData->sus1.isValid(), susData->sus2.isValid(), - susData->sus3.isValid(), susData->sus4.isValid(), susData->sus5.isValid(), - susData->sus6.isValid(), susData->sus7.isValid(), susData->sus8.isValid(), - susData->sus9.isValid(), susData->sus10.isValid(), susData->sus11.isValid()}; + bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid, + sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid}; float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]}, @@ -279,17 +301,14 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSu sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; double susMeanValue[3] = {0, 0, 0}; - float validSusCounter = 0; for (uint8_t i = 0; i < 12; i++) { if (validIds[i]) { susMeanValue[0] += susVecBody[0][i]; susMeanValue[1] += susVecBody[1][i]; susMeanValue[2] += susVecBody[2][i]; - validSusCounter += 1; } } - double divisor = 1 / validSusCounter; - VectorOperations::mulScalar(susMeanValue, divisor, sunDirEst, 3); + VectorOperations::normalize(susMeanValue, sunDirEst, 3); *sunDirEstValid = true; } @@ -410,26 +429,40 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { - // sensorValues->update(); + sensorValues->update(); // processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, // &outputValues->gcLatitude, &outputValues->gdLongitude); - // outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, - // sensorValues->mgm1, sensorValues->mgm1Valid, - // sensorValues->mgm2, sensorValues->mgm2Valid, - // sensorValues->mgm3, sensorValues->mgm3Valid, - // sensorValues->mgm4, sensorValues->mgm4Valid, now, - // &acsParameters->mgmHandlingParameters, outputValues->gcLatitude, - // outputValues->gdLongitude, sensorValues->gps0altitude, - // sensorValues->gps0Valid, - // outputValues->magFieldEst, &outputValues->magFieldEstValid, - // outputValues->magFieldModel, &outputValues->magFieldModelValid, - // outputValues->magneticFieldVectorDerivative, - // &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? + outputValues->mgmUpdated = processMgm( + sensorValues->mgm0Lis3Set.fieldStrengths.value, + sensorValues->mgm0Lis3Set.fieldStrengths.isValid(), + sensorValues->mgm1Rm3100Set.fieldStrengths.value, + sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(), + sensorValues->mgm2Lis3Set.fieldStrengths.value, + sensorValues->mgm2Lis3Set.fieldStrengths.isValid(), + sensorValues->mgm3Rm3100Set.fieldStrengths.value, + sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value, + sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters, + outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gps0altitude, + sensorValues->gps0Valid, outputValues->magFieldEst, &outputValues->magFieldEstValid, + outputValues->magFieldModel, &outputValues->magFieldModelValid, + outputValues->magneticFieldVectorDerivative, + &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? - processSus(susData, now, &acsParameters->susHandlingParameters, - &acsParameters->sunModelParameters, outputValues->sunDirEst, - &outputValues->sunDirEstValid, outputValues->sunDirModel, + processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(), + sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(), + sensorValues->susSets[2].channels.value, sensorValues->susSets[2].channels.isValid(), + sensorValues->susSets[3].channels.value, sensorValues->susSets[3].channels.isValid(), + sensorValues->susSets[4].channels.value, sensorValues->susSets[4].channels.isValid(), + sensorValues->susSets[5].channels.value, sensorValues->susSets[5].channels.isValid(), + sensorValues->susSets[6].channels.value, sensorValues->susSets[6].channels.isValid(), + sensorValues->susSets[7].channels.value, sensorValues->susSets[7].channels.isValid(), + sensorValues->susSets[8].channels.value, sensorValues->susSets[8].channels.isValid(), + sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(), + sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(), + sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(), + now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters, + outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid, outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // VALID outputs ? diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index bde4f34e..df2b2a37 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -16,18 +16,6 @@ #include "SusConverter.h" #include "config/classIds.h" -/*Planned: - * - Fusion of Sensor Measurements - - * sunDirEst (mean value) - * magField (mean value) - * rmuSatRate (rmus, mean value) - * - Models to get inertia values - - * sunModelDir (input: time) - * magModelField (input: position,time) - * - Low Pass Filter maybe - - * magField - * SunDirEst*/ - class SensorProcessing { public: void reset(); @@ -50,7 +38,15 @@ class SensorProcessing { bool *magFieldModelValid, double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid); // Output - void processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement, + void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value, + bool sus1valid, const uint16_t *sus2Value, bool sus2valid, + const uint16_t *sus3Value, bool sus3valid, const uint16_t *sus4Value, + bool sus4valid, const uint16_t *sus5Value, bool sus5valid, + const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, + bool sus7valid, const uint16_t *sus8Value, bool sus8valid, + const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value, + bool sus10valid, const uint16_t *sus11Value, bool sus11valid, + timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 11720aeb..9ca2b90b 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -5,30 +5,57 @@ * Author: rooob */ #include "SensorValues.h" -#include -#include + +#include #include +#include +#include + +#include namespace ACS { -SensorValues::SensorValues() { +SensorValues::SensorValues() {} + +SensorValues::~SensorValues() {} + +ReturnValue_t SensorValues::updateMgm() { + ReturnValue_t result; + PoolReadGuard pgMgm0(&mgm0Lis3Set), pgMgm1(&mgm1Rm3100Set), pgMgm2(&mgm2Lis3Set), + pgMgm3(&mgm3Rm3100Set), pgImtq(&imtqMgmSet); + + result = (pgMgm0.getReadResult() || pgMgm1.getReadResult() || pgMgm2.getReadResult() || + pgMgm3.getReadResult() || pgImtq.getReadResult()); + return result; } -SensorValues::~SensorValues() { +ReturnValue_t SensorValues::updateSus() { + ReturnValue_t result; + PoolReadGuard pgSus0(&susSets[0]), pgSus1(&susSets[1]), pgSus2(&susSets[2]), pgSus3(&susSets[3]), + pgSus4(&susSets[4]), pgSus5(&susSets[5]), pgSus6(&susSets[6]), pgSus7(&susSets[7]), + pgSus8(&susSets[8]), pgSus9(&susSets[9]), pgSus10(&susSets[10]), pgSus11(&susSets[11]); + + result = (pgSus0.getReadResult() || pgSus1.getReadResult() || pgSus2.getReadResult() || + pgSus3.getReadResult() || pgSus4.getReadResult() || pgSus5.getReadResult() || + pgSus6.getReadResult() || pgSus7.getReadResult() || pgSus8.getReadResult() || + pgSus9.getReadResult() || pgSus10.getReadResult() || pgSus11.getReadResult()); + return result; } ReturnValue_t SensorValues::update() { -// lp_var_t quaternion(objects::STAR_TRACKER, PoolIds::CALI_QW, nullptr, pool_rwm_t::VAR_READ); -// ReturnValue_t result = quaternion.read(); + updateSus(); + updateMgm(); -// if ( result != RETURN_OK) { -// return RETURN_FAILED; -// } -// quatJB[3] = static_cast(quaternion.value); -// quatJBValid = quaternion.isValid(); + // 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(); - return returnvalue::OK; + return returnvalue::OK; } -} - - +} // namespace ACS diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index 001b141b..daf4913c 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,99 +1,85 @@ -/* Created on: 08.03.2022 - * Author: Robin - */ - #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ -#include + +#include +#include "mission/devices/devicedefinitions/SusDefinitions.h" +#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" +#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" +#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h" namespace ACS { class SensorValues{ -public: - SensorValues(); - virtual ~SensorValues(); + public: + SensorValues(); + virtual ~SensorValues(); - ReturnValue_t update(); + ReturnValue_t update(); + ReturnValue_t updateMgm(); + ReturnValue_t updateSus(); - float mgm0[3]; - float mgm1[3]; - float mgm2[3]; - float mgm3[3]; - float mgm4[3]; + MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = + MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER); + RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = + RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER); + MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = + MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); + RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = + RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); + IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); - bool mgm0Valid; - bool mgm1Valid; - bool mgm2Valid; - bool mgm3Valid; - bool mgm4Valid; + std::array susSets{ + SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF), + SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB), + SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB), + SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF), + SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF), + SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB), + SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF), + SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB), + SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB), + SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF), + SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF), + SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB), + }; - float sus0[3]; - float sus1[3]; - float sus2[3]; - float sus3[3]; - float sus4[3]; - float sus5[3]; - float sus6[3]; - float sus7[3]; - float sus8[3]; - float sus9[3]; - float sus10[3]; - float sus11[3]; + double rmu0[3]; + double rmu1[3]; + double rmu2[3]; - bool sus0Valid; - bool sus1Valid; - bool sus2Valid; - bool sus3Valid; - bool sus4Valid; - bool sus5Valid; - bool sus6Valid; - bool sus7Valid; - bool sus8Valid; - bool sus9Valid; - bool sus10Valid; - bool sus11Valid; + bool rmu0Valid; + bool rmu1Valid; + bool rmu2Valid; + double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? + bool quatJBValid; + int strIntTime[2]; - double rmu0[3]; - double rmu1[3]; - double rmu2[3]; + double gps0latitude; // Reference is WGS84, so this one will probably be geodetic + double gps0longitude; // Should be geocentric for IGRF + double gps0altitude; + double gps0Velocity[3]; // speed over ground = ?? + double gps0Time; // utc - bool rmu0Valid; - bool rmu1Valid; - bool rmu2Valid; + // valid ids for gps values ! + int gps0TimeYear; + int gps0TimeMonth; + int gps0TimeHour; // should be double + bool gps0Valid; - double quatJB[4]; // output star tracker. quaternion or dcm ? refrence to which KOS? - bool quatJBValid; - int strIntTime[2]; - - double gps0latitude; //Reference is WGS84, so this one will probably be geodetic - double gps0longitude; //Should be geocentric for IGRF - double gps0altitude; - double gps0Velocity[3]; // speed over ground = ?? - double gps0Time; //utc - -// valid ids for gps values ! - int gps0TimeYear; - int gps0TimeMonth; - int gps0TimeHour; // should be double - bool gps0Valid; - - - bool mgt0valid; - -// Reaction wheel measurements - double speedRw0; // RPM [1/min] - double speedRw1; // RPM [1/min] - double speedRw2; // RPM [1/min] - double speedRw3; // RPM [1/min] - bool validRw0; - bool validRw1; - bool validRw2; - bool validRw3; + bool mgt0valid; + // Reaction wheel measurements + double speedRw0; // RPM [1/min] + double speedRw1; // RPM [1/min] + double speedRw2; // RPM [1/min] + double speedRw3; // RPM [1/min] + bool validRw0; + bool validRw1; + bool validRw2; + bool validRw3; }; } /* namespace ACS */ #endif /*ENSORVALUES_H_*/ - diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index ea52f5a2..26804f12 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -14,30 +14,26 @@ #include -bool SusConverter::checkSunSensorData(lp_vec_t susChannel) { - if (susChannel.value[0] <= susChannelValueCheckLow || - susChannel.value[0] > susChannelValueCheckHigh || - susChannel.value[0] > susChannel.value[GNDREF]) { +bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) { + if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh || + susChannel[0] > susChannel[GNDREF]) { return false; } - if (susChannel.value[1] <= susChannelValueCheckLow || - susChannel.value[1] > susChannelValueCheckHigh || - susChannel.value[1] > susChannel.value[GNDREF]) { + if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh || + susChannel[1] > susChannel[GNDREF]) { return false; }; - if (susChannel.value[2] <= susChannelValueCheckLow || - susChannel.value[2] > susChannelValueCheckHigh || - susChannel.value[2] > susChannel.value[GNDREF]) { + if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh || + susChannel[2] > susChannel[GNDREF]) { return false; }; - if (susChannel.value[3] <= susChannelValueCheckLow || - susChannel.value[3] > susChannelValueCheckHigh || - susChannel.value[3] > susChannel.value[GNDREF]) { + if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh || + susChannel[3] > susChannel[GNDREF]) { return false; }; - susChannelValueSum = 4 * susChannel.value[GNDREF] - (susChannel.value[0] + susChannel.value[1] + - susChannel.value[2] + susChannel.value[3]); + susChannelValueSum = + 4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]); if ((susChannelValueSum < susChannelValueSumHigh) && (susChannelValueSum > susChannelValueSumLow)) { return false; @@ -45,17 +41,17 @@ bool SusConverter::checkSunSensorData(lp_vec_t susChannel) { return true; } -void SusConverter::calcAngle(lp_vec_t susChannel) { +void SusConverter::calcAngle(const uint16_t susChannel[6]) { float xout, yout; float s = 0.03; // s=[mm] gap between diodes uint8_t d = 5; // d=[mm] edge length of the quadratic aperture uint8_t h = 1; // h=[mm] distance between diodes and aperture int ch0, ch1, ch2, ch3; // Substract measurement values from GNDREF zero current threshold - ch0 = susChannel.value[GNDREF] - susChannel.value[0]; - ch1 = susChannel.value[GNDREF] - susChannel.value[1]; - ch2 = susChannel.value[GNDREF] - susChannel.value[2]; - ch3 = susChannel.value[GNDREF] - susChannel.value[3]; + ch0 = susChannel[GNDREF] - susChannel[0]; + ch1 = susChannel[GNDREF] - susChannel[1]; + ch2 = susChannel[GNDREF] - susChannel[2]; + ch3 = susChannel[GNDREF] - susChannel[3]; // Calculation of x and y xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] @@ -124,11 +120,10 @@ float* SusConverter::calculateSunVector() { return sunVectorBodyFrame; } -float* SusConverter::getSunVectorSensorFrame(lp_vec_t susChannel, +float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10], const float coeffBeta[9][10]) { calcAngle(susChannel); calibration(coeffAlpha, coeffBeta); return calculateSunVector(); } - diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 32f9e954..01143a39 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -17,16 +17,14 @@ class SusConverter { public: SusConverter() {} - bool checkSunSensorData(lp_vec_t susChannel); + bool checkSunSensorData(const uint16_t susChannel[6]); - void calcAngle(lp_vec_t susChannel); + void calcAngle(const uint16_t susChannel[6]); void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]); float* calculateSunVector(); - bool getValidFlag(uint8_t susNumber); - float* getSunVectorSensorFrame(lp_vec_t susChannel, const float coeffAlpha[9][10], + float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10], const float coeffBeta[9][10]); - float* TransferSunVector(); private: float alphaBetaRaw[2]; //[°]