fixed inputs. removed outputs. minor fixes
EIVE/eive-obsw/pipeline/head This commit looks good
Details
EIVE/eive-obsw/pipeline/head This commit looks good
Details
This commit is contained in:
parent
bbf4be1cfe
commit
f77b3498ec
|
|
@ -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};
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
Loading…
Reference in New Issue