switched sensor input from AcsController to SensorValues
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
82e9c6d092
commit
076e67dd0b
|
|
@ -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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<float>::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<double>::mulScalar(susMeanValue, divisor, sunDirEst, 3);
|
||||
VectorOperations<double>::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 ?
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -5,30 +5,57 @@
|
|||
* Author: rooob
|
||||
*/
|
||||
#include "SensorValues.h"
|
||||
#include <stddef.h>
|
||||
#include <cmath>
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||
#include <fsfw/datapoollocal/LocalPoolVector.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
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<float> 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<double>(quaternion.value);
|
||||
// quatJBValid = quaternion.isValid();
|
||||
// lp_var_t<float> 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<double>(quaternion.value);
|
||||
// quatJBValid = quaternion.isValid();
|
||||
|
||||
return returnvalue::OK;
|
||||
return returnvalue::OK;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
} // namespace ACS
|
||||
|
|
|
|||
|
|
@ -1,99 +1,85 @@
|
|||
/* Created on: 08.03.2022
|
||||
* Author: Robin
|
||||
*/
|
||||
|
||||
#ifndef SENSORVALUES_H_
|
||||
#define SENSORVALUES_H_
|
||||
|
||||
#include <fsfw/returnvalues/returnvalue.h>
|
||||
|
||||
#include <commonObjects.h>
|
||||
#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<SUS::SusDataset, 12> 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_*/
|
||||
|
||||
|
|
|
|||
|
|
@ -14,30 +14,26 @@
|
|||
|
||||
#include <iostream>
|
||||
|
||||
bool SusConverter::checkSunSensorData(lp_vec_t<uint16_t, 6> 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<uint16_t, 6> susChannel) {
|
|||
return true;
|
||||
}
|
||||
|
||||
void SusConverter::calcAngle(lp_vec_t<uint16_t, 6> 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<uint16_t, 6> 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();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,16 +17,14 @@ class SusConverter {
|
|||
public:
|
||||
SusConverter() {}
|
||||
|
||||
bool checkSunSensorData(lp_vec_t<uint16_t, 6> susChannel);
|
||||
bool checkSunSensorData(const uint16_t susChannel[6]);
|
||||
|
||||
void calcAngle(lp_vec_t<uint16_t, 6> 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<uint16_t, 6> 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]; //[°]
|
||||
|
|
|
|||
Loading…
Reference in New Issue