Skip to content
Snippets Groups Projects
Commit affbeb72 authored by Davide Mor's avatar Davide Mor
Browse files

[MEA] Added mass clamping

parent a248c2ee
Branches
Tags
1 merge request!298[MEA] Updated with new version
......@@ -63,7 +63,8 @@ MEA::MEA(const Config &config)
baroR{config.baroR}, P{config.P}, x{0, 0, config.initialMass},
accelThresh{config.accelThresh}, speedThresh{config.speedThresh},
Kt{config.Kt}, alpha{config.alpha}, c{config.c}, coeffs{config.coeffs},
crossSection{config.crossSection}, ae{config.ae}, p0{config.p0}
crossSection{config.crossSection}, ae{config.ae}, p0{config.p0},
minMass{config.minMass}, maxMass{config.maxMass}
{
}
......@@ -81,6 +82,9 @@ void MEA::update(const Step &step)
// Run accelerometer correction
correctAccel(step);
// Compute current mass
computeMass();
// Run apogee prediction
computeApogee(step);
......@@ -164,13 +168,18 @@ void MEA::correctAccel(const Step &step)
x = x + K * (step.acceleration.x() - y);
}
void MEA::computeMass()
{
// Clamp the used mass, so that we don't use bogus values in case the filter
// fails BAD
mass = std::max(std::min(x(2), maxMass), minMass);
}
void MEA::computeApogee(const Step &step)
{
if (!step.hasSpeedAndAlt)
return;
float mass = x(2);
// TODO: Matlab uses CD_correction_shutDown, should we?
// Holy fuck, massive formula, split this for the love of god
apogee = step.mslAltitude +
......@@ -186,8 +195,9 @@ void MEA::updateState()
state.x0 = x(0);
state.x1 = x(1);
state.x2 = x(2);
state.estimatedMass = x(2);
state.estimatedMass = mass;
state.estimatedPressure = baroH * x;
state.estimatedApogee = apogee;
state.estimatedForce = force;
......
......@@ -61,6 +61,9 @@ public:
float ae; //< Efflux area
float p0; //< Pressure at nozzle exit
float minMass; //< Minimum mass used for predicted apogee
float maxMass; //< Maximum mass used for predicted apogee
};
struct Step
......@@ -104,6 +107,7 @@ private:
void computeForce(const Step &step);
void correctBaro(const Step &step);
void correctAccel(const Step &step);
void computeMass();
void computeApogee(const Step &step);
void updateState();
......@@ -124,6 +128,7 @@ private:
float q = 0.0f; //< Latest computed dynamic pressure
float force = 0.0f; //< Latest computed force
float mass = 0.0f; //< Latest computed mass
float apogee = 0.0f; //< Latest computed apogee
float accelThresh;
......@@ -139,6 +144,9 @@ private:
float ae;
float p0;
float minMass;
float maxMass;
MEAState state;
};
......
......@@ -39,19 +39,20 @@ struct MEAState
float x0;
float x1;
float x2;
static std::string header()
{
return "timestamp,estimatedPressure,estimatedMass,estimatedApogee,"
"estimatedForce,x0,"
"x1\n";
"x1,x2\n";
}
void print(std::ostream &os) const
{
os << timestamp << "," << estimatedPressure << "," << estimatedMass
<< "," << estimatedApogee << "," << estimatedForce << "," << x0
<< "," << x1 << "\n";
<< "," << x1 << "," << x2 << "\n";
}
};
......
......@@ -78,6 +78,9 @@ MEA::Config getMEAConfig()
config.P = Matrix<float, 3, 3>::Zero();
config.initialMass = 35.01f;
config.minMass = 20.0f;
config.maxMass = 40.0f;
// clang-format on
return config;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment