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

fixed inputs. removed outputs. minor fixes
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2022-10-10 09:54:06 +02:00
parent bbf4be1cfe
commit f77b3498ec
4 changed files with 86 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
mission/controller/AcsController.cpp
View File

@ -38,7 +38,7 @@ void AcsController::performControlOperation() {
break; break;
case SUBMODE_DETUMBLE: case SUBMODE_DETUMBLE:
// performDetumble(); performDetumble();
break; break;
case SUBMODE_PTG_GS: case SUBMODE_PTG_GS:
@ -64,10 +64,9 @@ void AcsController::performControlOperation() {
copySusData(); copySusData();
} }
} }
sif::debug << susData.sus0.value[0] << "," << susData.sus0.value[1] << ","
<< susData.sus0.value[2] << "," << susData.sus0.value[3] << "," mode = MODE_ON;
<< susData.sus0.value[4] << "," << susData.sus0.value[5] << std::endl; submode = SUBMODE_DETUMBLE;
sif::debug << susData.sus0.isValid() << std::endl;
} }
void AcsController::performSafe() {} void AcsController::performSafe() {}
@ -82,7 +81,7 @@ void AcsController::performDetumble() {
timeval now; // = {0,0}; timeval now; // = {0,0};
Clock::getClock_timeval(&now); Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters); sensorProcessing.process(&susData, now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf; ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf); navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};

4
mission/controller/acs/AcsParameters.h
View File

@ -16,11 +16,11 @@ public:
AcsParameters(); AcsParameters();
virtual ~AcsParameters(); virtual ~AcsParameters();
/*
virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId,
ParameterWrapper *parameterWrapper, ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues, uint16_t startAtIndex); const ParameterWrapper *newValues, uint16_t startAtIndex);
*/
struct OnBoardParams { struct OnBoardParams {
double sampleTime = 0.1; // [s] double sampleTime = 0.1; // [s]
} onBoardParams; } onBoardParams;

155
mission/controller/acs/SensorProcessing.cpp
View File

