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

added low pass filter for gyr and mgmDot values
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-04-05 11:25:44 +02:00
parent 4ca8c38c98
commit 7a7d0e650f
3 changed files with 21 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -72,6 +72,7 @@ class AcsParameters : public HasParametersIF {
float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
float mgmDerivativeFilterWeight = 0.5;
} mgmHandlingParameters; } mgmHandlingParameters;
struct SusHandlingParameters { struct SusHandlingParameters {
@ -780,6 +781,7 @@ class AcsParameters : public HasParametersIF {
pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
uint8_t preferAdis = true; uint8_t preferAdis = true;
float gyrFilterWeight = 0.6;
} gyrHandlingParameters; } gyrHandlingParameters;
struct RwHandlingParameters { struct RwHandlingParameters {

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

@ -150,6 +150,11 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
} }
timeOfSavedMagFieldEst = timeOfMgmMeasurement; timeOfSavedMagFieldEst = timeOfMgmMeasurement;
if (mgmDataProcessed->mgmVecTotDerivative.isValid()) {
lowPassFilter(mgmVecTotDerivative, mgmDataProcessed->mgmVecTotDerivative.value,
mgmParameters->mgmDerivativeFilterWeight);
}
{ {
PoolReadGuard pg(mgmDataProcessed); PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -527,6 +532,11 @@ void SensorProcessing::processGyr(
gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
} }
} }
if (gyrDataProcessed->gyrVecTot.isValid()) {
lowPassFilter(gyrVecTot, gyrDataProcessed->gyrVecTot.value, gyrParameters->gyrFilterWeight);
}
{ {
PoolReadGuard pg(gyrDataProcessed); PoolReadGuard pg(gyrDataProcessed);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -656,3 +666,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
&acsParameters->gyrHandlingParameters, gyrDataProcessed); &acsParameters->gyrHandlingParameters, gyrDataProcessed);
} }
void SensorProcessing::lowPassFilter(double *newValue, double *oldValue, const double weight) {
double leftSide[3] = {0, 0, 0}, rightSide[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(newValue, (1 - weight), leftSide, 3);
VectorOperations<double>::mulScalar(oldValue, weight, rightSide, 3);
VectorOperations<double>::add(leftSide, rightSide, newValue, 3);
}

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

@ -13,8 +13,6 @@
class SensorProcessing { class SensorProcessing {
public: public:
void reset();
SensorProcessing(); SensorProcessing();
virtual ~SensorProcessing(); virtual ~SensorProcessing();
@ -59,13 +57,13 @@ class SensorProcessing {
const AcsParameters::GyrHandlingParameters *gyrParameters, const AcsParameters::GyrHandlingParameters *gyrParameters,
acsctrl::GyrDataProcessed *gyrDataProcessed); acsctrl::GyrDataProcessed *gyrDataProcessed);
void processStr();
void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude, void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
const double gpsUnixSeconds, const bool validGps, const double gpsUnixSeconds, const bool validGps,
const AcsParameters::GpsParameters *gpsParameters, const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed); acsctrl::GpsDataProcessed *gpsDataProcessed);
void lowPassFilter(double *newValue, double *oldValue, const double weight);
double savedMgmVecTot[3] = {0.0, 0.0, 0.0}; double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
timeval timeOfSavedMagFieldEst; timeval timeOfSavedMagFieldEst;
double savedSusVecTot[3] = {0.0, 0.0, 0.0}; double savedSusVecTot[3] = {0.0, 0.0, 0.0};