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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +86 -91
4 changed files with 86 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit f77b3498ec - Show all commits

11
mission/controller/AcsController.cpp
View File

@ -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};

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

@ -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;

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

@ -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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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);
}

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

@ -9,11 +9,11 @@
#include <stdint.h> //uint8_t
#include <time.h> /*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,