Commit 943b51c8 by RBO lab

fix incorrect addition of mass_bias on deflation, use absolute pressures…

fix incorrect addition of mass_bias on deflation, use absolute pressures everywhere, comment out debugging prints
parent df5be5b0
......@@ -14,12 +14,12 @@ namespace AirserverController{
CntrlMass::CntrlMass(MsgConfigurationControllerMass *conf)
{
std::cout << "configuring mass controller" << std::endl;
//std::cout << "configuring mass controller" << std::endl;
id = conf->header_config.id;
config = *conf; //copy everything
std::cout << "[MASS CTRL] empty mass trackers (constructor)" << std::endl;
this->error= 0.0;
desired_mass = 0.0;
......@@ -28,18 +28,17 @@ namespace AirserverController{
SetData( config.out_massobserver_term_injector, 0.0 );
SetData( config.out_massobserver, 0.0 );
std::cout <<"[MASS CTRL] Coeff: " << std::endl
<<" inflation path: " << config.c_inflate
<< ", " << config.c_inflate_friction << ", "
<< config.c_inflate_injector << ", " << config.c_inflate_Psupply << ", "
<< config.c_inflate_Pout << std::endl;
std::cout <<" deflation path: " << config.c_deflate
<< ", " << config.c_deflate_friction << ", "
<< config.c_deflate_injector << ", " << config.c_deflate_Psupply << ", "
<< config.c_deflate_Pout << std::endl;
std::cout <<" inflation threshold: " << config.c_inflation_threshold << std::endl;
std::cout <<" deflation threshold: " << config.c_deflation_threshold << std::endl;
// std::cout <<"[MASS CTRL] Coeff: " << std::endl
// <<" inflation path: " << config.c_inflate
// << ", " << config.c_inflate_friction << ", "
// << config.c_inflate_injector << ", " << config.c_inflate_Psupply << ", "
// << config.c_inflate_Pout << std::endl;
// std::cout <<" deflation path: " << config.c_deflate
// << ", " << config.c_deflate_friction << ", "
// << config.c_deflate_injector << ", " << config.c_deflate_Psupply << ", "
// << config.c_deflate_Pout << std::endl;
// std::cout <<" inflation threshold: " << config.c_inflation_threshold << std::endl;
// std::cout <<" deflation threshold: " << config.c_deflation_threshold << std::endl;
}
......@@ -51,14 +50,16 @@ namespace AirserverController{
float CntrlMass::MFLeeCompanyModel(float Pi, float Po, float scale) {
// absolute pressure ratio
float ratio= (Pi+P_ENV)/(Po+P_ENV);
float dp= (Pi - Po);
float ratio= Pi/Po;
float mf= 0.0;
float dp= (Pi - Po);
if (dp < 0.0) {dp = 0.0;} //to avoid strange behavior
if(ratio >= 1.9) { // sonic
mf= scale * (Pi + P_ENV);
mf= scale * Pi;
}else{ //subsonic
mf= 2 * scale * sqrt(dp*(Po+P_ENV));
mf= 2 * scale * sqrt(dp*Po);
}
return mf;
......@@ -79,7 +80,7 @@ namespace AirserverController{
}
float CntrlMass::MFPSIModel(float Pi, float Po, float scale) {
float ratio= (Po+P_ENV)/(Pi+P_ENV);
float ratio= Po/Pi;
// The ratio of specific heats at constant pressure
// and constant volume, c_p / c_vv
......@@ -103,12 +104,10 @@ namespace AirserverController{
} else {
if(ratio >= critical_pressure_ratio) {
// 0.528 < ratio < 0.999
mf= scale * (Pi+P_ENV)*sqrt(2.0/ (gamma-1.0) )
* sqrt(pow(ratio,(2.0/gamma)) - pow(ratio,(gamma+1.0/gamma)) );
mf= scale * Pi * sqrt(2.0/ (gamma-1.0) ) * sqrt(pow(ratio,(2.0/gamma)) - pow(ratio,(gamma+1.0/gamma)) );
} else {
// ratio < 0.528
mf= scale * (Pi+P_ENV)
* sqrt( pow(critical_pressure_ratio,(gamma+1.0)/gamma) );
mf= scale * Pi * sqrt( pow(critical_pressure_ratio,(gamma+1.0)/gamma) );
}
}// ratio > 0.999
......@@ -118,6 +117,13 @@ namespace AirserverController{
int CntrlMass::run() {
if (gotDeactivated) { //on deactivation, make sure no valves stay open:
out_inflation_valvestate = 0.0;
out_deflation_valvestate = 0.0;
SetData(config.out_inflate, out_inflation_valvestate);
SetData(config.out_deflate, out_deflation_valvestate);
}
if(!activated){
//reset all internal states:
this->error= 0.0;
......@@ -132,9 +138,13 @@ namespace AirserverController{
return EXIT_SUCCESS;
}
//compute the absolute pressures:
float p_supply = P_ENV + GetData(config.pressure_supply);
float p_out = P_ENV + GetData(config.pressure_out);
// I N I T V A R I A B L E S
// current massflow for multiple models
float mf_curr_linear = 0.0;
float mf_curr_friction = 0.0;
float mf_curr_leeco = 0.0;
// error can change in each step
......@@ -143,36 +153,28 @@ namespace AirserverController{
// M A S S F L O W
// calculate current massflow
// if pressure_out has a slight offset
// that results in a neg. value when it is
// supposed to be zero, the nonlinear model fails
float P_out= GetData(config.pressure_out);
if( P_out < 0 ){
P_out= 0;
}
// I N F L A T I O N
if( GetData(config.out_inflate) == 1.0 ) {
// compute current mass flow
mf_curr_linear= MFLinearModel( GetData(config.pressure_supply), P_out, config.c_inflate_friction);
mf_curr_leeco = MFLeeCompanyModel( GetData(config.pressure_supply), P_out, config.c_inflate_injector);
mf_curr_friction= MFLinearModel( p_supply, p_out, config.c_inflate_friction);
mf_curr_leeco = MFLeeCompanyModel( p_supply, p_out, config.c_inflate_injector);
}
// D E F L A T I O N
if( GetData(config.out_deflate) == 1.0 ) {
// pressure out = 0
mf_curr_linear= MFLinearModel( P_out, 0.0, config.c_deflate_friction);
mf_curr_leeco = MFLeeCompanyModel( P_out, 0.0, config.c_deflate_injector);
// pressure out = ambient pressure
mf_curr_friction= MFLinearModel( p_out, P_ENV, config.c_deflate_friction);
mf_curr_leeco = MFLeeCompanyModel( p_out, P_ENV, config.c_deflate_injector);
}
//compute the mass change in the last timestep given the flow estimate:
float delta_mass_linear= mf_curr_linear* PERIOD_S;
float delta_mass_friction= mf_curr_friction* PERIOD_S;
float delta_mass_leeco = mf_curr_leeco * PERIOD_S;
// update mass estimate
observer_term_friction += delta_mass_linear;
observer_term_friction += delta_mass_friction;
observer_term_injector += delta_mass_leeco;
observer_mass_estimate = observer_mass_estimate + delta_mass_linear + delta_mass_leeco; // combine models
observer_mass_estimate = observer_mass_estimate + delta_mass_friction + delta_mass_leeco; // combine models
// export internal states to signals for monitoring purposes:
SetData( config.out_massobserver_term_friction, observer_term_friction );
......@@ -214,8 +216,8 @@ namespace AirserverController{
if(out_inflation_valvestate < 0.5) {
// Add constant inflation error at inflation start
float mass_bias = config.c_inflate;
mass_bias += GetData(config.pressure_supply) * config.c_inflate_Psupply;
mass_bias += GetData(config.pressure_out) * config.c_inflate_Pout;
mass_bias += p_supply * config.c_inflate_Psupply;
mass_bias += p_out * config.c_inflate_Pout;
observer_mass_estimate += mass_bias;
}
out_inflation_valvestate = 1.0;
......@@ -226,10 +228,10 @@ namespace AirserverController{
out_deflation_valvestate = GetData(config.out_deflate); //make sure to use real out state
}
// add constants deflation model when deflation starts
if(GetData(out_deflation_valvestate) < 0.5) {
if(out_deflation_valvestate < 0.5) {
float mass_bias = config.c_deflate;
mass_bias += GetData(config.pressure_supply) * config.c_deflate_Psupply;
mass_bias += GetData(config.pressure_out) * config.c_deflate_Pout;
mass_bias += p_supply * config.c_deflate_Psupply;
mass_bias += p_out * config.c_deflate_Pout;
observer_mass_estimate += mass_bias;
}
out_inflation_valvestate = 0.0;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment