diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 2b0689c7..91d38eff 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -38,7 +38,7 @@ void AcsController::performControlOperation() { break; case SUBMODE_DETUMBLE: - // performDetumble(); + performDetumble(); break; case SUBMODE_PTG_GS: @@ -64,10 +64,9 @@ void AcsController::performControlOperation() { copySusData(); } } - sif::debug << susData.sus0.value[0] << "," << susData.sus0.value[1] << "," - << susData.sus0.value[2] << "," << susData.sus0.value[3] << "," - << susData.sus0.value[4] << "," << susData.sus0.value[5] << std::endl; - sif::debug << susData.sus0.isValid() << std::endl; + + mode = MODE_ON; + submode = SUBMODE_DETUMBLE; } void AcsController::performSafe() {} @@ -82,7 +81,7 @@ void AcsController::performDetumble() { timeval now; // = {0,0}; Clock::getClock_timeval(&now); - sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); + sensorProcessing.process(&susData, now, &sensorValues, &outputValues, &acsParameters); ReturnValue_t validMekf; navigation.useMekf(&sensorValues, &outputValues, &validMekf); double magMomMtq[3] = {0, 0, 0}; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 39f2c3c6..c342dabc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -16,11 +16,11 @@ public: AcsParameters(); virtual ~AcsParameters(); - +/* virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues, uint16_t startAtIndex); - +*/ struct OnBoardParams { double sampleTime = 0.1; // [s] } onBoardParams; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 933800e2..c5db92ad 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -144,29 +144,29 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const return true; } -void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, +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)); + 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)); - 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 (!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()) { *sunDirEstValid = false; return; } else { @@ -177,97 +177,97 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSus 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 (susData->sus0.isValid()) { MatrixOperations::multiply( susParameters->sus0orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus0, susParameters->sus0coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus0, susParameters->sus0coeffAlpha, susParameters->sus0coeffBeta), sus0VecBody, 3, 3, 1); } - if (susData.sus1.isValid()) { + if (susData->sus1.isValid()) { MatrixOperations::multiply( susParameters->sus1orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus1, susParameters->sus1coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus1, susParameters->sus1coeffAlpha, susParameters->sus1coeffBeta), sus1VecBody, 3, 3, 1); } - if (susData.sus2.isValid()) { + if (susData->sus2.isValid()) { MatrixOperations::multiply( susParameters->sus2orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus2, susParameters->sus2coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus2, susParameters->sus2coeffAlpha, susParameters->sus2coeffBeta), sus2VecBody, 3, 3, 1); } - if (susData.sus3.isValid()) { + if (susData->sus3.isValid()) { MatrixOperations::multiply( susParameters->sus3orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus3, susParameters->sus3coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus3, susParameters->sus3coeffAlpha, susParameters->sus3coeffBeta), sus3VecBody, 3, 3, 1); } - if (susData.sus4.isValid()) { + if (susData->sus4.isValid()) { MatrixOperations::multiply( susParameters->sus4orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus4, susParameters->sus4coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus4, susParameters->sus4coeffAlpha, susParameters->sus4coeffBeta), sus4VecBody, 3, 3, 1); } - if (susData.sus5.isValid()) { + if (susData->sus5.isValid()) { MatrixOperations::multiply( susParameters->sus5orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus5, susParameters->sus5coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus5, susParameters->sus5coeffAlpha, susParameters->sus5coeffBeta), sus5VecBody, 3, 3, 1); } - if (susData.sus6.isValid()) { + if (susData->sus6.isValid()) { MatrixOperations::multiply( susParameters->sus6orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus6, susParameters->sus6coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus6, susParameters->sus6coeffAlpha, susParameters->sus6coeffBeta), sus6VecBody, 3, 3, 1); } - if (susData.sus7.isValid()) { + if (susData->sus7.isValid()) { MatrixOperations::multiply( susParameters->sus7orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus7, susParameters->sus7coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus7, susParameters->sus7coeffAlpha, susParameters->sus7coeffBeta), sus7VecBody, 3, 3, 1); } - if (susData.sus8.isValid()) { + if (susData->sus8.isValid()) { MatrixOperations::multiply( susParameters->sus8orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus8, susParameters->sus8coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus8, susParameters->sus8coeffAlpha, susParameters->sus8coeffBeta), sus8VecBody, 3, 3, 1); } - if (susData.sus9.isValid()) { + if (susData->sus9.isValid()) { MatrixOperations::multiply( susParameters->sus9orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus9, susParameters->sus9coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus9, susParameters->sus9coeffAlpha, susParameters->sus9coeffBeta), sus9VecBody, 3, 3, 1); } - if (susData.sus10.isValid()) { + if (susData->sus10.isValid()) { MatrixOperations::multiply( susParameters->sus10orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus10, susParameters->sus10coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus10, susParameters->sus10coeffAlpha, susParameters->sus10coeffBeta), sus10VecBody, 3, 3, 1); } - if (susData.sus11.isValid()) { + if (susData->sus11.isValid()) { MatrixOperations::multiply( susParameters->sus11orientationMatrix[0], - susConverter.getSunVectorSensorFrame(susData.sus11, susParameters->sus11coeffAlpha, + susConverter.getSunVectorSensorFrame(susData->sus11, susParameters->sus11coeffAlpha, susParameters->sus11coeffBeta), sus11VecBody, 3, 3, 1); } /* ------ Mean Value: susDirEst ------ */ - // Timo already done - 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] = { + 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()}; 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,7 +279,7 @@ void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSus sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}}; double susMeanValue[3] = {0, 0, 0}; - uint8_t validSusCounter = 0; + float validSusCounter = 0; for (uint8_t i = 0; i < 12; i++) { if (validIds[i]) { susMeanValue[0] += susVecBody[0][i]; @@ -407,40 +407,35 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo } } -void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, - ACS::OutputValues *outputValues, +void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now, + ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, const AcsParameters *acsParameters) { - sensorValues->update(); - processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, - &outputValues->gcLatitude, &outputValues->gdLongitude); + // 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->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 ? - processSus(sensorValues->sus0, sensorValues->sus0Valid, sensorValues->sus1, - sensorValues->sus1Valid, sensorValues->sus2, sensorValues->sus2Valid, sensorValues->sus3, - sensorValues->sus3Valid, sensorValues->sus4, sensorValues->sus4Valid, sensorValues->sus5, - sensorValues->sus5Valid, sensorValues->sus6, sensorValues->sus6Valid, sensorValues->sus7, - sensorValues->sus7Valid, sensorValues->sus8, sensorValues->sus8Valid, sensorValues->sus9, - sensorValues->sus9Valid, sensorValues->sus10, sensorValues->sus10Valid, sensorValues->sus11, - sensorValues->sus11Valid, now, &acsParameters->susHandlingParameters, - &acsParameters->sunModelParameters, outputValues->sunDirEst, &outputValues->sunDirEstValid, - outputValues->sunDirModel, &outputValues->sunDirModelValid, - outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // - VALID outputs ? -*/ - processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, - sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now, - &acsParameters->rmuHandlingParameters, outputValues->satRateEst, - &outputValues->satRateEstValid); + processSus(susData, now, &acsParameters->susHandlingParameters, + &acsParameters->sunModelParameters, outputValues->sunDirEst, + &outputValues->sunDirEstValid, outputValues->sunDirModel, + &outputValues->sunDirModelValid, outputValues->sunVectorDerivative, + &outputValues->sunVectorDerivativeValid); + // VALID outputs ? + + // processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1, + // sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now, + // &acsParameters->rmuHandlingParameters, outputValues->satRateEst, + // &outputValues->satRateEstValid); } diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index baa9c5b6..bde4f34e 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -9,11 +9,11 @@ #include //uint8_t #include /*purpose, timeval ?*/ -#include "SusConverter.h" #include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "AcsParameters.h" #include "OutputValues.h" #include "SensorValues.h" +#include "SusConverter.h" #include "config/classIds.h" /*Planned: @@ -35,7 +35,8 @@ class SensorProcessing { SensorProcessing(AcsParameters *acsParameters_); virtual ~SensorProcessing(); - void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues, + void process(acsctrl::SusDataRaw *susData, timeval now, ACS::SensorValues *sensorValues, + ACS::OutputValues *outputValues, const AcsParameters *acsParameters); // Will call protected functions private: protected: @@ -49,7 +50,7 @@ class SensorProcessing { bool *magFieldModelValid, double *magneticFieldVectorDerivative, bool *magneticFieldVectorDerivativeValid); // Output - void processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, + void processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,