@ -144,29 +144,29 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
return true; return true;
} }
void SensorProcessing::processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, void SensorProcessing::processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement,
const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters, const AcsParameters::SunModelParameters *sunModelParameters,
double *sunDirEst, bool *sunDirEstValid, double *sunDirEst, bool *sunDirEstValid,
double *sunVectorInertial, bool *sunVectorInertialValid, double *sunVectorInertial, bool *sunVectorInertialValid,
double *sunVectorDerivative, bool *sunVectorDerivativeValid) { double *sunVectorDerivative, bool *sunVectorDerivativeValid) {
susData.sus0.setValid(susConverter.checkSunSensorData(susData.sus0)); susData->sus0.setValid(susConverter.checkSunSensorData(susData->sus0));
susData.sus1.setValid(susConverter.checkSunSensorData(susData.sus1)); susData->sus1.setValid(susConverter.checkSunSensorData(susData->sus1));
susData.sus2.setValid(susConverter.checkSunSensorData(susData.sus2)); susData->sus2.setValid(susConverter.checkSunSensorData(susData->sus2));
susData.sus3.setValid(susConverter.checkSunSensorData(susData.sus3)); susData->sus3.setValid(susConverter.checkSunSensorData(susData->sus3));
susData.sus4.setValid(susConverter.checkSunSensorData(susData.sus4)); susData->sus4.setValid(susConverter.checkSunSensorData(susData->sus4));
susData.sus5.setValid(susConverter.checkSunSensorData(susData.sus5)); susData->sus5.setValid(susConverter.checkSunSensorData(susData->sus5));
susData.sus6.setValid(susConverter.checkSunSensorData(susData.sus6)); susData->sus6.setValid(susConverter.checkSunSensorData(susData->sus6));
susData.sus7.setValid(susConverter.checkSunSensorData(susData.sus7)); susData->sus7.setValid(susConverter.checkSunSensorData(susData->sus7));
susData.sus8.setValid(susConverter.checkSunSensorData(susData.sus8)); susData->sus8.setValid(susConverter.checkSunSensorData(susData->sus8));
susData.sus9.setValid(susConverter.checkSunSensorData(susData.sus9)); susData->sus9.setValid(susConverter.checkSunSensorData(susData->sus9));
susData.sus10.setValid(susConverter.checkSunSensorData(susData.sus10)); susData->sus10.setValid(susConverter.checkSunSensorData(susData->sus10));
susData.sus11.setValid(susConverter.checkSunSensorData(susData.sus11)); susData->sus11.setValid(susConverter.checkSunSensorData(susData->sus11));
if (!susData.sus0.isValid() && !susData.sus1.isValid() && !susData.sus2.isValid() && if (!susData->sus0.isValid() && !susData->sus1.isValid() && !susData->sus2.isValid() &&
!susData.sus3.isValid() && !susData.sus4.isValid() && !susData.sus5.isValid() && !susData->sus3.isValid() && !susData->sus4.isValid() && !susData->sus5.isValid() &&
!susData.sus6.isValid() && !susData.sus7.isValid() && !susData.sus8.isValid() && !susData->sus6.isValid() && !susData->sus7.isValid() && !susData->sus8.isValid() &&
!susData.sus9.isValid() && !susData.sus10.isValid() && !susData.sus11.isValid()) { !susData->sus9.isValid() && !susData->sus10.isValid() && !susData->sus11.isValid()) {
*sunDirEstValid = false; *sunDirEstValid = false;
return; return;
} else { } 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}, 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}; sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};
if (susData.sus0.isValid()) { if (susData->sus0.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus0orientationMatrix[0], susParameters->sus0orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus0, susParameters->sus0coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus0, susParameters->sus0coeffAlpha,
susParameters->sus0coeffBeta), susParameters->sus0coeffBeta),
sus0VecBody, 3, 3, 1); sus0VecBody, 3, 3, 1);
} }
if (susData.sus1.isValid()) { if (susData->sus1.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus1orientationMatrix[0], susParameters->sus1orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus1, susParameters->sus1coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus1, susParameters->sus1coeffAlpha,
susParameters->sus1coeffBeta), susParameters->sus1coeffBeta),
sus1VecBody, 3, 3, 1); sus1VecBody, 3, 3, 1);
} }
if (susData.sus2.isValid()) { if (susData->sus2.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus2orientationMatrix[0], susParameters->sus2orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus2, susParameters->sus2coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus2, susParameters->sus2coeffAlpha,
susParameters->sus2coeffBeta), susParameters->sus2coeffBeta),
sus2VecBody, 3, 3, 1); sus2VecBody, 3, 3, 1);
} }
if (susData.sus3.isValid()) { if (susData->sus3.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus3orientationMatrix[0], susParameters->sus3orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus3, susParameters->sus3coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus3, susParameters->sus3coeffAlpha,
susParameters->sus3coeffBeta), susParameters->sus3coeffBeta),
sus3VecBody, 3, 3, 1); sus3VecBody, 3, 3, 1);
} }
if (susData.sus4.isValid()) { if (susData->sus4.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus4orientationMatrix[0], susParameters->sus4orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus4, susParameters->sus4coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus4, susParameters->sus4coeffAlpha,
susParameters->sus4coeffBeta), susParameters->sus4coeffBeta),
sus4VecBody, 3, 3, 1); sus4VecBody, 3, 3, 1);
} }
if (susData.sus5.isValid()) { if (susData->sus5.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus5orientationMatrix[0], susParameters->sus5orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus5, susParameters->sus5coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus5, susParameters->sus5coeffAlpha,
susParameters->sus5coeffBeta), susParameters->sus5coeffBeta),
sus5VecBody, 3, 3, 1); sus5VecBody, 3, 3, 1);
} }
if (susData.sus6.isValid()) { if (susData->sus6.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus6orientationMatrix[0], susParameters->sus6orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus6, susParameters->sus6coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus6, susParameters->sus6coeffAlpha,
susParameters->sus6coeffBeta), susParameters->sus6coeffBeta),
sus6VecBody, 3, 3, 1); sus6VecBody, 3, 3, 1);
} }
if (susData.sus7.isValid()) { if (susData->sus7.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus7orientationMatrix[0], susParameters->sus7orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus7, susParameters->sus7coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus7, susParameters->sus7coeffAlpha,
susParameters->sus7coeffBeta), susParameters->sus7coeffBeta),
sus7VecBody, 3, 3, 1); sus7VecBody, 3, 3, 1);
} }
if (susData.sus8.isValid()) { if (susData->sus8.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus8orientationMatrix[0], susParameters->sus8orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus8, susParameters->sus8coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus8, susParameters->sus8coeffAlpha,
susParameters->sus8coeffBeta), susParameters->sus8coeffBeta),
sus8VecBody, 3, 3, 1); sus8VecBody, 3, 3, 1);
} }
if (susData.sus9.isValid()) { if (susData->sus9.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus9orientationMatrix[0], susParameters->sus9orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus9, susParameters->sus9coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus9, susParameters->sus9coeffAlpha,
susParameters->sus9coeffBeta), susParameters->sus9coeffBeta),
sus9VecBody, 3, 3, 1); sus9VecBody, 3, 3, 1);
} }
if (susData.sus10.isValid()) { if (susData->sus10.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus10orientationMatrix[0], susParameters->sus10orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus10, susParameters->sus10coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus10, susParameters->sus10coeffAlpha,
susParameters->sus10coeffBeta), susParameters->sus10coeffBeta),
sus10VecBody, 3, 3, 1); sus10VecBody, 3, 3, 1);
} }
if (susData.sus11.isValid()) { if (susData->sus11.isValid()) {
MatrixOperations<float>::multiply( MatrixOperations<float>::multiply(
susParameters->sus11orientationMatrix[0], susParameters->sus11orientationMatrix[0],
susConverter.getSunVectorSensorFrame(susData.sus11, susParameters->sus11coeffAlpha, susConverter.getSunVectorSensorFrame(susData->sus11, susParameters->sus11coeffAlpha,
susParameters->sus11coeffBeta), susParameters->sus11coeffBeta),
sus11VecBody, 3, 3, 1); sus11VecBody, 3, 3, 1);
} }
/* ------ Mean Value: susDirEst ------ */ /* ------ Mean Value: susDirEst ------ */
// Timo already done bool validIds[12] = {
bool validIds[12] = {susData.sus0.isValid(), susData.sus1.isValid(), susData.sus2.isValid(), susData->sus0.isValid(), susData->sus1.isValid(), susData->sus2.isValid(),
susData.sus3.isValid(), susData.sus4.isValid(), susData.sus5.isValid(), susData->sus3.isValid(), susData->sus4.isValid(), susData->sus5.isValid(),
susData.sus6.isValid(), susData.sus7.isValid(), susData.sus8.isValid(), susData->sus6.isValid(), susData->sus7.isValid(), susData->sus8.isValid(),
susData.sus9.isValid(), susData.sus10.isValid(), susData.sus11.isValid()}; susData->sus9.isValid(), susData->sus10.isValid(), susData->sus11.isValid()};
float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0], float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0], sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[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]}}; sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
double susMeanValue[3] = {0, 0, 0}; double susMeanValue[3] = {0, 0, 0};
uint8_t validSusCounter = 0; float validSusCounter = 0;
for (uint8_t i = 0; i < 12; i++) { for (uint8_t i = 0; i < 12; i++) {
if (validIds[i]) { if (validIds[i]) {
susMeanValue[0] += susVecBody[0][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, void SensorProcessing::process(acsctrl::SusDataRaw *susData, timeval now,
ACS::OutputValues *outputValues, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
const AcsParameters *acsParameters) { const AcsParameters *acsParameters) {
sensorValues->update(); // sensorValues->update();
processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid, // processGps(sensorValues->gps0latitude, sensorValues->gps0longitude, sensorValues->gps0Valid,
&outputValues->gcLatitude, &outputValues->gdLongitude); // &outputValues->gcLatitude, &outputValues->gdLongitude);
/*outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid, // outputValues->mgmUpdated = processMgm(sensorValues->mgm0, sensorValues->mgm0Valid,
sensorValues->mgm1, sensorValues->mgm1Valid, // sensorValues->mgm1, sensorValues->mgm1Valid,
sensorValues->mgm2, sensorValues->mgm2Valid, // sensorValues->mgm2, sensorValues->mgm2Valid,
sensorValues->mgm3, sensorValues->mgm3Valid, // sensorValues->mgm3, sensorValues->mgm3Valid,
sensorValues->mgm4, sensorValues->mgm4Valid, now, // sensorValues->mgm4, sensorValues->mgm4Valid, now,
&acsParameters->mgmHandlingParameters, outputValues->gcLatitude, // &acsParameters->mgmHandlingParameters, outputValues->gcLatitude,
outputValues->gdLongitude, sensorValues->gps0altitude, // outputValues->gdLongitude, sensorValues->gps0altitude,
sensorValues->gps0Valid, // sensorValues->gps0Valid,
outputValues->magFieldEst, &outputValues->magFieldEstValid, // outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid, // outputValues->magFieldModel, &outputValues->magFieldModelValid,
outputValues->magneticFieldVectorDerivative, // outputValues->magneticFieldVectorDerivative,
&outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ? // &outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ?
processSus(sensorValues->sus0, sensorValues->sus0Valid, sensorValues->sus1, processSus(susData, now, &acsParameters->susHandlingParameters,
sensorValues->sus1Valid, sensorValues->sus2, sensorValues->sus2Valid, sensorValues->sus3, &acsParameters->sunModelParameters, outputValues->sunDirEst,
sensorValues->sus3Valid, sensorValues->sus4, sensorValues->sus4Valid, sensorValues->sus5, &outputValues->sunDirEstValid, outputValues->sunDirModel,
sensorValues->sus5Valid, sensorValues->sus6, sensorValues->sus6Valid, sensorValues->sus7, &outputValues->sunDirModelValid, outputValues->sunVectorDerivative,
sensorValues->sus7Valid, sensorValues->sus8, sensorValues->sus8Valid, sensorValues->sus9, &outputValues->sunVectorDerivativeValid);
sensorValues->sus9Valid, sensorValues->sus10, sensorValues->sus10Valid, sensorValues->sus11, // VALID outputs ?
sensorValues->sus11Valid, now, &acsParameters->susHandlingParameters,
&acsParameters->sunModelParameters, outputValues->sunDirEst, &outputValues->sunDirEstValid, // processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1,
outputValues->sunDirModel, &outputValues->sunDirModelValid, // sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now,
outputValues->sunVectorDerivative, &outputValues->sunVectorDerivativeValid); // // &acsParameters->rmuHandlingParameters, outputValues->satRateEst,
VALID outputs ? // &outputValues->satRateEstValid);
*/
processRmu(sensorValues->rmu0, sensorValues->rmu0Valid, sensorValues->rmu1,
sensorValues->rmu1Valid, sensorValues->rmu2, sensorValues->rmu2Valid, now,
&acsParameters->rmuHandlingParameters, outputValues->satRateEst,
&outputValues->satRateEstValid);
} }

7
mission/controller/acs/SensorProcessing.h
View File

@ -9,11 +9,11 @@
#include <stdint.h> //uint8_t #include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/ #include <time.h> /*purpose, timeval ?*/
#include "SusConverter.h"
#include "../controllerdefinitions/AcsCtrlDefinitions.h" #include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h" #include "AcsParameters.h"
#include "OutputValues.h" #include "OutputValues.h"
#include "SensorValues.h" #include "SensorValues.h"
#include "SusConverter.h"
#include "config/classIds.h" #include "config/classIds.h"
/*Planned: /*Planned:
@ -35,7 +35,8 @@ class SensorProcessing {
SensorProcessing(AcsParameters *acsParameters_); SensorProcessing(AcsParameters *acsParameters_);
virtual ~SensorProcessing(); 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 const AcsParameters *acsParameters); // Will call protected functions
private: private:
protected: protected:
@ -49,7 +50,7 @@ class SensorProcessing {
bool *magFieldModelValid, double *magneticFieldVectorDerivative, bool *magFieldModelValid, double *magneticFieldVectorDerivative,
bool *magneticFieldVectorDerivativeValid); // Output bool *magneticFieldVectorDerivativeValid); // Output
void processSus(acsctrl::SusDataRaw susData, timeval timeOfSusMeasurement, void processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement,
const AcsParameters::SusHandlingParameters *susParameters, const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid, bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,